Copter: FlightMode - convert STABILIZE flight mode

ArduCopter/Copter.cpp

@@ -26,6 +26,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 Copter::Copter(void)
     : DataFlash(DataFlash_Class::create(fwver.fw_string, g.log_bitmask)),
     flight_modes(&g.flight_mode1),
+    flightmode(&flightmode_stabilize),
     control_mode(STABILIZE),
     scaleLongDown(1),
     wp_bearing(0),

ArduCopter/Copter.h

@@ -961,8 +961,6 @@ private:
     void smart_rtl_save_position();
     bool sport_init(bool ignore_checks);
     void sport_run();
-    bool stabilize_init(bool ignore_checks);
-    void stabilize_run();
     void crash_check();
     void parachute_check();
     void parachute_release();
@@ -1011,8 +1009,6 @@ private:
     void update_heli_control_dynamics(void);
     void heli_update_landing_swash();
     void heli_update_rotor_speed_targets();
-    bool heli_stabilize_init(bool ignore_checks);
-    void heli_stabilize_run();
     void read_inertia();
     bool land_complete_maybe();
     void update_land_and_crash_detectors();
@@ -1171,6 +1167,13 @@ private:
 
     Copter::FlightMode_ALTHOLD flightmode_althold{*this};
 
+#if FRAME_CONFIG == HELI_FRAME
+    Copter::FlightMode_STABILIZE_Heli flightmode_stabilize{*this};
+#else
+    Copter::FlightMode_STABILIZE flightmode_stabilize{*this};
+#endif
+
+
 public:
     void mavlink_delay_cb();
     void failsafe_check();

ArduCopter/FlightMode.h

@@ -224,3 +224,49 @@ protected:
 private:
 
 };
+
+
+
+class FlightMode_STABILIZE : public FlightMode {
+
+public:
+
+    FlightMode_STABILIZE(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 true; }
+    virtual bool allows_arming(bool from_gcs) const override { return true; };
+    virtual bool is_autopilot() const override { return false; }
+
+protected:
+
+    const char *name() const override { return "STABILIZE"; }
+    const char *name4() const override { return "STAB"; }
+
+private:
+
+};
+
+#if FRAME_CONFIG == HELI_FRAME
+class FlightMode_STABILIZE_Heli : public FlightMode_STABILIZE {
+
+public:
+
+    FlightMode_STABILIZE_Heli(Copter &copter) :
+        Copter::FlightMode_STABILIZE(copter)
+        { }
+
+    bool init(bool ignore_checks) override;
+    void run() override; // should be called at 100hz or more
+
+protected:
+
+private:
+
+};
+#endif

ArduCopter/control_stabilize.cpp

@@ -5,10 +5,10 @@
  */
 
 // stabilize_init - initialise stabilize controller
-bool Copter::stabilize_init(bool ignore_checks)
+bool Copter::FlightMode_STABILIZE::init(bool ignore_checks)
 {
     // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
-    if (motors->armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) &&
+    if (motors->armed() && ap.land_complete && !_copter.mode_has_manual_throttle(_copter.control_mode) &&
             (get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) {
         return false;
     }
@@ -20,7 +20,7 @@ bool Copter::stabilize_init(bool ignore_checks)
 
 // stabilize_run - runs the main stabilize controller
 // should be called at 100hz or more
-void Copter::stabilize_run()
+void Copter::FlightMode_STABILIZE::run()
 {
     float target_roll, target_pitch;
     float target_yaw_rate;
@@ -41,6 +41,8 @@ void Copter::stabilize_run()
     // apply SIMPLE mode transform to pilot inputs
     update_simple_mode();
 
+    AP_Vehicle::MultiCopter &aparm = _copter.aparm;
+
     // convert pilot input to lean angles
     // To-Do: convert get_pilot_desired_lean_angles to return angles as floats
     get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);

ArduCopter/flight_mode.cpp

@@ -47,11 +47,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
             break;
 
         case STABILIZE:
-            #if FRAME_CONFIG == HELI_FRAME
-                success = heli_stabilize_init(ignore_checks);
-            #else
-                success = stabilize_init(ignore_checks);
-            #endif
+                success = flightmode_stabilize.init(ignore_checks);
+                if (success) {
+                    flightmode = &flightmode_stabilize;
+                }
             break;
 
         case ALT_HOLD:
@@ -194,14 +193,6 @@ void Copter::update_flight_mode()
     }
 
     switch (control_mode) {
-        case STABILIZE:
-            #if FRAME_CONFIG == HELI_FRAME
-                heli_stabilize_run();
-            #else
-                stabilize_run();
-            #endif
-            break;
-
         case AUTO:
             auto_run();
             break;
@@ -409,9 +400,6 @@ void Copter::notify_flight_mode()
 
     // set flight mode string
     switch (control_mode) {
-        case STABILIZE:
-            notify.set_flight_mode_str("STAB");
-            break;
         case AUTO:
             notify.set_flight_mode_str("AUTO");
             break;

ArduCopter/heli_control_stabilize.cpp

@@ -6,21 +6,21 @@
  */
 
 // stabilize_init - initialise stabilize controller
-bool Copter::heli_stabilize_init(bool ignore_checks)
+bool Copter::FlightMode_STABILIZE_Heli::init(bool ignore_checks)
 {
     // set target altitude to zero for reporting
     // To-Do: make pos controller aware when it's active/inactive so it can always report the altitude error?
     pos_control->set_alt_target(0);
 
     // set stab collective true to use stabilize scaled collective pitch range
-    input_manager.set_use_stab_col(true);
+    _copter.input_manager.set_use_stab_col(true);
 
     return true;
 }
 
 // stabilize_run - runs the main stabilize controller
 // should be called at 100hz or more
-void Copter::heli_stabilize_run()
+void Copter::FlightMode_STABILIZE_Heli::run()
 {
     float target_roll, target_pitch;
     float target_yaw_rate;
@@ -33,16 +33,16 @@ void Copter::heli_stabilize_run()
     // Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
     
     if(!motors->armed()) {
-        heli_flags.init_targets_on_arming=true;
+        _copter.heli_flags.init_targets_on_arming = true;
         attitude_control->set_yaw_target_to_current_heading();
         attitude_control->reset_rate_controller_I_terms();
     }
     
-    if(motors->armed() && heli_flags.init_targets_on_arming) {
+    if(motors->armed() && _copter.heli_flags.init_targets_on_arming) {
         attitude_control->set_yaw_target_to_current_heading();
         attitude_control->reset_rate_controller_I_terms();
         if (motors->get_interlock()) {
-            heli_flags.init_targets_on_arming=false;
+            _copter.heli_flags.init_targets_on_arming=false;
         }
     }
 
@@ -56,13 +56,13 @@ void Copter::heli_stabilize_run()
 
     // convert pilot input to lean angles
     // To-Do: convert get_pilot_desired_lean_angles to return angles as floats
-    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
+    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, _copter.aparm.angle_max);
 
     // get pilot's desired yaw rate
     target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
 
     // get pilot's desired throttle
-    pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
+    pilot_throttle_scaled = _copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
 
     // call attitude controller
     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());