Copter: FlightMode - convert GUIDED flight mode

ArduCopter/Copter.cpp

@@ -33,7 +33,6 @@ Copter::Copter(void)
     home_bearing(0),
     home_distance(0),
     wp_distance(0),
-    guided_mode(Guided_TakeOff),
     rtl_state(RTL_InitialClimb),
     rtl_state_complete(false),
     smart_rtl_state(SmartRTL_PathFollow),

ArduCopter/Copter.h

@@ -392,9 +392,6 @@ private:
         float descend_max; // centimetres
     } nav_payload_place;
 
-    // Guided
-    GuidedMode guided_mode;  // controls which controller is run (pos or vel)
-
     // RTL
     RTLState rtl_state;  // records state of rtl (initial climb, returning home, etc)
     bool rtl_state_complete; // set to true if the current state is completed
@@ -841,31 +838,8 @@ private:
     float get_throttle_assist(float velz, float pilot_throttle_scaled);
     bool flip_init(bool ignore_checks);
     void flip_run();
-    bool guided_init(bool ignore_checks);
-    bool guided_takeoff_start(float final_alt_above_home);
-    void guided_pos_control_start();
-    void guided_vel_control_start();
-    void guided_posvel_control_start();
-    void guided_angle_control_start();
-    bool guided_set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
-    bool guided_set_destination(const Location_Class& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
-    void guided_set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
-    bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
-    void guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
-    void guided_run();
-    void guided_takeoff_run();
-    void guided_pos_control_run();
-    void guided_vel_control_run();
-    void guided_posvel_control_run();
-    void guided_angle_control_run();
-    void guided_set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des);
-    void guided_limit_clear();
-    void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm);
-    void guided_limit_init_time_and_pos();
-    bool guided_limit_check();
     bool guided_nogps_init(bool ignore_checks);
     void guided_nogps_run();
-    void guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);
     bool land_init(bool ignore_checks);
     void land_run();
     void land_gps_run();
@@ -1128,6 +1102,8 @@ private:
 
     Copter::FlightMode_CIRCLE flightmode_circle{*this, circle_nav};
 
+    Copter::FlightMode_GUIDED flightmode_guided{*this};
+
     Copter::FlightMode_LOITER flightmode_loiter{*this};
 
 #if FRAME_CONFIG == HELI_FRAME

ArduCopter/FlightMode.h

@@ -413,3 +413,65 @@ private:
 #endif
 
 };
+
+
+
+class FlightMode_GUIDED : public FlightMode {
+
+public:
+
+    FlightMode_GUIDED(Copter &copter) :
+        Copter::FlightMode(copter)        { }
+
+    bool init(bool ignore_checks) override;
+    void run() override; // should be called at 100hz or more
+
+    bool requires_GPS() const override { return true; }
+    bool has_manual_throttle() const override { return false; }
+    bool allows_arming(bool from_gcs) const override {
+        if (from_gcs) {
+            return true;
+        }
+        return false;
+    };
+    bool is_autopilot() const override { return true; }
+
+    void set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
+    bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
+    bool set_destination(const Location_Class& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
+    void set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
+    bool set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
+
+    void limit_clear();
+    void limit_init_time_and_pos();
+    void limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm);
+    bool limit_check();
+
+    bool takeoff_start(float final_alt_above_home);
+
+    GuidedMode mode() { return guided_mode; }
+
+    void angle_control_start();
+    void angle_control_run();
+
+protected:
+
+    const char *name() const override { return "GUIDED"; }
+    const char *name4() const override { return "GUID"; }
+
+private:
+
+    void pos_control_start();
+    void vel_control_start();
+    void posvel_control_start();
+    void takeoff_run();
+    void pos_control_run();
+    void vel_control_run();
+    void posvel_control_run();
+    void set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des);
+    void set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);
+
+    // controls which controller is run (pos or vel):
+    GuidedMode guided_mode = Guided_TakeOff;
+
+};

ArduCopter/GCS_Mavlink.cpp

@@ -1364,7 +1364,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
             use_yaw_rate = true;
         }
 
-        copter.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]),
+        copter.flightmode_guided.set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]),
             climb_rate_cms, use_yaw_rate, packet.body_yaw_rate);
 
         break;
@@ -1446,16 +1446,16 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
 
         // send request
         if (!pos_ignore && !vel_ignore && acc_ignore) {
-            if (copter.guided_set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
+            if (copter.flightmode_guided.set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
                 result = MAV_RESULT_ACCEPTED;
             } else {
                 result = MAV_RESULT_FAILED;
             }
         } else if (pos_ignore && !vel_ignore && acc_ignore) {
-            copter.guided_set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
+            copter.flightmode_guided.set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
             result = MAV_RESULT_ACCEPTED;
         } else if (!pos_ignore && vel_ignore && acc_ignore) {
-            if (copter.guided_set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
+            if (copter.flightmode_guided.set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
                 result = MAV_RESULT_ACCEPTED;
             } else {
                 result = MAV_RESULT_FAILED;
@@ -1548,16 +1548,16 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
         }
 
         if (!pos_ignore && !vel_ignore && acc_ignore) {
-            if (copter.guided_set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
+            if (copter.flightmode_guided.set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
                 result = MAV_RESULT_ACCEPTED;
             } else {
                 result = MAV_RESULT_FAILED;
             }
         } else if (pos_ignore && !vel_ignore && acc_ignore) {
-            copter.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
+            copter.flightmode_guided.set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
             result = MAV_RESULT_ACCEPTED;
         } else if (!pos_ignore && vel_ignore && acc_ignore) {
-            if (copter.guided_set_destination(pos_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
+            if (copter.flightmode_guided.set_destination(pos_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
                 result = MAV_RESULT_ACCEPTED;
             } else {
                 result = MAV_RESULT_FAILED;

ArduCopter/commands_logic.cpp

@@ -578,7 +578,7 @@ void Copter::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
 {
     if (cmd.p1 > 0) {
         // initialise guided limits
-        guided_limit_init_time_and_pos();
+        flightmode_guided.limit_init_time_and_pos();
 
         // set spline navigation target
         flightmode_auto.nav_guided_start();
@@ -649,7 +649,7 @@ void Copter::do_gripper(const AP_Mission::Mission_Command& cmd)
 // do_guided_limits - pass guided limits to guided controller
 void Copter::do_guided_limits(const AP_Mission::Mission_Command& cmd)
 {
-    guided_limit_set(cmd.p1 * 1000, // convert seconds to ms
+    flightmode_guided.limit_set(cmd.p1 * 1000, // convert seconds to ms
                      cmd.content.guided_limits.alt_min * 100.0f,    // convert meters to cm
                      cmd.content.guided_limits.alt_max * 100.0f,    // convert meters to cm
                      cmd.content.guided_limits.horiz_max * 100.0f); // convert meters to cm
@@ -1005,7 +1005,7 @@ bool Copter::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
     }
 
     // check time and position limits
-    return guided_limit_check();
+    return flightmode_guided.limit_check();
 }
 #endif  // NAV_GUIDED
 
@@ -1104,7 +1104,7 @@ bool Copter::do_guided(const AP_Mission::Mission_Command& cmd)
         {
             // set wp_nav's destination
             Location_Class dest(cmd.content.location);
-            return guided_set_destination(dest);
+            return flightmode_guided.set_destination(dest);
         }
 
         case MAV_CMD_CONDITION_YAW:

ArduCopter/control_auto.cpp

@@ -39,7 +39,7 @@ bool Copter::FlightMode_AUTO::init(bool ignore_checks)
         wp_nav->wp_and_spline_init();
 
         // clear guided limits
-        _copter.guided_limit_clear();
+        _copter.flightmode_guided.limit_clear();
 
         // start/resume the mission (based on MIS_RESTART parameter)
         mission.start_or_resume();
@@ -534,10 +534,10 @@ void Copter::FlightMode_AUTO::nav_guided_start()
     _mode = Auto_NavGuided;
 
     // call regular guided flight mode initialisation
-    _copter.guided_init(true);
+    _copter.flightmode_guided.init(true);
 
     // initialise guided start time and position as reference for limit checking
-    _copter.guided_limit_init_time_and_pos();
+    _copter.flightmode_guided.limit_init_time_and_pos();
 }
 
 // auto_nav_guided_run - allows control by external navigation controller
@@ -545,7 +545,7 @@ void Copter::FlightMode_AUTO::nav_guided_start()
 void Copter::FlightMode_AUTO::nav_guided_run()
 {
     // call regular guided flight mode run function
-    _copter.guided_run();
+    _copter.flightmode_guided.run();
 }
 #endif  // NAV_GUIDED
 

ArduCopter/control_avoid_adsb.cpp

@@ -13,7 +13,7 @@
 bool Copter::avoid_adsb_init(const bool ignore_checks)
 {
     // re-use guided mode
-    return guided_init(ignore_checks);
+    return flightmode_guided.init(ignore_checks);
 }
 
 bool Copter::avoid_adsb_set_velocity(const Vector3f& velocity_neu)
@@ -24,7 +24,7 @@ bool Copter::avoid_adsb_set_velocity(const Vector3f& velocity_neu)
     }
 
     // re-use guided mode's velocity controller
-    guided_set_velocity(velocity_neu);
+    flightmode_guided.set_velocity(velocity_neu);
     return true;
 }
 
@@ -34,5 +34,5 @@ void Copter::avoid_adsb_run()
     // re-use guided mode's velocity controller
     // Note: this is safe from interference from GCSs and companion computer's whose guided mode
     //       position and velocity requests will be ignored while the vehicle is not in guided mode
-    guided_run();
+    flightmode_guided.run();
 }

ArduCopter/control_guided.cpp

@@ -36,13 +36,13 @@ struct Guided_Limit {
 } guided_limit;
 
 // guided_init - initialise guided controller
-bool Copter::guided_init(bool ignore_checks)
+bool Copter::FlightMode_GUIDED::init(bool ignore_checks)
 {
-    if (position_ok() || ignore_checks) {
+    if (_copter.position_ok() || ignore_checks) {
         // initialise yaw
-        set_auto_yaw_mode(get_default_auto_yaw_mode(false));
+        set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false));
         // start in position control mode
-        guided_pos_control_start();
+        pos_control_start();
         return true;
     }else{
         return false;
@@ -51,17 +51,17 @@ bool Copter::guided_init(bool ignore_checks)
 
 
 // guided_takeoff_start - initialises waypoint controller to implement take-off
-bool Copter::guided_takeoff_start(float final_alt_above_home)
+bool Copter::FlightMode_GUIDED::takeoff_start(float final_alt_above_home)
 {
     guided_mode = Guided_TakeOff;
 
     // initialise wpnav destination
-    Location_Class target_loc = current_loc;
+    Location_Class target_loc = _copter.current_loc;
     target_loc.set_alt_cm(final_alt_above_home, Location_Class::ALT_FRAME_ABOVE_HOME);
 
     if (!wp_nav->set_wp_destination(target_loc)) {
         // failure to set destination can only be because of missing terrain data
-        Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
+        _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
         // failure is propagated to GCS with NAK
         return false;
     }
@@ -73,13 +73,13 @@ bool Copter::guided_takeoff_start(float final_alt_above_home)
     set_throttle_takeoff();
 
     // get initial alt for WP_NAVALT_MIN
-    auto_takeoff_set_start_alt();
+    _copter.auto_takeoff_set_start_alt();
 
     return true;
 }
 
 // initialise guided mode's position controller
-void Copter::guided_pos_control_start()
+void Copter::FlightMode_GUIDED::pos_control_start()
 {
     // set to position control mode
     guided_mode = Guided_WP;
@@ -95,11 +95,11 @@ void Copter::guided_pos_control_start()
     wp_nav->set_wp_destination(stopping_point, false);
 
     // initialise yaw
-    set_auto_yaw_mode(get_default_auto_yaw_mode(false));
+    set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false));
 }
 
 // initialise guided mode's velocity controller
-void Copter::guided_vel_control_start()
+void Copter::FlightMode_GUIDED::vel_control_start()
 {
     // set guided_mode to velocity controller
     guided_mode = Guided_Velocity;
@@ -118,7 +118,7 @@ void Copter::guided_vel_control_start()
 }
 
 // initialise guided mode's posvel controller
-void Copter::guided_posvel_control_start()
+void Copter::FlightMode_GUIDED::posvel_control_start()
 {
     // set guided_mode to velocity controller
     guided_mode = Guided_PosVel;
@@ -146,7 +146,7 @@ void Copter::guided_posvel_control_start()
 }
 
 // initialise guided mode's angle controller
-void Copter::guided_angle_control_start()
+void Copter::FlightMode_GUIDED::angle_control_start()
 {
     // set guided_mode to velocity controller
     guided_mode = Guided_Angle;
@@ -163,7 +163,7 @@ void Copter::guided_angle_control_start()
 
     // initialise targets
     guided_angle_state.update_time_ms = millis();
-    guided_angle_state.roll_cd = ahrs.roll_sensor;
+    guided_angle_state.roll_cd = _copter.ahrs.roll_sensor;
     guided_angle_state.pitch_cd = ahrs.pitch_sensor;
     guided_angle_state.yaw_cd = ahrs.yaw_sensor;
     guided_angle_state.climb_rate_cms = 0.0f;
@@ -177,49 +177,49 @@ void Copter::guided_angle_control_start()
 // guided_set_destination - sets guided mode's target destination
 // Returns true if the fence is enabled and guided waypoint is within the fence
 // else return false if the waypoint is outside the fence
-bool Copter::guided_set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
+bool Copter::FlightMode_GUIDED::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
 {
     // ensure we are in position control mode
     if (guided_mode != Guided_WP) {
-        guided_pos_control_start();
+        pos_control_start();
     }
 
 #if AC_FENCE == ENABLED
     // reject destination if outside the fence
     Location_Class dest_loc(destination);
-    if (!fence.check_destination_within_fence(dest_loc)) {
+    if (!_copter.fence.check_destination_within_fence(dest_loc)) {
-        Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
+        _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
         // failure is propagated to GCS with NAK
         return false;
     }
 #endif
 
     // set yaw state
-    guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
+    set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
 
     // no need to check return status because terrain data is not used
     wp_nav->set_wp_destination(destination, false);
 
     // log target
-    Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
+    _copter.Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
     return true;
 }
 
 // sets guided mode's target from a Location object
 // returns false if destination could not be set (probably caused by missing terrain data)
 // or if the fence is enabled and guided waypoint is outside the fence
-bool Copter::guided_set_destination(const Location_Class& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
+bool Copter::FlightMode_GUIDED::set_destination(const Location_Class& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
 {
     // ensure we are in position control mode
     if (guided_mode != Guided_WP) {
-        guided_pos_control_start();
+        pos_control_start();
     }
 
 #if AC_FENCE == ENABLED
     // reject destination outside the fence.
     // Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
-    if (!fence.check_destination_within_fence(dest_loc)) {
+    if (!_copter.fence.check_destination_within_fence(dest_loc)) {
-        Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
+        _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
         // failure is propagated to GCS with NAK
         return false;
     }
@@ -227,76 +227,76 @@ bool Copter::guided_set_destination(const Location_Class& dest_loc, bool use_yaw
 
     if (!wp_nav->set_wp_destination(dest_loc)) {
         // failure to set destination can only be because of missing terrain data
-        Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
+        _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
         // failure is propagated to GCS with NAK
         return false;
     }
 
     // set yaw state
-    guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
+    set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
 
     // log target
-    Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
+    _copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
     return true;
 }
 
 // guided_set_velocity - sets guided mode's target velocity
-void Copter::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
+void Copter::FlightMode_GUIDED::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
 {
     // check we are in velocity control mode
     if (guided_mode != Guided_Velocity) {
-        guided_vel_control_start();
+        vel_control_start();
     }
 
     // set yaw state
-    guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
+    set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
 
     // record velocity target
     guided_vel_target_cms = velocity;
     vel_update_time_ms = millis();
 
     // log target
-    Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity);
+    _copter.Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity);
 }
 
 // set guided mode posvel target
-bool Copter::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
+bool Copter::FlightMode_GUIDED::set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
 {
     // check we are in velocity control mode
     if (guided_mode != Guided_PosVel) {
-        guided_posvel_control_start();
+        posvel_control_start();
     }
 
 #if AC_FENCE == ENABLED
     // reject destination if outside the fence
     Location_Class dest_loc(destination);
-    if (!fence.check_destination_within_fence(dest_loc)) {
+    if (!_copter.fence.check_destination_within_fence(dest_loc)) {
-        Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
+        _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
         // failure is propagated to GCS with NAK
         return false;
     }
 #endif
 
     // set yaw state
-    guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
+    set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
 
     posvel_update_time_ms = millis();
     guided_pos_target_cm = destination;
     guided_vel_target_cms = velocity;
 
-    pos_control->set_pos_target(guided_pos_target_cm);
+    _copter.pos_control->set_pos_target(guided_pos_target_cm);
 
     // log target
-    Log_Write_GuidedTarget(guided_mode, destination, velocity);
+    _copter.Log_Write_GuidedTarget(guided_mode, destination, velocity);
     return true;
 }
 
 // set guided mode angle target
-void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads)
+void Copter::FlightMode_GUIDED::set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads)
 {
     // check we are in velocity control mode
     if (guided_mode != Guided_Angle) {
-        guided_angle_control_start();
+        angle_control_start();
     }
 
     // convert quaternion to euler angles
@@ -312,52 +312,52 @@ void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms, bool us
 
     // interpret positive climb rate as triggering take-off
     if (motors->armed() && !ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) {
-        set_auto_armed(true);
+        _copter.set_auto_armed(true);
     }
 
     // log target
-    Log_Write_GuidedTarget(guided_mode,
+    _copter.Log_Write_GuidedTarget(guided_mode,
                            Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd),
                            Vector3f(0.0f, 0.0f, guided_angle_state.climb_rate_cms));
 }
 
 // guided_run - runs the guided controller
 // should be called at 100hz or more
-void Copter::guided_run()
+void Copter::FlightMode_GUIDED::run()
 {
     // call the correct auto controller
     switch (guided_mode) {
 
     case Guided_TakeOff:
         // run takeoff controller
-        guided_takeoff_run();
+        takeoff_run();
         break;
 
     case Guided_WP:
         // run position controller
-        guided_pos_control_run();
+        pos_control_run();
         break;
 
     case Guided_Velocity:
         // run velocity controller
-        guided_vel_control_run();
+        vel_control_run();
         break;
 
     case Guided_PosVel:
         // run position-velocity controller
-        guided_posvel_control_run();
+        posvel_control_run();
         break;
 
     case Guided_Angle:
         // run angle controller
-        guided_angle_control_run();
+        angle_control_run();
         break;
     }
  }
 
 // guided_takeoff_run - takeoff in guided mode
 //      called by guided_run at 100hz or more
-void Copter::guided_takeoff_run()
+void Copter::FlightMode_GUIDED::takeoff_run()
 {
     // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
     if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
@@ -379,7 +379,7 @@ void Copter::guided_takeoff_run()
 
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!failsafe.radio) {
+    if (!_copter.failsafe.radio) {
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
     }
@@ -400,18 +400,18 @@ void Copter::guided_takeoff_run()
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
     // run waypoint controller
-    failsafe_terrain_set_status(wp_nav->update_wpnav());
+    _copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
 
     // call z-axis position controller (wpnav should have already updated it's alt target)
-    pos_control->update_z_controller();
+    _copter.pos_control->update_z_controller();
 
     // call attitude controller
-    auto_takeoff_attitude_run(target_yaw_rate);
+    _copter.auto_takeoff_attitude_run(target_yaw_rate);
 }
 
 // guided_pos_control_run - runs the guided position controller
 // called from guided_run
-void Copter::guided_pos_control_run()
+void Copter::FlightMode_GUIDED::pos_control_run()
 {
     // if not auto armed or motors not enabled set throttle to zero and exit immediately
     if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
@@ -429,7 +429,7 @@ void Copter::guided_pos_control_run()
 
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!failsafe.radio) {
+    if (!_copter.failsafe.radio) {
         // 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)) {
@@ -441,13 +441,13 @@ void Copter::guided_pos_control_run()
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
     // run waypoint controller
-    failsafe_terrain_set_status(wp_nav->update_wpnav());
+    _copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
 
     // call z-axis position controller (wpnav should have already updated it's alt target)
     pos_control->update_z_controller();
 
     // call attitude controller
-    if (auto_yaw_mode == AUTO_YAW_HOLD) {
+    if (_copter.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 (auto_yaw_mode == AUTO_YAW_RATE) {
@@ -461,7 +461,7 @@ void Copter::guided_pos_control_run()
 
 // guided_vel_control_run - runs the guided velocity controller
 // called from guided_run
-void Copter::guided_vel_control_run()
+void Copter::FlightMode_GUIDED::vel_control_run()
 {
     // if not auto armed or motors not enabled set throttle to zero and exit immediately
     if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
@@ -481,7 +481,7 @@ void Copter::guided_vel_control_run()
 
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!failsafe.radio) {
+    if (!_copter.failsafe.radio) {
         // 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)) {
@@ -496,20 +496,20 @@ void Copter::guided_vel_control_run()
     uint32_t tnow = millis();
     if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
         if (!pos_control->get_desired_velocity().is_zero()) {
-            guided_set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f, 0.0f, 0.0f));
+            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);
         }
     } else {
-        guided_set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms);
+        set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms);
     }
 
     // call velocity controller which includes z axis controller
     pos_control->update_vel_controller_xyz(ekfNavVelGainScaler);
 
     // call attitude controller
-    if (auto_yaw_mode == AUTO_YAW_HOLD) {
+    if (_copter.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 (auto_yaw_mode == AUTO_YAW_RATE) {
@@ -523,7 +523,7 @@ void Copter::guided_vel_control_run()
 
 // guided_posvel_control_run - runs the guided spline controller
 // called from guided_run
-void Copter::guided_posvel_control_run()
+void Copter::FlightMode_GUIDED::posvel_control_run()
 {
     // if not auto armed or motors not enabled set throttle to zero and exit immediately
     if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
@@ -545,7 +545,7 @@ void Copter::guided_posvel_control_run()
     // process pilot's yaw input
     float target_yaw_rate = 0;
 
-    if (!failsafe.radio) {
+    if (!_copter.failsafe.radio) {
         // 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)) {
@@ -589,7 +589,7 @@ void Copter::guided_posvel_control_run()
     pos_control->update_z_controller();
 
     // call attitude controller
-    if (auto_yaw_mode == AUTO_YAW_HOLD) {
+    if (_copter.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 (auto_yaw_mode == AUTO_YAW_RATE) {
@@ -603,7 +603,7 @@ void Copter::guided_posvel_control_run()
 
 // guided_angle_control_run - runs the guided angle controller
 // called from guided_run
-void Copter::guided_angle_control_run()
+void Copter::FlightMode_GUIDED::angle_control_run()
 {
     // if not auto armed or motors not enabled set throttle to zero and exit immediately
     if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) {
@@ -625,7 +625,7 @@ void Copter::guided_angle_control_run()
     float roll_in = guided_angle_state.roll_cd;
     float pitch_in = guided_angle_state.pitch_cd;
     float total_in = norm(roll_in, pitch_in);
-    float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), aparm.angle_max);
+    float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), _copter.aparm.angle_max);
     if (total_in > angle_max) {
         float ratio = angle_max / total_in;
         roll_in *= ratio;
@@ -667,7 +667,7 @@ void Copter::guided_angle_control_run()
 }
 
 // helper function to update position controller's desired velocity while respecting acceleration limits
-void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des)
+void Copter::FlightMode_GUIDED::set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des)
 {
     // get current desired velocity
     Vector3f curr_vel_des = pos_control->get_desired_velocity();
@@ -691,7 +691,7 @@ void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vecto
 
 #if AC_AVOID_ENABLED
     // limit the velocity to prevent fence violations
-    avoid.adjust_velocity(pos_control->get_pos_xy_kP(), pos_control->get_accel_xy(), curr_vel_des);
+    _copter.avoid.adjust_velocity(pos_control->get_pos_xy_kP(), pos_control->get_accel_xy(), curr_vel_des);
     // get avoidance adjusted climb rate
     curr_vel_des.z = get_avoidance_adjusted_climbrate(curr_vel_des.z);    
 #endif
@@ -701,7 +701,7 @@ void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vecto
 }
 
 // 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, bool relative_angle)
+void Copter::FlightMode_GUIDED::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);
@@ -713,7 +713,7 @@ void Copter::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate,
 // Guided Limit code
 
 // guided_limit_clear - clear/turn off guided limits
-void Copter::guided_limit_clear()
+void Copter::FlightMode_GUIDED::limit_clear()
 {
     guided_limit.timeout_ms = 0;
     guided_limit.alt_min_cm = 0.0f;
@@ -722,7 +722,7 @@ void Copter::guided_limit_clear()
 }
 
 // guided_limit_set - set guided timeout and movement limits
-void Copter::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
+void Copter::FlightMode_GUIDED::limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
 {
     guided_limit.timeout_ms = timeout_ms;
     guided_limit.alt_min_cm = alt_min_cm;
@@ -732,7 +732,7 @@ void Copter::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_m
 
 // guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking
 //  only called from AUTO mode's auto_nav_guided_start function
-void Copter::guided_limit_init_time_and_pos()
+void Copter::FlightMode_GUIDED::limit_init_time_and_pos()
 {
     // initialise start time
     guided_limit.start_time = AP_HAL::millis();
@@ -743,7 +743,7 @@ void Copter::guided_limit_init_time_and_pos()
 
 // guided_limit_check - returns true if guided mode has breached a limit
 //  used when guided is invoked from the NAV_GUIDED_ENABLE mission command
-bool Copter::guided_limit_check()
+bool Copter::FlightMode_GUIDED::limit_check()
 {
     // check if we have passed the timeout
     if ((guided_limit.timeout_ms > 0) && (millis() - guided_limit.start_time >= guided_limit.timeout_ms)) {

ArduCopter/control_guided_nogps.cpp

@@ -8,7 +8,7 @@
 bool Copter::guided_nogps_init(bool ignore_checks)
 {
     // start in angle control mode
-    guided_angle_control_start();
+    flightmode_guided.angle_control_start();
     return true;
 }
 
@@ -17,6 +17,6 @@ bool Copter::guided_nogps_init(bool ignore_checks)
 void Copter::guided_nogps_run()
 {
     // run angle controller
-    guided_angle_control_run();
+    flightmode_guided.angle_control_run();
 }
 

ArduCopter/flight_mode.cpp

@@ -82,7 +82,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
             break;
 
         case GUIDED:
-            success = guided_init(ignore_checks);
+            success = flightmode_guided.init(ignore_checks);
+            if (success) {
+                flightmode = &flightmode_guided;
+            }
             break;
 
         case LAND:
@@ -203,10 +206,6 @@ void Copter::update_flight_mode()
 
     switch (control_mode) {
 
-        case GUIDED:
-            guided_run();
-            break;
-
         case LAND:
             land_run();
             break;
@@ -325,7 +324,6 @@ bool Copter::mode_requires_GPS()
         return flightmode->requires_GPS();
     }
     switch (control_mode) {
-        case GUIDED:
         case RTL:
         case SMART_RTL:
         case DRIFT:
@@ -376,7 +374,6 @@ void Copter::notify_flight_mode()
         return;
     }
     switch (control_mode) {
-        case GUIDED:
         case RTL:
         case AVOID_ADSB:
         case GUIDED_NOGPS:
@@ -393,9 +390,6 @@ void Copter::notify_flight_mode()
 
     // set flight mode string
     switch (control_mode) {
-        case GUIDED:
-            notify.set_flight_mode_str("GUID");
-            break;
         case RTL:
             notify.set_flight_mode_str("RTL ");
             break;

ArduCopter/navigation.cpp

@@ -37,11 +37,11 @@ void Copter::calc_wp_distance()
         break;
 
     case GUIDED:
-        if (guided_mode == Guided_WP) {
+        if (flightmode_guided.mode() == Guided_WP) {
             wp_distance = wp_nav->get_wp_distance_to_destination();
             break;
         }
-        if (guided_mode == Guided_PosVel) {
+        if (flightmode_guided.mode() == Guided_PosVel) {
             wp_distance = pos_control->get_distance_to_target();
             break;
         }
@@ -69,11 +69,11 @@ void Copter::calc_wp_bearing()
         break;
 
     case GUIDED:
-        if (guided_mode == Guided_WP) {
+        if (flightmode_guided.mode() == Guided_WP) {
             wp_bearing = wp_nav->get_wp_bearing_to_destination();
             break;
         }
-        if (guided_mode == Guided_PosVel) {
+        if (flightmode_guided.mode() == Guided_PosVel) {
             wp_bearing = pos_control->get_bearing_to_target();
             break;
         }

ArduCopter/takeoff.cpp

@@ -36,7 +36,7 @@ bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
 
         switch(control_mode) {
             case GUIDED:
-                if (guided_takeoff_start(takeoff_alt_cm)) {
+                if (flightmode_guided.takeoff_start(takeoff_alt_cm)) {
                     set_auto_armed(true);
                     return true;
                 }