Copter: create an AutoYaw helper object to hold auto-yaw state

ArduCopter/Attitude.cpp

@@ -33,30 +33,27 @@ float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
  * yaw controllers
  *************************************************************/
 
-// get_roi_yaw - returns heading towards location held in roi_WP
-// should be called at 100hz
-float Copter::get_roi_yaw()
+// roi_yaw - returns heading towards location held in roi_WP
+float Copter::Mode::AutoYaw::roi_yaw()
 {
-    static uint8_t roi_yaw_counter = 0;     // used to reduce update rate to 100hz
-
     roi_yaw_counter++;
     if (roi_yaw_counter >= 4) {
         roi_yaw_counter = 0;
-        yaw_look_at_WP_bearing = get_bearing_cd(inertial_nav.get_position(), roi_WP);
+        _roi_yaw = get_bearing_cd(copter.inertial_nav.get_position(), roi_WP);
     }
 
-    return yaw_look_at_WP_bearing;
+    return _roi_yaw;
 }
 
-float Copter::get_look_ahead_yaw()
+float Copter::Mode::AutoYaw::look_ahead_yaw()
 {
-    const Vector3f& vel = inertial_nav.get_velocity();
+    const Vector3f& vel = copter.inertial_nav.get_velocity();
     float speed = norm(vel.x,vel.y);
     // Commanded Yaw to automatically look ahead.
-    if (position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) {
-        yaw_look_ahead_bearing = degrees(atan2f(vel.y,vel.x))*100.0f;
+    if (copter.position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) {
+        _look_ahead_yaw = degrees(atan2f(vel.y,vel.x))*100.0f;
     }
-    return yaw_look_ahead_bearing;
+    return _look_ahead_yaw;
 }
 
 /*************************************************************

ArduCopter/Copter.cpp

@@ -32,7 +32,6 @@ Copter::Copter(void)
     super_simple_cos_yaw(1.0),
     land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),
     rc_throttle_control_in_filter(1.0f),
-    auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP),
     inertial_nav(ahrs),
     param_loader(var_info),
     flightmode(&mode_stabilize)

ArduCopter/Copter.h

@@ -462,28 +462,6 @@ private:
     // Current location of the vehicle (altitude is relative to home)
     Location_Class current_loc;
 
-    // Navigation Yaw control
-    // auto flight mode's yaw mode
-    uint8_t auto_yaw_mode;
-
-    // Yaw will point at this location if auto_yaw_mode is set to AUTO_YAW_ROI
-    Vector3f roi_WP;
-
-    // bearing from current location to the yaw_look_at_WP
-    float yaw_look_at_WP_bearing;
-
-    // yaw used for YAW_LOOK_AT_HEADING yaw_mode
-    int32_t yaw_look_at_heading;
-
-    // Deg/s we should turn
-    int16_t yaw_look_at_heading_slew;
-
-    // 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;
-
     // IMU variables
     // Integration time (in seconds) for the gyros (DCM algorithm)
     // Updated with the fast loop
@@ -693,8 +671,6 @@ private:
 
     // Attitude.cpp
     float get_pilot_desired_yaw_rate(int16_t stick_angle);
-    float get_roi_yaw();
-    float get_look_ahead_yaw();
     void update_throttle_hover();
     void set_throttle_takeoff();
     float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid = 0.0f);
@@ -844,15 +820,6 @@ private:
     void update_flight_mode();
     void notify_flight_mode();
 
-    // mode_auto.cpp
-    uint8_t get_default_auto_yaw_mode(bool rtl);
-    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, bool 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();
-
     // mode_land.cpp
     void set_mode_land_with_pause(mode_reason_t reason);
     bool landing_with_GPS();

ArduCopter/GCS_Mavlink.cpp

@@ -924,7 +924,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
                 roi_loc.lat = packet.x;
                 roi_loc.lng = packet.y;
                 roi_loc.alt = (int32_t)(packet.z * 100.0f);
-                copter.set_auto_yaw_roi(roi_loc);
+                copter.flightmode->auto_yaw.set_roi(roi_loc);
                 result = MAV_RESULT_ACCEPTED;
                 break;
             }
@@ -1002,7 +1002,11 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
             if ((packet.param1 >= 0.0f)   &&
             	(packet.param1 <= 360.0f) &&
             	(is_zero(packet.param4) || is_equal(packet.param4,1.0f))) {
-            	copter.set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, is_positive(packet.param4));
+                copter.flightmode->auto_yaw.set_fixed_yaw(
+                    packet.param1,
+                    packet.param2,
+                    (int8_t)packet.param3,
+                    is_positive(packet.param4));
                 result = MAV_RESULT_ACCEPTED;
             } else {
                 result = MAV_RESULT_FAILED;
@@ -1066,14 +1070,17 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
             roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
             roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
             roi_loc.alt = (int32_t)(packet.param7 * 100.0f);
-            copter.set_auto_yaw_roi(roi_loc);
+            copter.flightmode->auto_yaw.set_roi(roi_loc);
             result = MAV_RESULT_ACCEPTED;
             break;
 
         case MAV_CMD_DO_MOUNT_CONTROL:
 #if MOUNT == ENABLED
             if(!copter.camera_mount.has_pan_control()) {
-                copter.set_auto_yaw_look_at_heading((float)packet.param3 / 100.0f,0.0f,0,0);
+                copter.flightmode->auto_yaw.set_fixed_yaw(
+                    (float)packet.param3 / 100.0f,
+                    0.0f,
+                    0,0);
             }
             copter.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7);
             result = MAV_RESULT_ACCEPTED;
@@ -1653,7 +1660,11 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
     //deprecated. Use MAV_CMD_DO_MOUNT_CONTROL
     case MAVLINK_MSG_ID_MOUNT_CONTROL:
         if(!copter.camera_mount.has_pan_control()) {
-            copter.set_auto_yaw_look_at_heading(mavlink_msg_mount_control_get_input_c(msg)/100.0f, 0.0f, 0, 0);
+            copter.flightmode->auto_yaw.set_fixed_yaw(
+                mavlink_msg_mount_control_get_input_c(msg)/100.0f,
+                0.0f,
+                0,
+                0);
         }
         copter.camera_mount.control_msg(msg);
         break;

ArduCopter/defines.h

@@ -15,7 +15,7 @@ enum autopilot_yaw_mode {
     AUTO_YAW_HOLD =             0,  // pilot controls the heading
     AUTO_YAW_LOOK_AT_NEXT_WP =  1,  // point towards next waypoint (no pilot input accepted)
     AUTO_YAW_ROI =              2,  // point towards a location held in roi_WP (no pilot input accepted)
-    AUTO_YAW_LOOK_AT_HEADING =  3,  // point towards a particular angle (not pilot input accepted)
+    AUTO_YAW_FIXED =            3,  // point towards a particular angle (no 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)

ArduCopter/mode.cpp

@@ -1,5 +1,7 @@
 #include "Copter.h"
 
+Copter::Mode::AutoYaw Copter::Mode::auto_yaw;
+
 /*
  * High level calls to set and update flight modes logic for individual
  * flight modes is in control_acro.cpp, control_stabilize.cpp, etc
@@ -26,11 +28,10 @@ Copter::Mode::Mode(void) :
     ap(copter.ap),
     takeoff_state(copter.takeoff_state),
     ekfGndSpdLimit(copter.ekfGndSpdLimit),
-    ekfNavVelGainScaler(copter.ekfNavVelGainScaler),
 #if FRAME_CONFIG == HELI_FRAME
     heli_flags(copter.heli_flags),
 #endif
-    auto_yaw_mode(copter.auto_yaw_mode)
+    ekfNavVelGainScaler(copter.ekfNavVelGainScaler)
 { };
 
 // return the static controller object corresponding to supplied mode
@@ -596,21 +597,6 @@ void Copter::Mode::set_throttle_takeoff()
     return copter.set_throttle_takeoff();
 }
 
-void Copter::Mode::set_auto_yaw_mode(uint8_t yaw_mode)
-{
-    return copter.set_auto_yaw_mode(yaw_mode);
-}
-
-void Copter::Mode::set_auto_yaw_rate(float turn_rate_cds)
-{
-    return copter.set_auto_yaw_rate(turn_rate_cds);
-}
-
-void Copter::Mode::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle)
-{
-    return copter.set_auto_yaw_look_at_heading(angle_deg, turn_rate_dps, direction, relative_angle);
-}
-
 void Copter::Mode::takeoff_timer_start(float alt_cm)
 {
     return copter.takeoff_timer_start(alt_cm);
@@ -626,16 +612,6 @@ void Copter::Mode::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeo
     return copter.takeoff_get_climb_rates(pilot_climb_rate, takeoff_climb_rate);
 }
 
-float Copter::Mode::get_auto_heading()
-{
-    return copter.get_auto_heading();
-}
-
-float Copter::Mode::get_auto_yaw_rate_cds()
-{
-    return copter.get_auto_yaw_rate_cds();
-}
-
 float Copter::Mode::get_avoidance_adjusted_climbrate(float target_rate)
 {
     return copter.get_avoidance_adjusted_climbrate(target_rate);
@@ -645,3 +621,8 @@ uint16_t Copter::Mode::get_pilot_speed_dn()
 {
     return copter.get_pilot_speed_dn();
 }
+
+void Copter::Mode::AutoYaw::set_mode_to_default(bool rtl)
+{
+    set_mode(default_mode(rtl));
+}

ArduCopter/mode.h

@@ -10,7 +10,67 @@ class Mode {
 
     // constructor
     Mode(void);
-    
+
+public:
+
+    // Navigation Yaw control
+    class AutoYaw {
+
+    public:
+
+        // yaw(): main product of AutoYaw; the heading:
+        float yaw();
+
+        // mode(): current method of determining desired yaw:
+        autopilot_yaw_mode mode() const { return (autopilot_yaw_mode)_mode; }
+        void set_mode_to_default(bool rtl);
+        void set_mode(autopilot_yaw_mode new_mode);
+        autopilot_yaw_mode default_mode(bool rtl) const;
+
+        // rate_cds(): desired yaw rate in centidegrees/second:
+        float rate_cds() const;
+        void set_rate(float new_rate_cds);
+
+        // set_roi(...): set a "look at" location:
+        void set_roi(const Location &roi_location);
+
+        void set_fixed_yaw(float angle_deg,
+                           float turn_rate_dps,
+                           int8_t direction,
+                           bool relative_angle);
+
+    private:
+
+        float look_ahead_yaw();
+        float roi_yaw();
+
+        // auto flight mode's yaw mode
+        uint8_t _mode = AUTO_YAW_LOOK_AT_NEXT_WP;
+
+        // Yaw will point at this location if mode is set to AUTO_YAW_ROI
+        Vector3f roi_WP;
+
+        // bearing from current location to the ROI
+        float _roi_yaw;
+
+        // yaw used for YAW_FIXED yaw_mode
+        int32_t _fixed_yaw;
+
+        // Deg/s we should turn
+        int16_t _fixed_yaw_slewrate;
+
+        // heading when in yaw_look_ahead_yaw
+        float _look_ahead_yaw;
+
+        // turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE
+        float _rate_cds;
+
+        // used to reduce update rate to 100hz:
+        uint8_t roi_yaw_counter;
+
+    };
+    static AutoYaw auto_yaw;
+
 protected:
 
     virtual bool init(bool ignore_checks) = 0;
@@ -74,10 +134,6 @@ protected:
     // scale factor applied to velocity controller gain to prevent optical flow noise causing excessive angle demand noise
     float &ekfNavVelGainScaler;
 
-    // Navigation Yaw control
-    // auto flight mode's yaw mode
-    uint8_t &auto_yaw_mode;
-
 #if FRAME_CONFIG == HELI_FRAME
     heli_flags_t &heli_flags;
 #endif
@@ -96,14 +152,9 @@ protected:
     GCS_Copter &gcs();
     void Log_Write_Event(uint8_t id);
     void set_throttle_takeoff(void);
-    void set_auto_yaw_mode(uint8_t yaw_mode);
-    void set_auto_yaw_rate(float turn_rate_cds);
-    void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle);
     void takeoff_timer_start(float alt_cm);
     void takeoff_stop(void);
     void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate);
-    float get_auto_heading(void);
-    float get_auto_yaw_rate_cds(void);
     float get_avoidance_adjusted_climbrate(float target_rate);
     uint16_t get_pilot_speed_dn(void);
 

ArduCopter/mode_auto.cpp

@@ -33,8 +33,8 @@ bool Copter::ModeAuto::init(bool ignore_checks)
 
         // stop ROI from carrying over from previous runs of the mission
         // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
-        if (copter.auto_yaw_mode == AUTO_YAW_ROI) {
-            set_auto_yaw_mode(AUTO_YAW_HOLD);
+        if (auto_yaw.mode() == AUTO_YAW_ROI) {
+            auto_yaw.set_mode(AUTO_YAW_HOLD);
         }
 
         // initialise waypoint and spline controller
@@ -118,7 +118,7 @@ bool Copter::ModeAuto::loiter_start()
     wp_nav->set_wp_destination(stopping_point);
 
     // hold yaw at current heading
-    set_auto_yaw_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AUTO_YAW_HOLD);
 
     return true;
 }
@@ -170,7 +170,7 @@ void Copter::ModeAuto::takeoff_start(const Location& dest_loc)
     }
 
     // initialise yaw
-    set_auto_yaw_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AUTO_YAW_HOLD);
 
     // clear i term when we're taking off
     set_throttle_takeoff();
@@ -189,8 +189,8 @@ void Copter::ModeAuto::wp_start(const Vector3f& destination)
 
     // initialise yaw
     // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
-    if (copter.auto_yaw_mode != AUTO_YAW_ROI) {
-        set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
+    if (auto_yaw.mode() != AUTO_YAW_ROI) {
+        auto_yaw.set_mode_to_default(false);
     }
 }
 
@@ -208,8 +208,8 @@ void Copter::ModeAuto::wp_start(const Location_Class& dest_loc)
 
     // initialise yaw
     // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
-    if (copter.auto_yaw_mode != AUTO_YAW_ROI) {
-        set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
+    if (auto_yaw.mode() != AUTO_YAW_ROI) {
+        auto_yaw.set_mode_to_default(false);
     }
 }
 
@@ -239,7 +239,7 @@ void Copter::ModeAuto::land_start(const Vector3f& destination)
     }
 
     // initialise yaw
-    set_auto_yaw_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AUTO_YAW_HOLD);
 }
 
 // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
@@ -286,10 +286,10 @@ void Copter::ModeAuto::circle_movetoedge_start(const Location_Class &circle_cent
         const Vector3f &curr_pos = inertial_nav.get_position();
         float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y);
         if (dist_to_center > copter.circle_nav->get_radius() && dist_to_center > 500) {
-            set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
+            auto_yaw.set_mode_to_default(false);
         } else {
             // vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
-            set_auto_yaw_mode(AUTO_YAW_HOLD);
+            auto_yaw.set_mode(AUTO_YAW_HOLD);
         }
     } else {
         circle_start();
@@ -323,8 +323,8 @@ void Copter::ModeAuto::spline_start(const Location_Class& destination, bool stop
 
     // initialise yaw
     // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
-    if (copter.auto_yaw_mode != AUTO_YAW_ROI) {
-        set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
+    if (auto_yaw.mode() != AUTO_YAW_ROI) {
+        auto_yaw.set_mode_to_default(false);
     }
 }
 
@@ -778,7 +778,7 @@ void Copter::ModeAuto::wp_run()
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
-            set_auto_yaw_mode(AUTO_YAW_HOLD);
+            auto_yaw.set_mode(AUTO_YAW_HOLD);
         }
     }
 
@@ -792,12 +792,12 @@ void Copter::ModeAuto::wp_run()
     pos_control->update_z_controller();
 
     // call attitude controller
-    if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
+    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);
     } else {
         // roll, pitch from waypoint controller, yaw heading from auto_heading()
-        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true);
+        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true);
     }
 }
 
@@ -821,7 +821,7 @@ void Copter::ModeAuto::spline_run()
         // get pilot's desired yaw rat
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
-            set_auto_yaw_mode(AUTO_YAW_HOLD);
+            auto_yaw.set_mode(AUTO_YAW_HOLD);
         }
     }
 
@@ -835,12 +835,12 @@ void Copter::ModeAuto::spline_run()
     pos_control->update_z_controller();
 
     // call attitude controller
-    if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
+    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);
     } else {
         // roll, pitch from waypoint controller, yaw heading from auto_heading()
-        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true);
+        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true);
     }
 }
 
@@ -937,7 +937,7 @@ void Copter::ModeAuto::payload_place_start(const Vector3f& destination)
     }
 
     // initialise yaw
-    set_auto_yaw_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AUTO_YAW_HOLD);
 }
 
 // auto_payload_place_run - places an object in auto mode
@@ -1320,7 +1320,7 @@ void Copter::ModeAuto::do_within_distance(const AP_Mission::Mission_Command& cmd
 
 void Copter::ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd)
 {
-	set_auto_yaw_look_at_heading(
+	auto_yaw.set_fixed_yaw(
 		cmd.content.yaw.angle_deg,
 		cmd.content.yaw.turn_rate_dps,
 		cmd.content.yaw.direction,
@@ -1355,7 +1355,7 @@ void Copter::ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd)
 //	TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint
 void Copter::ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd)
 {
-    copter.set_auto_yaw_roi(cmd.content.location);
+    auto_yaw.set_roi(cmd.content.location);
 }
 
 // point the camera to a specified angle
@@ -1363,7 +1363,7 @@ void Copter::ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd)
 {
 #if MOUNT == ENABLED
     if(!copter.camera_mount.has_pan_control()) {
-        copter.set_auto_yaw_look_at_heading(cmd.content.mount_control.yaw,0.0f,0,0);
+        auto_yaw.set_fixed_yaw(cmd.content.mount_control.yaw,0.0f,0,0);
     }
     copter.camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw);
 #endif
@@ -1758,16 +1758,12 @@ bool Copter::ModeAuto::verify_within_distance()
 bool Copter::ModeAuto::verify_yaw()
 {
     // set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command)
-    if (auto_yaw_mode != AUTO_YAW_LOOK_AT_HEADING) {
-        set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
+    if (auto_yaw.mode() != AUTO_YAW_FIXED) {
+        auto_yaw.set_mode(AUTO_YAW_FIXED);
     }
 
     // check if we are within 2 degrees of the target heading
-    if (labs(wrap_180_cd(ahrs.yaw_sensor-copter.yaw_look_at_heading)) <= 200) {
-        return true;
-    }else{
-        return false;
-    }
+    return (labs(wrap_180_cd(ahrs.yaw_sensor-auto_yaw.yaw())) <= 200);
 }
 
 // verify_nav_wp - check if we have reached the next way point
@@ -1877,11 +1873,11 @@ bool Copter::ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
 
 #endif
 
-// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
+// default_mode - returns auto_yaw.mode() based on WP_YAW_BEHAVIOR parameter
 // set rtl parameter to true if this is during an RTL
-uint8_t Copter::get_default_auto_yaw_mode(bool rtl)
+autopilot_yaw_mode Copter::Mode::AutoYaw::default_mode(bool rtl) const
 {
-    switch (g.wp_yaw_behavior) {
+    switch (copter.g.wp_yaw_behavior) {
 
         case WP_YAW_BEHAVIOR_NONE:
             return AUTO_YAW_HOLD;
@@ -1902,35 +1898,35 @@ uint8_t Copter::get_default_auto_yaw_mode(bool rtl)
     }
 }
 
-// set_auto_yaw_mode - sets the yaw mode for auto
-void Copter::set_auto_yaw_mode(uint8_t yaw_mode)
+// set_mode - sets the yaw mode for auto
+void Copter::Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode)
 {
     // return immediately if no change
-    if (auto_yaw_mode == yaw_mode) {
+    if (_mode == yaw_mode) {
         return;
     }
-    auto_yaw_mode = yaw_mode;
+    _mode = yaw_mode;
 
     // perform initialisation
-    switch (auto_yaw_mode) {
+    switch (_mode) {
 
     case AUTO_YAW_LOOK_AT_NEXT_WP:
         // wpnav will initialise heading when wpnav's set_destination method is called
         break;
 
     case AUTO_YAW_ROI:
-        // point towards a location held in yaw_look_at_WP
-        yaw_look_at_WP_bearing = ahrs.yaw_sensor;
+        // look ahead until we know otherwise
+        _roi_yaw = copter.ahrs.yaw_sensor;
         break;
 
-    case AUTO_YAW_LOOK_AT_HEADING:
-        // keep heading pointing in the direction held in yaw_look_at_heading
-        // caller should set the yaw_look_at_heading
+    case AUTO_YAW_FIXED:
+        // keep heading pointing in the direction held in fixed_yaw
+        // caller should set the fixed_yaw
         break;
 
     case AUTO_YAW_LOOK_AHEAD:
         // Commanded Yaw to automatically look ahead.
-        yaw_look_ahead_bearing = ahrs.yaw_sensor;
+        _look_ahead_yaw = copter.ahrs.yaw_sensor;
         break;
 
     case AUTO_YAW_RESETTOARMEDYAW:
@@ -1939,66 +1935,65 @@ void Copter::set_auto_yaw_mode(uint8_t yaw_mode)
 
     case AUTO_YAW_RATE:
         // initialise target yaw rate to zero
-        auto_yaw_rate_cds = 0.0f;
+        _rate_cds = 0.0f;
         break;
     }
 }
 
-// set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode
-void Copter::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle)
+// set_fixed_yaw - sets the yaw look at heading for auto mode
+void Copter::Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle)
 {
-    // get current yaw target
-    int32_t curr_yaw_target = attitude_control->get_att_target_euler_cd().z;
+    const int32_t curr_yaw_target = copter.attitude_control->get_att_target_euler_cd().z;
 
     // calculate final angle as relative to vehicle heading or absolute
     if (!relative_angle) {
         // absolute angle
-        yaw_look_at_heading = wrap_360_cd(angle_deg * 100);
+        _fixed_yaw = wrap_360_cd(angle_deg * 100);
     } else {
         // relative angle
         if (direction < 0) {
             angle_deg = -angle_deg;
         }
-        yaw_look_at_heading = wrap_360_cd((angle_deg * 100) + curr_yaw_target);
+        _fixed_yaw = wrap_360_cd((angle_deg * 100) + curr_yaw_target);
     }
 
     // get turn speed
     if (is_zero(turn_rate_dps)) {
         // default to regular auto slew rate
-        yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE;
+        _fixed_yaw_slewrate = AUTO_YAW_SLEW_RATE;
     } else {
-        int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - curr_yaw_target) / 100) / turn_rate_dps;
-        yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360);    // deg / sec
+        const int32_t turn_rate = (wrap_180_cd(_fixed_yaw - curr_yaw_target) / 100) / turn_rate_dps;
+        _fixed_yaw_slewrate = constrain_int32(turn_rate, 1, 360);    // deg / sec
     }
 
     // set yaw mode
-    set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
+    set_mode(AUTO_YAW_FIXED);
 
     // TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction.  1 = clockwise, -1 = counterclockwise
 }
 
-// set_auto_yaw_roi - sets the yaw to look at roi for auto mode
-void Copter::set_auto_yaw_roi(const Location &roi_location)
+// set_roi - sets the yaw to look at roi for auto mode
+void Copter::Mode::AutoYaw::set_roi(const Location &roi_location)
 {
     // if location is zero lat, lon and altitude turn off ROI
     if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) {
         // set auto yaw mode back to default assuming the active command is a waypoint command.  A more sophisticated method is required to ensure we return to the proper yaw control for the active command
-        set_auto_yaw_mode(get_default_auto_yaw_mode(false));
+        auto_yaw.set_mode_to_default(false);
 #if MOUNT == ENABLED
         // switch off the camera tracking if enabled
-        if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
-            camera_mount.set_mode_to_default();
+        if (copter.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
+            copter.camera_mount.set_mode_to_default();
         }
 #endif  // MOUNT == ENABLED
     } else {
 #if MOUNT == ENABLED
         // check if mount type requires us to rotate the quad
-        if(!camera_mount.has_pan_control()) {
-            roi_WP = pv_location_to_vector(roi_location);
-            set_auto_yaw_mode(AUTO_YAW_ROI);
+        if(!copter.camera_mount.has_pan_control()) {
+            roi_WP = copter.pv_location_to_vector(roi_location);
+            auto_yaw.set_mode(AUTO_YAW_ROI);
         }
         // send the command to the camera mount
-        camera_mount.set_roi_target(roi_location);
+        copter.camera_mount.set_roi_target(roi_location);
 
         // TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink.  Currently we only handle mode 4 (see below)
         //      0: do nothing
@@ -2009,53 +2004,54 @@ void Copter::set_auto_yaw_roi(const Location &roi_location)
 #else
         // if we have no camera mount aim the quad at the location
         roi_WP = pv_location_to_vector(roi_location);
-        set_auto_yaw_mode(AUTO_YAW_ROI);
+        auto_yaw.set_mode(AUTO_YAW_ROI);
 #endif  // MOUNT == ENABLED
     }
 }
 
 // set auto yaw rate in centi-degrees per second
-void Copter::set_auto_yaw_rate(float turn_rate_cds)
+void Copter::Mode::AutoYaw::set_rate(float turn_rate_cds)
 {
-    set_auto_yaw_mode(AUTO_YAW_RATE);
-    auto_yaw_rate_cds = turn_rate_cds;
+    set_mode(AUTO_YAW_RATE);
+    _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)
+// yaw - returns target heading depending upon auto_yaw.mode()
+float Copter::Mode::AutoYaw::yaw()
 {
-    switch(auto_yaw_mode) {
+    switch(_mode) {
 
     case AUTO_YAW_ROI:
         // point towards a location held in roi_WP
-        return get_roi_yaw();
+        return roi_yaw();
 
-    case AUTO_YAW_LOOK_AT_HEADING:
-        // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed
-        return yaw_look_at_heading;
+    case AUTO_YAW_FIXED:
+        // keep heading pointing in the direction held in fixed_yaw
+        // with no pilot input allowed
+        return _fixed_yaw;
 
     case AUTO_YAW_LOOK_AHEAD:
         // Commanded Yaw to automatically look ahead.
-        return get_look_ahead_yaw();
+        return look_ahead_yaw();
 
     case AUTO_YAW_RESETTOARMEDYAW:
         // changes yaw to be same as when quad was armed
-        return initial_armed_bearing;
+        return copter.initial_armed_bearing;
 
     case AUTO_YAW_LOOK_AT_NEXT_WP:
     default:
         // point towards next waypoint.
         // we don't use wp_bearing because we don't want the copter to turn too much during flight
-        return wp_nav->get_yaw();
+        return copter.wp_nav->get_yaw();
     }
 }
 
-// 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)
+// returns yaw rate normally set by SET_POSITION_TARGET mavlink
+// messages (positive is clockwise, negative is counter clockwise)
+float Copter::Mode::AutoYaw::rate_cds() const
 {
-    if (auto_yaw_mode == AUTO_YAW_RATE) {
-        return auto_yaw_rate_cds;
+    if (_mode == AUTO_YAW_RATE) {
+        return _rate_cds;
     }
 
     // return zero turn rate (this should never happen)

ArduCopter/mode_guided.cpp

@@ -40,7 +40,7 @@ bool Copter::ModeGuided::init(bool ignore_checks)
 {
     if (copter.position_ok() || ignore_checks) {
         // initialise yaw
-        set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
+        auto_yaw.set_mode_to_default(false);
         // start in position control mode
         pos_control_start();
         return true;
@@ -67,7 +67,7 @@ bool Copter::ModeGuided::takeoff_start(float final_alt_above_home)
     }
 
     // initialise yaw
-    set_auto_yaw_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AUTO_YAW_HOLD);
 
     // clear i term when we're taking off
     set_throttle_takeoff();
@@ -95,7 +95,7 @@ void Copter::ModeGuided::pos_control_start()
     wp_nav->set_wp_destination(stopping_point, false);
 
     // initialise yaw
-    set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
+    auto_yaw.set_mode_to_default(false);
 }
 
 // initialise guided mode's velocity controller
@@ -140,7 +140,7 @@ void Copter::ModeGuided::posvel_control_start()
     pos_control->set_accel_z(wp_nav->get_accel_z());
 
     // pilot always controls yaw
-    set_auto_yaw_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AUTO_YAW_HOLD);
 }
 
 // initialise guided mode's angle controller
@@ -169,7 +169,7 @@ void Copter::ModeGuided::angle_control_start()
     guided_angle_state.use_yaw_rate = false;
 
     // pilot always controls yaw
-    set_auto_yaw_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AUTO_YAW_HOLD);
 }
 
 // guided_set_destination - sets guided mode's target destination
@@ -417,7 +417,7 @@ void Copter::ModeGuided::pos_control_run()
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
-            set_auto_yaw_mode(AUTO_YAW_HOLD);
+            auto_yaw.set_mode(AUTO_YAW_HOLD);
         }
     }
 
@@ -431,15 +431,15 @@ void Copter::ModeGuided::pos_control_run()
     pos_control->update_z_controller();
 
     // call attitude controller
-    if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
+    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);
-    } else if (auto_yaw_mode == AUTO_YAW_RATE) {
+    } 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());
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.rate_cds());
     } 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(), get_auto_heading(), true);
+        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true);
     }
 }
 
@@ -461,7 +461,7 @@ void Copter::ModeGuided::vel_control_run()
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
-            set_auto_yaw_mode(AUTO_YAW_HOLD);
+            auto_yaw.set_mode(AUTO_YAW_HOLD);
         }
     }
 
@@ -474,8 +474,8 @@ void Copter::ModeGuided::vel_control_run()
         if (!pos_control->get_desired_velocity().is_zero()) {
             set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f, 0.0f, 0.0f));
         }
-        if (auto_yaw_mode == AUTO_YAW_RATE) {
-            set_auto_yaw_rate(0.0f);
+        if (auto_yaw.mode() == AUTO_YAW_RATE) {
+            auto_yaw.set_rate(0.0f);
         }
     } else {
         set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms);
@@ -485,15 +485,15 @@ void Copter::ModeGuided::vel_control_run()
     pos_control->update_vel_controller_xyz(ekfNavVelGainScaler);
 
     // call attitude controller
-    if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
+    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);
-    } else if (auto_yaw_mode == AUTO_YAW_RATE) {
+    } 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());
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.rate_cds());
     } 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(), get_auto_heading(), true);
+        attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.yaw(), true);
     }
 }
 
@@ -517,7 +517,7 @@ void Copter::ModeGuided::posvel_control_run()
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
-            set_auto_yaw_mode(AUTO_YAW_HOLD);
+            auto_yaw.set_mode(AUTO_YAW_HOLD);
         }
     }
 
@@ -528,8 +528,8 @@ void Copter::ModeGuided::posvel_control_run()
     uint32_t tnow = millis();
     if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
         guided_vel_target_cms.zero();
-        if (auto_yaw_mode == AUTO_YAW_RATE) {
-            set_auto_yaw_rate(0.0f);
+        if (auto_yaw.mode() == AUTO_YAW_RATE) {
+            auto_yaw.set_rate(0.0f);
         }
     }
 
@@ -553,15 +553,15 @@ void Copter::ModeGuided::posvel_control_run()
     pos_control->update_z_controller();
 
     // call attitude controller
-    if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
+    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);
-    } else if (auto_yaw_mode == AUTO_YAW_RATE) {
+    } 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());
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.rate_cds());
     } 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(), get_auto_heading(), true);
+        attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.yaw(), true);
     }
 }
 
@@ -662,9 +662,9 @@ void Copter::ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const
 void Copter::ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
 {
     if (use_yaw) {
-        set_auto_yaw_look_at_heading(yaw_cd / 100.0f, 0.0f, 0, relative_angle);
+        auto_yaw.set_fixed_yaw(yaw_cd / 100.0f, 0.0f, 0, relative_angle);
     } else if (use_yaw_rate) {
-        set_auto_yaw_rate(yaw_rate_cds);
+        auto_yaw.set_rate(yaw_rate_cds);
     }
 }
 

ArduCopter/mode_rtl.cpp

@@ -108,7 +108,7 @@ void Copter::ModeRTL::climb_start()
     wp_nav->set_fast_waypoint(true);
 
     // hold current yaw during initial climb
-    set_auto_yaw_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AUTO_YAW_HOLD);
 }
 
 // rtl_return_start - initialise return to home
@@ -123,7 +123,7 @@ void Copter::ModeRTL::return_start()
     }
 
     // initialise yaw to point home (maybe)
-    set_auto_yaw_mode(copter.get_default_auto_yaw_mode(true));
+    auto_yaw.set_mode_to_default(true);
 }
 
 // rtl_climb_return_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
@@ -143,7 +143,7 @@ void Copter::ModeRTL::climb_return_run()
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
-            set_auto_yaw_mode(AUTO_YAW_HOLD);
+            auto_yaw.set_mode(AUTO_YAW_HOLD);
         }
     }
 
@@ -157,12 +157,12 @@ void Copter::ModeRTL::climb_return_run()
     pos_control->update_z_controller();
 
     // call attitude controller
-    if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
+    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);
     }else{
         // roll, pitch from waypoint controller, yaw heading from auto_heading()
-        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true);
+        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true);
     }
 
     // check if we've completed this stage of RTL
@@ -177,10 +177,10 @@ void Copter::ModeRTL::loiterathome_start()
     _loiter_start_time = millis();
 
     // yaw back to initial take-off heading yaw unless pilot has already overridden yaw
-    if(copter.get_default_auto_yaw_mode(true) != AUTO_YAW_HOLD) {
-        set_auto_yaw_mode(AUTO_YAW_RESETTOARMEDYAW);
+    if(auto_yaw.default_mode(true) != AUTO_YAW_HOLD) {
+        auto_yaw.set_mode(AUTO_YAW_RESETTOARMEDYAW);
     } else {
-        set_auto_yaw_mode(AUTO_YAW_HOLD);
+        auto_yaw.set_mode(AUTO_YAW_HOLD);
     }
 }
 
@@ -201,7 +201,7 @@ void Copter::ModeRTL::loiterathome_run()
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
-            set_auto_yaw_mode(AUTO_YAW_HOLD);
+            auto_yaw.set_mode(AUTO_YAW_HOLD);
         }
     }
 
@@ -215,17 +215,17 @@ void Copter::ModeRTL::loiterathome_run()
     pos_control->update_z_controller();
 
     // call attitude controller
-    if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
+    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);
     }else{
         // roll, pitch from waypoint controller, yaw heading from auto_heading()
-        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true);
+        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true);
     }
 
     // check if we've completed this stage of RTL
     if ((millis() - _loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) {
-        if (copter.auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) {
+        if (auto_yaw.mode() == AUTO_YAW_RESETTOARMEDYAW) {
             // check if heading is within 2 degrees of heading when vehicle was armed
             if (labs(wrap_180_cd(ahrs.yaw_sensor-copter.initial_armed_bearing)) <= 200) {
                 _state_complete = true;
@@ -250,7 +250,7 @@ void Copter::ModeRTL::descent_start()
     pos_control->set_target_to_stopping_point_z();
 
     // initialise yaw
-    set_auto_yaw_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AUTO_YAW_HOLD);
 }
 
 // rtl_descent_run - implements the final descent to the RTL_ALT
@@ -332,7 +332,7 @@ void Copter::ModeRTL::land_start()
     }
 
     // initialise yaw
-    set_auto_yaw_mode(AUTO_YAW_HOLD);
+    auto_yaw.set_mode(AUTO_YAW_HOLD);
 }
 
 bool Copter::ModeRTL::landing_gear_should_be_deployed() const

ArduCopter/mode_smart_rtl.cpp

@@ -22,7 +22,7 @@ bool Copter::ModeSmartRTL::init(bool ignore_checks)
         wp_nav->set_wp_destination(stopping_point);
 
         // initialise yaw to obey user parameter
-        copter.set_auto_yaw_mode(copter.get_default_auto_yaw_mode(true));
+        auto_yaw.set_mode_to_default(true);
 
         // wait for cleanup of return path
         smart_rtl_state = SmartRTL_WaitForPathCleanup;
@@ -65,7 +65,7 @@ void Copter::ModeSmartRTL::wait_cleanup_run()
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
     wp_nav->update_wpnav();
     pos_control->update_z_controller();
-    attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true);
+    attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true);
 
     // check if return path is computed and if yes, begin journey home
     if (g2.smart_rtl.request_thorough_cleanup()) {
@@ -101,12 +101,12 @@ void Copter::ModeSmartRTL::path_follow_run()
     pos_control->update_z_controller();
 
     // call attitude controller
-    if (auto_yaw_mode == AUTO_YAW_HOLD) {
+    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(), 0);
     } else {
         // roll, pitch from waypoint controller, yaw heading from auto_heading()
-        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true);
+        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true);
     }
 }
 
@@ -129,7 +129,7 @@ void Copter::ModeSmartRTL::pre_land_position_run()
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
     wp_nav->update_wpnav();
     pos_control->update_z_controller();
-    attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true);
+    attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true);
 }
 
 // save current position for use by the smart_rtl flight mode