Copter: add auto-yaw-rate and replace guided_angle_state.use_yaw_rate

We use the auto_yaw_mode in auto and guided modes to allow various yaw behaviours
This commit adds a new AUTO_YAW_RATE control to the mode and uses it within guided mode.
This new RATE control is not currently used within auto mode because there is no way (yet) for a mission command to specify a desired rate.

ArduCopter/Copter.h

@@ -491,6 +491,9 @@ private:
     // heading when in yaw_look_ahead_bearing
     float yaw_look_ahead_bearing;
 
+    // turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE
+    float auto_yaw_rate_cds;
+
     // Delay Mission Scripting Command
     int32_t condition_value;  // used in condition commands (eg delay, change alt, etc.)
     uint32_t condition_start;
@@ -819,7 +822,9 @@ private:
     void set_auto_yaw_mode(uint8_t yaw_mode);
     void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle);
     void set_auto_yaw_roi(const Location &roi_location);
+    void set_auto_yaw_rate(float turn_rate_cds);
     float get_auto_heading(void);
+    float get_auto_yaw_rate_cds();
     bool autotune_init(bool ignore_checks);
     void autotune_stop();
     bool autotune_start(bool ignore_checks);

ArduCopter/control_auto.cpp

@@ -644,6 +644,11 @@ void Copter::set_auto_yaw_mode(uint8_t yaw_mode)
     case AUTO_YAW_RESETTOARMEDYAW:
         // initial_armed_bearing will be set during arming so no init required
         break;
+
+    case AUTO_YAW_RATE:
+        // initialise target yaw rate to zero
+        auto_yaw_rate_cds = 0.0f;
+        break;
     }
 }
 
@@ -717,6 +722,13 @@ void Copter::set_auto_yaw_roi(const Location &roi_location)
     }
 }
 
+// set auto yaw rate in centi-degrees per second
+void Copter::set_auto_yaw_rate(float turn_rate_cds)
+{
+    set_auto_yaw_mode(AUTO_YAW_RATE);
+    auto_yaw_rate_cds = turn_rate_cds;
+}
+
 // get_auto_heading - returns target heading depending upon auto_yaw_mode
 // 100hz update rate
 float Copter::get_auto_heading(void)
@@ -747,6 +759,17 @@ float Copter::get_auto_heading(void)
     }
 }
 
+// returns yaw rate held in auto_yaw_rate and normally set by SET_POSITION_TARGET mavlink messages (positive it clockwise, negative is counter clockwise)
+float Copter::get_auto_yaw_rate_cds(void)
+{
+    if (auto_yaw_mode == AUTO_YAW_RATE) {
+        return auto_yaw_rate_cds;
+    }
+
+    // return zero turn rate (this should never happen)
+    return 0.0f;
+}
+
 // auto_payload_place_start - initialises controller to implement a placing
 void Copter::auto_payload_place_start()
 {

ArduCopter/control_guided.cpp

@@ -23,9 +23,8 @@ struct {
     float yaw_cd;
     float yaw_rate_cds;
     float climb_rate_cms;
-    bool use_yaw;
     bool use_yaw_rate;
-} static guided_angle_state = {0, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, false, false};
+} static guided_angle_state;
 
 struct Guided_Limit {
     uint32_t timeout_ms;  // timeout (in seconds) from the time that guided is invoked
@@ -439,14 +438,12 @@ void Copter::guided_pos_control_run()
     if (auto_yaw_mode == AUTO_YAW_HOLD) {
         // roll & pitch from waypoint controller, yaw rate from pilot
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
-    } else {
-        if (guided_angle_state.use_yaw_rate) {
-            // roll & pitch from waypoint controller, yaw rate from GCS
-            attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), guided_angle_state.yaw_rate_cds, get_smoothing_gain());
+    } else if (auto_yaw_mode == AUTO_YAW_RATE) {
+        // roll & pitch from waypoint controller, yaw rate from mavlink command or mission item
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain());
     } else {
         // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
-            attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), guided_angle_state.use_yaw ? guided_angle_state.yaw_cd: get_auto_heading(), true, get_smoothing_gain());
-        }
+        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
     }
 }
 
@@ -498,14 +495,12 @@ void Copter::guided_vel_control_run()
     if (auto_yaw_mode == AUTO_YAW_HOLD) {
         // roll & pitch from waypoint controller, yaw rate from pilot
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain());
-    } else {
-        if (guided_angle_state.use_yaw_rate) {
-            // roll & pitch from waypoint controller, yaw rate from GCS
-            attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), guided_angle_state.yaw_rate_cds, get_smoothing_gain());
+    } else if (auto_yaw_mode == AUTO_YAW_RATE) {
+        // roll & pitch from velocity controller, yaw rate from mavlink command or mission item
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain());
     } else {
         // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
-            attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), guided_angle_state.use_yaw ? guided_angle_state.yaw_cd: get_auto_heading(), true, get_smoothing_gain());
-        }
+        attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
     }
 }
 
@@ -577,14 +572,12 @@ void Copter::guided_posvel_control_run()
     if (auto_yaw_mode == AUTO_YAW_HOLD) {
         // roll & pitch from waypoint controller, yaw rate from pilot
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain());
-    } else {
-        if (guided_angle_state.use_yaw_rate) {
-            // roll & pitch from waypoint controller, yaw rate from GCS
-            attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), guided_angle_state.yaw_rate_cds, get_smoothing_gain());
+    } else if (auto_yaw_mode == AUTO_YAW_RATE) {
+        // roll & pitch from position-velocity controller, yaw rate from mavlink command or mission item
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain());
     } else {
         // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
-            attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), guided_angle_state.use_yaw ? guided_angle_state.yaw_cd: get_auto_heading(), true, get_smoothing_gain());
-        }
+        attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
     }
 }
 
@@ -690,13 +683,14 @@ void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vecto
     pos_control->set_desired_velocity(curr_vel_des);
 }
 
-// helper function to set guided yaw state
+// helper function to set yaw state and targets
 void Copter::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds)
 {
-    guided_angle_state.use_yaw = use_yaw;
-    guided_angle_state.yaw_cd = yaw_cd;
-    guided_angle_state.use_yaw_rate = use_yaw_rate;
-    guided_angle_state.yaw_rate_cds = yaw_rate_cds;
+    if (use_yaw) {
+        set_auto_yaw_look_at_heading(yaw_cd, 0.0f, 0, 0);
+    } else if (use_yaw_rate) {
+        set_auto_yaw_rate(yaw_rate_cds);
+    }
 }
 
 // Guided Limit code

ArduCopter/defines.h

@@ -18,6 +18,7 @@ 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 copter is moving
     AUTO_YAW_RESETTOARMEDYAW =  5,  // point towards heading at time motors were armed
+    AUTO_YAW_RATE =             6,  // turn at a specified rate (held in auto_yaw_rate)
 };
 
 // Ch6... Ch12 aux switch control