Copter: a FlightMode base class

ArduCopter/Copter.h

@@ -522,7 +522,12 @@ private:
 
     // Attitude, Position and Waypoint navigation objects
     // To-Do: move inertial nav up or other navigation variables down here
-    AC_AttitudeControl *attitude_control;
+#if FRAME_CONFIG == HELI_FRAME
+    #define AC_AttitudeControl_t AC_AttitudeControl_Heli
+#else
+    #define AC_AttitudeControl_t AC_AttitudeControl_Multi
+#endif
+    AC_AttitudeControl_t *attitude_control;
     AC_PosControl *pos_control;
     AC_WPNav *wp_nav;
     AC_Circle *circle_nav;
@@ -1160,6 +1165,10 @@ private:
     
     uint16_t get_pilot_speed_dn();    
 
+#include "FlightMode.h"
+
+    Copter::FlightMode *flightmode;
+
 public:
     void mavlink_delay_cb();
     void failsafe_check();

ArduCopter/FlightMode.h

@@ -0,0 +1,153 @@
+#pragma once
+
+// this class is #included into the Copter:: namespace
+
+class FlightMode {
+    friend class Copter;
+    friend class AP_Arming_Copter;
+
+public:
+
+    FlightMode(Copter &copter) :
+        _copter(copter),
+        g(copter.g),
+        g2(copter.g2),
+        wp_nav(_copter.wp_nav),
+        pos_control(_copter.pos_control),
+        inertial_nav(_copter.inertial_nav),
+        ahrs(_copter.ahrs),
+        attitude_control(_copter.attitude_control),
+        motors(_copter.motors),
+        channel_roll(_copter.channel_roll),
+        channel_pitch(_copter.channel_pitch),
+        channel_throttle(_copter.channel_throttle),
+        channel_yaw(_copter.channel_yaw),
+        G_Dt(_copter.G_Dt),
+        ap(_copter.ap),
+        takeoff_state(_copter.takeoff_state),
+        ekfGndSpdLimit(_copter.ekfGndSpdLimit),
+        ekfNavVelGainScaler(_copter.ekfNavVelGainScaler),
+        auto_yaw_mode(_copter.auto_yaw_mode)
+        { };
+
+protected:
+
+    virtual bool init(bool ignore_checks) = 0;
+    virtual void run() = 0; // should be called at 100hz or more
+
+    virtual bool is_autopilot() const { return false; }
+    virtual bool requires_GPS() const = 0;
+    virtual bool has_manual_throttle() const = 0;
+    virtual bool allows_arming(bool from_gcs) const = 0;
+    void print_FlightMode(AP_HAL::BetterStream *port) const {
+        port->print(name());
+    }
+    virtual const char *name() const = 0;
+
+    // returns a string for this flightmode, exactly 4 bytes
+    virtual const char *name4() const = 0;
+
+    Copter &_copter;
+
+    // convenience references to avoid code churn in conversion:
+    Parameters &g;
+    ParametersG2 &g2;
+    AC_WPNav *&wp_nav;
+    AC_PosControl *&pos_control;
+    AP_InertialNav &inertial_nav;
+    AP_AHRS &ahrs;
+    AC_AttitudeControl_t *&attitude_control;
+    MOTOR_CLASS *&motors;
+    RC_Channel *&channel_roll;
+    RC_Channel *&channel_pitch;
+    RC_Channel *&channel_throttle;
+    RC_Channel *&channel_yaw;
+    float &G_Dt;
+    ap_t ≈
+    takeoff_state_t &takeoff_state;
+
+    // gnd speed limit required to observe optical flow sensor limits
+    float &ekfGndSpdLimit;
+
+    // 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;
+
+    // pass-through functions to reduce code churn on conversion;
+    // these are candidates for moving into the FlightMode base
+    // class.
+    virtual void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max) {
+        _copter.get_pilot_desired_lean_angles(roll_in, pitch_in, roll_out, pitch_out, angle_max);
+    }
+    virtual float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt) {
+        return _copter.get_surface_tracking_climb_rate(target_rate, current_alt_target, dt);
+    }
+    virtual float get_pilot_desired_yaw_rate(int16_t stick_angle) {
+        return _copter.get_pilot_desired_yaw_rate(stick_angle);
+    }
+    virtual float get_pilot_desired_climb_rate(float throttle_control) {
+        return _copter.get_pilot_desired_climb_rate(throttle_control);
+    }
+    virtual float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid = 0.0f) {
+        return _copter.get_pilot_desired_throttle(throttle_control, thr_mid);
+    }
+    virtual float get_non_takeoff_throttle() {
+        return _copter.get_non_takeoff_throttle();
+    }
+    virtual void update_simple_mode(void) {
+        _copter.update_simple_mode();
+    }
+    virtual float get_smoothing_gain() {
+        return _copter.get_smoothing_gain();
+    }
+    virtual bool set_mode(control_mode_t mode, mode_reason_t reason) {
+        return _copter.set_mode(mode, reason);
+    }
+    virtual void set_land_complete(bool b) {
+        return _copter.set_land_complete(b);
+    }
+    GCS_Copter &gcs() {
+        return _copter.gcs();
+    }
+    virtual void Log_Write_Event(uint8_t id) {
+        return _copter.Log_Write_Event(id);
+    }
+    virtual void set_throttle_takeoff() {
+        return _copter.set_throttle_takeoff();
+    }
+    virtual void set_auto_yaw_mode(uint8_t yaw_mode) {
+        return _copter.set_auto_yaw_mode(yaw_mode);
+    }
+    void set_auto_yaw_rate(float turn_rate_cds) {
+        return _copter.set_auto_yaw_rate(turn_rate_cds);
+    }
+    void 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);
+    }
+    virtual void takeoff_timer_start(float alt_cm) {
+        return _copter.takeoff_timer_start(alt_cm);
+    }
+    virtual void takeoff_stop() {
+        return _copter.takeoff_stop();
+    }
+    virtual void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate) {
+        return _copter.takeoff_get_climb_rates(pilot_climb_rate, takeoff_climb_rate);
+    }
+    float get_auto_heading() {
+        return _copter.get_auto_heading();
+    }
+    float get_auto_yaw_rate_cds() {
+        return _copter.get_auto_yaw_rate_cds();
+    }
+    float get_avoidance_adjusted_climbrate(float target_rate) {
+        return _copter.get_avoidance_adjusted_climbrate(target_rate);
+    }
+    uint16_t get_pilot_speed_dn() {
+        return _copter.get_pilot_speed_dn();
+    }
+
+    // end pass-through functions
+};

ArduCopter/flight_mode.cpp

@@ -24,7 +24,6 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
         return true;
     }
 
-
 #if FRAME_CONFIG == HELI_FRAME
     // do not allow helis to enter a non-manual throttle mode if the
     // rotor runup is not complete
@@ -33,6 +32,12 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
     }
 #endif
 
+    // for transition, we assume no flightmode object will be used in
+    // the new mode, and if the transition fails we reset the
+    // flightmode to the previous value
+    Copter::FlightMode* old_flightmode = flightmode;
+    flightmode = nullptr;
+
     switch (mode) {
         case ACRO:
             #if FRAME_CONFIG == HELI_FRAME
@@ -160,6 +165,7 @@ failed:
 #endif
         
     } else {
+        flightmode = old_flightmode;
         // Log error that we failed to enter desired flight mode
         Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
         gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
@@ -181,6 +187,10 @@ void Copter::update_flight_mode()
     // Update EKF speed limit - used to limit speed when we are using optical flow
     ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
 
+    if (flightmode != nullptr) {
+        flightmode->run();
+    }
+
     switch (control_mode) {
         case ACRO:
             #if FRAME_CONFIG == HELI_FRAME
@@ -332,6 +342,9 @@ void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_contr
 // returns true or false whether mode requires GPS
 bool Copter::mode_requires_GPS(control_mode_t mode)
 {
+    if (flightmode != nullptr) {
+        return flightmode->requires_GPS();
+    }
     switch (mode) {
         case AUTO:
         case GUIDED:
@@ -366,6 +379,9 @@ bool Copter::mode_has_manual_throttle(control_mode_t mode)
 //  arming_from_gcs should be set to true if the arming request comes from the ground station
 bool Copter::mode_allows_arming(control_mode_t mode, bool arming_from_gcs)
 {
+    if (flightmode != nullptr) {
+        return flightmode->allows_arming(arming_from_gcs);
+    }
     if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || mode == DRIFT || mode == SPORT || mode == THROW || (arming_from_gcs && (mode == GUIDED || mode == GUIDED_NOGPS))) {
         return true;
     }
@@ -377,6 +393,11 @@ void Copter::notify_flight_mode(control_mode_t mode)
 {
     AP_Notify::flags.flight_mode = mode;
 
+    if (flightmode != nullptr) {
+        AP_Notify::flags.autopilot_mode = flightmode->is_autopilot();
+        notify.set_flight_mode_str(flightmode->name4());
+        return;
+    }
     switch (mode) {
         case AUTO:
         case GUIDED: