Copter: FlightMode - convert FLIP flight mode

ArduCopter/Copter.h

@@ -419,9 +419,6 @@ private:
     int32_t nav_delay_time_max;  // used for delaying the navigation commands (eg land,takeoff etc.)
     uint32_t nav_delay_time_start;
 
-    // Flip
-    Vector3f flip_orig_attitude;         // original vehicle attitude before flip
-
     // throw mode state
     struct {
         ThrowModeStage stage;
@@ -817,8 +814,6 @@ private:
     bool brake_init(bool ignore_checks);
     void brake_run();
     void brake_timeout_to_loiter_ms(uint32_t timeout_ms);
-    bool flip_init(bool ignore_checks);
-    void flip_run();
     bool guided_nogps_init(bool ignore_checks);
     void guided_nogps_run();
     void land_run_vertical_control(bool pause_descent = false);
@@ -1064,6 +1059,8 @@ private:
 
     Copter::FlightMode_DRIFT flightmode_drift{*this};
 
+    Copter::FlightMode_FLIP flightmode_flip{*this};
+
     Copter::FlightMode_GUIDED flightmode_guided{*this};
 
     Copter::FlightMode_LAND flightmode_land{*this};

ArduCopter/FlightMode.h

@@ -633,3 +633,33 @@ protected:
 private:
 
 };
+
+
+
+class FlightMode_FLIP : public FlightMode {
+
+public:
+
+    FlightMode_FLIP(Copter &copter) :
+        Copter::FlightMode(copter)
+        { }
+
+    virtual bool init(bool ignore_checks) override;
+    virtual void run() override; // should be called at 100hz or more
+
+    virtual bool requires_GPS() const override { return false; }
+    virtual bool has_manual_throttle() const override { return false; }
+    virtual bool allows_arming(bool from_gcs) const override { return false; };
+    virtual bool is_autopilot() const override { return false; }
+
+protected:
+
+    const char *name() const override { return "FLIP"; }
+    const char *name4() const override { return "FLIP"; }
+
+private:
+
+    // Flip
+    Vector3f flip_orig_attitude;         // original vehicle attitude before flip
+
+};

ArduCopter/control_flip.cpp

@@ -30,6 +30,7 @@
 #define FLIP_PITCH_BACK      1      // used to set flip_dir
 #define FLIP_PITCH_FORWARD  -1      // used to set flip_dir
 
+// FIXME? these should be instance variables?
 FlipState flip_state;               // current state of flip
 control_mode_t   flip_orig_control_mode;   // flight mode when flip was initated
 uint32_t  flip_start_time;          // time since flip began
@@ -37,15 +38,15 @@ int8_t    flip_roll_dir;            // roll direction (-1 = roll left, 1 = roll
 int8_t    flip_pitch_dir;           // pitch direction (-1 = pitch forward, 1 = pitch back)
 
 // flip_init - initialise flip controller
-bool Copter::flip_init(bool ignore_checks)
+bool Copter::FlightMode_FLIP::init(bool ignore_checks)
 {
     // only allow flip from ACRO, Stabilize, AltHold or Drift flight modes
-    if (control_mode != ACRO && control_mode != STABILIZE && control_mode != ALT_HOLD) {
+    if (_copter.control_mode != ACRO && _copter.control_mode != STABILIZE && _copter.control_mode != ALT_HOLD) {
         return false;
     }
 
     // if in acro or stabilize ensure throttle is above zero
-    if (ap.throttle_zero && (control_mode == ACRO || control_mode == STABILIZE)) {
+    if (ap.throttle_zero && (_copter.control_mode == ACRO || _copter.control_mode == STABILIZE)) {
         return false;
     }
 
@@ -60,7 +61,7 @@ bool Copter::flip_init(bool ignore_checks)
     }
 
     // capture original flight mode so that we can return to it after completion
-    flip_orig_control_mode = control_mode;
+    flip_orig_control_mode = _copter.control_mode;
 
     // initialise state
     flip_state = Flip_Start;
@@ -83,8 +84,9 @@ bool Copter::flip_init(bool ignore_checks)
     Log_Write_Event(DATA_FLIP_START);
 
     // capture current attitude which will be used during the Flip_Recovery stage
-    flip_orig_attitude.x = constrain_float(ahrs.roll_sensor, -aparm.angle_max, aparm.angle_max);
+    float angle_max = _copter.aparm.angle_max;
-    flip_orig_attitude.y = constrain_float(ahrs.pitch_sensor, -aparm.angle_max, aparm.angle_max);
+    flip_orig_attitude.x = constrain_float(ahrs.roll_sensor, -angle_max, angle_max);
+    flip_orig_attitude.y = constrain_float(ahrs.pitch_sensor, -angle_max, angle_max);
     flip_orig_attitude.z = ahrs.yaw_sensor;
 
     return true;
@@ -92,7 +94,7 @@ bool Copter::flip_init(bool ignore_checks)
 
 // flip_run - runs the flip controller
 // should be called at 100hz or more
-void Copter::flip_run()
+void Copter::FlightMode_FLIP::run()
 {
     float throttle_out;
     float recovery_angle;
@@ -191,9 +193,9 @@ void Copter::flip_run()
         // check for successful recovery
         if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) {
             // restore original flight mode
-            if (!set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
+            if (!_copter.set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
                 // this should never happen but just in case
-                set_mode(STABILIZE, MODE_REASON_UNKNOWN);
+                _copter.set_mode(STABILIZE, MODE_REASON_UNKNOWN);
             }
             // log successful completion
             Log_Write_Event(DATA_FLIP_END);
@@ -202,12 +204,12 @@ void Copter::flip_run()
 
     case Flip_Abandon:
         // restore original flight mode
-        if (!set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
+        if (!_copter.set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
             // this should never happen but just in case
-            set_mode(STABILIZE, MODE_REASON_UNKNOWN);
+            _copter.set_mode(STABILIZE, MODE_REASON_UNKNOWN);
         }
         // log abandoning flip
-        Log_Write_Error(ERROR_SUBSYSTEM_FLIP,ERROR_CODE_FLIP_ABANDONED);
+        _copter.Log_Write_Error(ERROR_SUBSYSTEM_FLIP,ERROR_CODE_FLIP_ABANDONED);
         break;
     }
 

ArduCopter/flight_mode.cpp

@@ -117,7 +117,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
             break;
 
         case FLIP:
-            success = flip_init(ignore_checks);
+            success = flightmode_flip.init(ignore_checks);
+            if (success) {
+                flightmode = &flightmode_flip;
+            }
             break;
 
 #if AUTOTUNE_ENABLED == ENABLED
@@ -218,10 +221,6 @@ void Copter::update_flight_mode()
 
     switch (control_mode) {
 
-        case FLIP:
-            flip_run();
-            break;
-
 #if AUTOTUNE_ENABLED == ENABLED
         case AUTOTUNE:
             autotune_run();
@@ -382,9 +381,6 @@ void Copter::notify_flight_mode()
 
     // set flight mode string
     switch (control_mode) {
-        case FLIP:
-            notify.set_flight_mode_str("FLIP");
-            break;
         case AUTOTUNE:
             notify.set_flight_mode_str("ATUN");
             break;