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; } }