From d8b171e52b42324949086986e80ac6a9219758ef Mon Sep 17 00:00:00 2001 From: Rakesh Vivekanandan Date: Wed, 26 Jul 2023 15:23:55 -0700 Subject: [PATCH] Sub: Introduced AUTO_YAW_RATE mode and its features. --- ArduSub/defines.h | 3 ++- ArduSub/mode.h | 1 + ArduSub/mode_auto.cpp | 11 +++++++++++ ArduSub/mode_guided.cpp | 4 ++++ 4 files changed, 18 insertions(+), 1 deletion(-) diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 20224305a9749..170b466966cbd 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -26,7 +26,8 @@ enum autopilot_yaw_mode { AUTO_YAW_LOOK_AT_HEADING = 3, // point towards a particular angle (not pilot input accepted) AUTO_YAW_LOOK_AHEAD = 4, // point in the direction the vehicle is moving AUTO_YAW_RESETTOARMEDYAW = 5, // point towards heading at time motors were armed - AUTO_YAW_CORRECT_XTRACK = 6 // steer the sub in order to correct for crosstrack error during line following + AUTO_YAW_CORRECT_XTRACK = 6, // steer the sub in order to correct for crosstrack error during line following + AUTO_YAW_RATE = 7 // steer the sub with the desired yaw rate }; // Acro Trainer types diff --git a/ArduSub/mode.h b/ArduSub/mode.h index ffe01a525e4db..daee2c8a35732 100644 --- a/ArduSub/mode.h +++ b/ArduSub/mode.h @@ -297,6 +297,7 @@ class ModeGuided : public Mode float get_auto_heading(); void guided_limit_clear(); void set_auto_yaw_mode(autopilot_yaw_mode yaw_mode); + void set_yaw_rate(float turn_rate_dps); protected: diff --git a/ArduSub/mode_auto.cpp b/ArduSub/mode_auto.cpp index 4370e1dfd8d5b..423e2baab5059 100644 --- a/ArduSub/mode_auto.cpp +++ b/ArduSub/mode_auto.cpp @@ -377,6 +377,17 @@ void ModeAuto::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps // TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction. 1 = clockwise, -1 = counterclockwise } + +// sets the desired yaw rate +void ModeGuided::set_yaw_rate(float turn_rate_dps) +{ + // set sub to desired yaw rate + sub.yaw_look_at_heading_slew = MIN(turn_rate_dps, AUTO_YAW_SLEW_RATE); // deg / sec + + // set yaw mode + set_auto_yaw_mode(AUTO_YAW_RATE); +} + // set_auto_yaw_roi - sets the yaw to look at roi for auto mode void ModeAuto::set_auto_yaw_roi(const Location &roi_location) { diff --git a/ArduSub/mode_guided.cpp b/ArduSub/mode_guided.cpp index 4c65cca42c77f..af2f43cdae544 100644 --- a/ArduSub/mode_guided.cpp +++ b/ArduSub/mode_guided.cpp @@ -577,6 +577,10 @@ void ModeGuided::set_auto_yaw_mode(autopilot_yaw_mode yaw_mode) case AUTO_YAW_RESETTOARMEDYAW: // initial_armed_bearing will be set during arming so no init required break; + + case AUTO_YAW_RATE: + // set target yaw rate to yaw_look_at_heading_slew + break; } }