Copter: create Copter::Mode::_TakeOff subobject from takeoff_state

ArduCopter/mode.cpp

@@ -352,9 +352,9 @@ void Copter::Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_o
     // roll_out and pitch_out are returned
 }
 
-bool Copter::Mode::takeoff_triggered(const float target_climb_rate) const
+bool Copter::Mode::_TakeOff::triggered(const float target_climb_rate) const
 {
-    if (!ap.land_complete) {
+    if (!copter.ap.land_complete) {
         // can't take off if we're already flying
         return false;
     }
@@ -363,7 +363,7 @@ bool Copter::Mode::takeoff_triggered(const float target_climb_rate) const
         return false;
     }
 #if FRAME_CONFIG == HELI_FRAME
-    if (!motors->rotor_runup_complete()) {
+    if (!copter.motors->rotor_runup_complete()) {
         // hold heli on the ground until rotor speed runup has finished
         return false;
     }

ArduCopter/mode.h

@@ -134,21 +134,27 @@ protected:
     ap_t ≈
 
     // auto-takeoff support; takeoff state is shared across all mode instances
-    typedef struct {
-        bool running;
+    class _TakeOff {
+    public:
+        void start(float alt_cm);
+        void stop();
+        void get_climb_rates(float& pilot_climb_rate,
+                             float& takeoff_climb_rate);
+        bool triggered(float target_climb_rate) const;
+
+        bool running() const { return _running; }
+    private:
+        bool _running;
         float max_speed;
         float alt_delta;
         uint32_t start_ms;
-    } takeoff_state_t;
+    };
 
-    static takeoff_state_t takeoff_state;
+    static _TakeOff takeoff;
 
-    void takeoff_timer_start(float alt_cm);
-    void takeoff_stop();
-    void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate);
+    static void takeoff_stop() { takeoff.stop(); }
 
     // takeoff support
-    bool takeoff_triggered(float target_climb_rate) const;
     virtual bool do_user_takeoff_start(float takeoff_alt_cm);
 
     // gnd speed limit required to observe optical flow sensor limits

ArduCopter/mode_althold.cpp

@@ -45,7 +45,7 @@ void Copter::ModeAltHold::run()
     // Alt Hold State Machine Determination
     if (!motors->armed() || !motors->get_interlock()) {
         althold_state = AltHold_MotorStopped;
-    } else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) {
+    } else if (takeoff.running() || takeoff.triggered(target_climb_rate)) {
         althold_state = AltHold_Takeoff;
     } else if (!ap.auto_armed || ap.land_complete) {
         althold_state = AltHold_Landed;
@@ -85,8 +85,8 @@ void Copter::ModeAltHold::run()
         motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
         // initiate take-off
-        if (!takeoff_state.running) {
-            takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
+        if (!takeoff.running()) {
+            takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
             // indicate we are taking off
             set_land_complete(false);
             // clear i terms
@@ -94,7 +94,7 @@ void Copter::ModeAltHold::run()
         }
 
         // get take-off adjusted pilot and takeoff climb rates
-        takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
+        takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate);
 
         // get avoidance adjusted climb rate
         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);

ArduCopter/mode_flowhold.cpp

@@ -244,7 +244,7 @@ void Copter::ModeFlowHold::run()
     
     if (!copter.motors->armed() || !copter.motors->get_interlock()) {
         flowhold_state = FlowHold_MotorStopped;
-    } else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) {
+    } else if (takeoff.running() || takeoff.triggered(target_climb_rate)) {
         flowhold_state = FlowHold_Takeoff;
     } else if (!copter.ap.auto_armed || copter.ap.land_complete) {
         flowhold_state = FlowHold_Landed;
@@ -304,8 +304,8 @@ void Copter::ModeFlowHold::run()
         copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
         // initiate take-off
-        if (!takeoff_state.running) {
-            takeoff_timer_start(constrain_float(copter.g.pilot_takeoff_alt,0.0f,1000.0f));
+        if (!takeoff.running()) {
+            takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
             // indicate we are taking off
             copter.set_land_complete(false);
             // clear i terms
@@ -313,7 +313,7 @@ void Copter::ModeFlowHold::run()
         }
 
         // get take-off adjusted pilot and takeoff climb rates
-        takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
+        takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate);
 
         // get avoidance adjusted climb rate
         target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate);

ArduCopter/mode_loiter.cpp

@@ -105,7 +105,7 @@ void Copter::ModeLoiter::run()
     // Loiter State Machine Determination
     if (!motors->armed() || !motors->get_interlock()) {
         loiter_state = Loiter_MotorStopped;
-    } else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) {
+    } else if (takeoff.running() || takeoff.triggered(target_climb_rate)) {
         loiter_state = Loiter_Takeoff;
     } else if (!ap.auto_armed || ap.land_complete) {
         loiter_state = Loiter_Landed;
@@ -141,8 +141,8 @@ void Copter::ModeLoiter::run()
         motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
         // initiate take-off
-        if (!takeoff_state.running) {
-            takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
+        if (!takeoff.running()) {
+            takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
             // indicate we are taking off
             set_land_complete(false);
             // clear i term when we're taking off
@@ -150,7 +150,7 @@ void Copter::ModeLoiter::run()
         }
 
         // get takeoff adjusted pilot and takeoff climb rates
-        takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
+        takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate);
 
         // get avoidance adjusted climb rate
         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);

ArduCopter/mode_poshold.cpp

@@ -155,17 +155,17 @@ void Copter::ModePosHold::run()
         target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
 
         // get takeoff adjusted pilot and takeoff climb rates
-        takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
+        takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate);
 
         // check for take-off
 #if FRAME_CONFIG == HELI_FRAME
         // helicopters are held on the ground until rotor speed runup has finished
-        if (ap.land_complete && (takeoff_state.running || (target_climb_rate > 0.0f && motors->rotor_runup_complete()))) {
+        if (ap.land_complete && (takeoff.running() || (target_climb_rate > 0.0f && motors->rotor_runup_complete()))) {
 #else
-        if (ap.land_complete && (takeoff_state.running || target_climb_rate > 0.0f)) {
+        if (ap.land_complete && (takeoff.running() || target_climb_rate > 0.0f)) {
 #endif
-            if (!takeoff_state.running) {
-                takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
+            if (!takeoff.running()) {
+                takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
             }
 
             // indicate we are taking off

ArduCopter/mode_sport.cpp

@@ -74,7 +74,7 @@ void Copter::ModeSport::run()
     // State Machine Determination
     if (!motors->armed() || !motors->get_interlock()) {
         sport_state = Sport_MotorStopped;
-    } else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) {
+    } else if (takeoff.running() || takeoff.triggered(target_climb_rate)) {
         sport_state = Sport_Takeoff;
     } else if (!ap.auto_armed || ap.land_complete) {
         sport_state = Sport_Landed;
@@ -106,8 +106,8 @@ void Copter::ModeSport::run()
         motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
         // initiate take-off
-        if (!takeoff_state.running) {
-            takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
+        if (!takeoff.running()) {
+            takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
             // indicate we are taking off
             set_land_complete(false);
             // clear i terms
@@ -115,7 +115,7 @@ void Copter::ModeSport::run()
         }
 
         // get take-off adjusted pilot and takeoff climb rates
-        takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
+        takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate);
 
         // get avoidance adjusted climb rate
         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);

ArduCopter/takeoff.cpp

@@ -1,6 +1,6 @@
 #include "Copter.h"
 
-Copter::Mode::takeoff_state_t Copter::Mode::takeoff_state;
+Copter::Mode::_TakeOff Copter::Mode::takeoff;
 
 // This file contains the high-level takeoff logic for Loiter, PosHold, AltHold, Sport modes.
 //   The take-off can be initiated from a GCS NAV_TAKEOFF command which includes a takeoff altitude
@@ -9,7 +9,7 @@ Copter::Mode::takeoff_state_t Copter::Mode::takeoff_state;
 
 bool Copter::Mode::do_user_takeoff_start(float takeoff_alt_cm)
 {
-    takeoff_timer_start(takeoff_alt_cm);
+    copter.flightmode->takeoff.start(takeoff_alt_cm);
     return true;
 }
 
@@ -48,60 +48,61 @@ bool Copter::Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
 }
 
 // start takeoff to specified altitude above home in centimeters
-void Copter::Mode::takeoff_timer_start(float alt_cm)
+void Copter::Mode::_TakeOff::start(float alt_cm)
 {
     // calculate climb rate
-    float speed = MIN(wp_nav->get_speed_up(), MAX(g.pilot_speed_up*2.0f/3.0f, g.pilot_speed_up-50.0f));
+    const float speed = MIN(copter.wp_nav->get_speed_up(), MAX(copter.g.pilot_speed_up*2.0f/3.0f, copter.g.pilot_speed_up-50.0f));
 
     // sanity check speed and target
-    if (takeoff_state.running || speed <= 0.0f || alt_cm <= 0.0f) {
+    if (running() || speed <= 0.0f || alt_cm <= 0.0f) {
         return;
     }
 
     // initialise takeoff state
-    takeoff_state.running = true;
-    takeoff_state.max_speed = speed;
-    takeoff_state.start_ms = millis();
-    takeoff_state.alt_delta = alt_cm;
+    _running = true;
+    max_speed = speed;
+    start_ms = millis();
+    alt_delta = alt_cm;
 }
 
 // stop takeoff
-void Copter::Mode::takeoff_stop()
+void Copter::Mode::_TakeOff::stop()
 {
-    takeoff_state.running = false;
-    takeoff_state.start_ms = 0;
+    _running = false;
+    start_ms = 0;
 }
 
 // returns pilot and takeoff climb rates
 //  pilot_climb_rate is both an input and an output
 //  takeoff_climb_rate is only an output
 //  has side-effect of turning takeoff off when timeout as expired
-void Copter::Mode::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate)
+void Copter::Mode::_TakeOff::get_climb_rates(float& pilot_climb_rate,
+                                                  float& takeoff_climb_rate)
 {
     // return pilot_climb_rate if take-off inactive
-    if (!takeoff_state.running) {
+    if (!_running) {
         takeoff_climb_rate = 0.0f;
         return;
     }
 
     // acceleration of 50cm/s/s
-    static const float takeoff_accel = 50.0f;
-    float takeoff_minspeed = MIN(50.0f,takeoff_state.max_speed);
-    float time_elapsed = (millis()-takeoff_state.start_ms)*1.0e-3f;
-    float speed = MIN(time_elapsed*takeoff_accel+takeoff_minspeed, takeoff_state.max_speed);
+    static constexpr float TAKEOFF_ACCEL = 50.0f;
+    const float takeoff_minspeed = MIN(50.0f, max_speed);
+    const float time_elapsed = (millis() - start_ms) * 1.0e-3f;
+    const float speed = MIN(time_elapsed * TAKEOFF_ACCEL + takeoff_minspeed, max_speed);
 
-    float time_to_max_speed = (takeoff_state.max_speed-takeoff_minspeed)/takeoff_accel;
+    const float time_to_max_speed = (max_speed - takeoff_minspeed) / TAKEOFF_ACCEL;
     float height_gained;
     if (time_elapsed <= time_to_max_speed) {
-        height_gained = 0.5f*takeoff_accel*sq(time_elapsed) + takeoff_minspeed*time_elapsed;
+        height_gained = 0.5f * TAKEOFF_ACCEL * sq(time_elapsed) + takeoff_minspeed * time_elapsed;
     } else {
-        height_gained = 0.5f*takeoff_accel*sq(time_to_max_speed) + takeoff_minspeed*time_to_max_speed +
-                        (time_elapsed-time_to_max_speed)*takeoff_state.max_speed;
+        height_gained = 0.5f * TAKEOFF_ACCEL * sq(time_to_max_speed) + takeoff_minspeed * time_to_max_speed +
+                        (time_elapsed - time_to_max_speed) * max_speed;
     }
 
     // check if the takeoff is over
-    if (height_gained >= takeoff_state.alt_delta) {
-        takeoff_stop();
+    if (height_gained >= alt_delta) {
+        stop();
     }
 
     // if takeoff climb rate is zero return
@@ -118,7 +119,7 @@ void Copter::Mode::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeo
         // if overall climb rate is still positive, move to take-off climb rate
         if (takeoff_climb_rate + pilot_climb_rate > 0.0f) {
             takeoff_climb_rate = takeoff_climb_rate + pilot_climb_rate;
-            pilot_climb_rate = 0;
+            pilot_climb_rate = 0.0f;
         } else {
             // if overall is negative, move to pilot climb rate
             pilot_climb_rate = pilot_climb_rate + takeoff_climb_rate;