Copter: FlightMode - convert ACRO flight mode

ArduCopter/Copter.h

@@ -795,8 +795,6 @@ private:
     bool verify_yaw();
     MAV_RESULT mavlink_compassmot(mavlink_channel_t chan);
     void delay(uint32_t ms);
-    bool acro_init(bool ignore_checks);
-    void acro_run();
     void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out);
     bool althold_init(bool ignore_checks);
     void althold_run();
@@ -1015,8 +1013,6 @@ private:
     void update_heli_control_dynamics(void);
     void heli_update_landing_swash();
     void heli_update_rotor_speed_targets();
-    bool heli_acro_init(bool ignore_checks);
-    void heli_acro_run();
     bool heli_stabilize_init(bool ignore_checks);
     void heli_stabilize_run();
     void read_inertia();
@@ -1169,6 +1165,13 @@ private:
 
     Copter::FlightMode *flightmode;
 
+#if FRAME_CONFIG == HELI_FRAME
+    Copter::FlightMode_ACRO_Heli flightmode_acro{*this};
+#else
+    Copter::FlightMode_ACRO flightmode_acro{*this};
+#endif
+
+
 public:
     void mavlink_delay_cb();
     void failsafe_check();

ArduCopter/FlightMode.h

@@ -151,3 +151,50 @@ protected:
 
     // end pass-through functions
 };
+
+
+class FlightMode_ACRO : public FlightMode {
+
+public:
+
+    FlightMode_ACRO(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 is_autopilot() const override { return false; }
+    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; };
+
+protected:
+
+    const char *name() const override { return "ACRO"; }
+    const char *name4() const override { return "ACRO"; }
+
+    void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out);
+
+private:
+
+};
+
+#if FRAME_CONFIG == HELI_FRAME
+class FlightMode_ACRO_Heli : public FlightMode_ACRO {
+
+public:
+
+    FlightMode_ACRO_Heli(Copter &copter) :
+        Copter::FlightMode_ACRO(copter)
+        { }
+
+    bool init(bool ignore_checks) override;
+    void run() override; // should be called at 100hz or more
+
+    void get_pilot_desired_yaw_rate(int16_t yaw_in, float &yaw_out);
+
+protected:
+private:
+};
+#endif
+

ArduCopter/control_acro.cpp

@@ -1,16 +1,16 @@
 #include "Copter.h"
 
+#include "FlightMode.h"
 
 /*
  * Init and run calls for acro flight mode
  */
 
-// acro_init - initialise acro controller
-bool Copter::acro_init(bool ignore_checks)
+bool Copter::FlightMode_ACRO::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) &&
-           (get_pilot_desired_throttle(channel_throttle->get_control_in(), g2.acro_thr_mid) > get_non_takeoff_throttle())) {
+   if (motors->armed() && ap.land_complete && !_copter.mode_has_manual_throttle(_copter.control_mode) &&
+           (get_pilot_desired_throttle(channel_throttle->get_control_in(), _copter.g2.acro_thr_mid) > _copter.get_non_takeoff_throttle())) {
        return false;
    }
    // set target altitude to zero for reporting
@@ -19,9 +19,7 @@ bool Copter::acro_init(bool ignore_checks)
    return true;
 }
 
-// acro_run - runs the acro controller
-// should be called at 100hz or more
-void Copter::acro_run()
+void Copter::FlightMode_ACRO::run()
 {
     float target_roll, target_pitch, target_yaw;
     float pilot_throttle_scaled;
@@ -54,11 +52,13 @@ void Copter::acro_run()
 
 // get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates
 // returns desired angle rates in centi-degrees-per-second
-void Copter::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
+void Copter::FlightMode_ACRO::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
 {
     float rate_limit;
     Vector3f rate_ef_level, rate_bf_level, rate_bf_request;
 
+    AP_Vehicle::MultiCopter &aparm = _copter.aparm;
+
     // apply circular limit to pitch and roll inputs
     float total_in = norm(pitch_in, roll_in);
 

ArduCopter/flight_mode.cpp

@@ -40,11 +40,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
 
     switch (mode) {
         case ACRO:
-            #if FRAME_CONFIG == HELI_FRAME
-                success = heli_acro_init(ignore_checks);
-            #else
-                success = acro_init(ignore_checks);
-            #endif
+                success = flightmode_acro.init(ignore_checks);
+                if (success) {
+                    flightmode = &flightmode_acro;
+                }
             break;
 
         case STABILIZE:
@@ -192,14 +191,6 @@ void Copter::update_flight_mode()
     }
 
     switch (control_mode) {
-        case ACRO:
-            #if FRAME_CONFIG == HELI_FRAME
-                heli_acro_run();
-            #else
-                acro_run();
-            #endif
-            break;
-
         case STABILIZE:
             #if FRAME_CONFIG == HELI_FRAME
                 heli_stabilize_run();
@@ -422,9 +413,6 @@ void Copter::notify_flight_mode()
         case STABILIZE:
             notify.set_flight_mode_str("STAB");
             break;
-        case ACRO:
-            notify.set_flight_mode_str("ACRO");
-            break;
         case ALT_HOLD:
             notify.set_flight_mode_str("ALTH");
             break;

ArduCopter/heli_control_acro.cpp

@@ -6,7 +6,7 @@
  */
 
 // heli_acro_init - initialise acro controller
-bool Copter::heli_acro_init(bool ignore_checks)
+bool Copter::FlightMode_ACRO_Heli::init(bool ignore_checks)
 {
     // if heli is equipped with a flybar, then tell the attitude controller to pass through controls directly to servos
     attitude_control->use_flybar_passthrough(motors->has_flybar(), motors->supports_yaw_passthrough());
@@ -14,7 +14,7 @@ bool Copter::heli_acro_init(bool ignore_checks)
     motors->set_acro_tail(true);
     
     // set stab collective false to use full collective pitch range
-    input_manager.set_use_stab_col(false);
+    _copter.input_manager.set_use_stab_col(false);
 
     // always successfully enter acro
     return true;
@@ -22,7 +22,7 @@ bool Copter::heli_acro_init(bool ignore_checks)
 
 // heli_acro_run - runs the acro controller
 // should be called at 100hz or more
-void Copter::heli_acro_run()
+void Copter::FlightMode_ACRO_Heli::run()
 {
     float target_roll, target_pitch, target_yaw;
     float pilot_throttle_scaled;
@@ -34,16 +34,16 @@ void Copter::heli_acro_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_attitude_target_to_current_attitude();
         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_attitude_target_to_current_attitude();
         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;
         }
     }   
 
@@ -91,7 +91,7 @@ void Copter::heli_acro_run()
     }
 
     // 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());
 
     // output pilot's throttle without angle boost
     attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);