Copter: encapsulate auto takeoff into an ojbect

similar to the encapsulation of "user takeoff" into an object

ArduCopter/Copter.h

@@ -235,6 +235,8 @@ class Copter : public AP_Vehicle {
     friend class ModeAutorotate;
     friend class ModeTurtle;
 
+    friend class _AutoTakeoff;
+
     Copter(void);
 
 private:

ArduCopter/mode.h

@@ -8,6 +8,27 @@ class ParametersG2;
 
 class GCS_Copter;
 
+// object shared by both Guided and Auto for takeoff.
+// position controller controls vehicle but the user can control the yaw.
+class _AutoTakeoff {
+public:
+    void run();
+    void start(float complete_alt_cm, bool terrain_alt);
+    bool get_position(Vector3p& completion_pos);
+
+    bool complete;          // true when takeoff is complete
+
+private:
+    // altitude above-ekf-origin below which auto takeoff does not control horizontal position
+    bool no_nav_active;
+    float no_nav_alt_cm;
+
+    // auto takeoff variables
+    float complete_alt_cm;  // completion altitude expressed in cm above ekf origin or above terrain (depending upon auto_takeoff_terrain_alt)
+    bool terrain_alt;       // true if altitudes are above terrain
+    Vector3p complete_pos;  // target takeoff position as offset from ekf origin in cm
+};
+
 class Mode {
 
 public:
@@ -51,6 +72,8 @@ class Mode {
     // do not allow copying
     CLASS_NO_COPY(Mode);
 
+    friend class _AutoTakeoff;
+
     // returns a unique number specific to this mode
     virtual Number mode_number() const = 0;
 
@@ -215,21 +238,7 @@ class Mode {
 
     virtual bool do_user_takeoff_start(float takeoff_alt_cm);
 
-    // method shared by both Guided and Auto for takeoff.
-    // position controller controls vehicle but the user can control the yaw.
-    void auto_takeoff_run();
-    void auto_takeoff_start(float complete_alt_cm, bool terrain_alt);
-    bool auto_takeoff_get_position(Vector3p& completion_pos);
-
-    // altitude above-ekf-origin below which auto takeoff does not control horizontal position
-    static bool auto_takeoff_no_nav_active;
-    static float auto_takeoff_no_nav_alt_cm;
-
-    // auto takeoff variables
-    static float auto_takeoff_complete_alt_cm;  // completion altitude expressed in cm above ekf origin or above terrain (depending upon auto_takeoff_terrain_alt)
-    static bool auto_takeoff_terrain_alt;       // true if altitudes are above terrain
-    static bool auto_takeoff_complete;          // true when takeoff is complete
-    static Vector3p auto_takeoff_complete_pos;  // target takeoff position as offset from ekf origin in cm
+    static _AutoTakeoff auto_takeoff;
 
 public:
     // Navigation Yaw control

ArduCopter/mode_auto.cpp

@@ -350,7 +350,7 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
     pos_control->init_z_controller();
 
     // initialise alt for WP_NAVALT_MIN and set completion alt
-    auto_takeoff_start(alt_target_cm, alt_target_terrain);
+    auto_takeoff.start(alt_target_cm, alt_target_terrain);
 
     // set submode
     set_submode(SubMode::TAKEOFF);
@@ -364,7 +364,7 @@ bool ModeAuto::wp_start(const Location& dest_loc)
         Vector3f stopping_point;
         if (_mode == SubMode::TAKEOFF) {
             Vector3p takeoff_complete_pos;
-            if (auto_takeoff_get_position(takeoff_complete_pos)) {
+            if (auto_takeoff.get_position(takeoff_complete_pos)) {
                 stopping_point = takeoff_complete_pos.tofloat();
             }
         }
@@ -536,7 +536,7 @@ bool ModeAuto::is_landing() const
 
 bool ModeAuto::is_taking_off() const
 {
-    return ((_mode == SubMode::TAKEOFF) && !auto_takeoff_complete);
+    return ((_mode == SubMode::TAKEOFF) && !auto_takeoff.complete);
 }
 
 // auto_payload_place_start - initialises controller to implement a placing
@@ -965,7 +965,7 @@ void ModeAuto::takeoff_run()
     if ((copter.g2.auto_options & (int32_t)Options::AllowTakeOffWithoutRaisingThrottle) != 0) {
         copter.set_auto_armed(true);
     }
-    auto_takeoff_run();
+    auto_takeoff.run();
 }
 
 // auto_wp_run - runs the auto waypoint controller
@@ -1326,13 +1326,13 @@ void ModeAuto::payload_place_run()
         FALLTHROUGH;
 
     case PayloadPlaceStateType_Ascent_Start: {
-        auto_takeoff_start(nav_payload_place.descent_start_altitude_cm, false);
+        auto_takeoff.start(nav_payload_place.descent_start_altitude_cm, false);
         nav_payload_place.state = PayloadPlaceStateType_Ascent;
         }
         break;
 
     case PayloadPlaceStateType_Ascent:
-        if (auto_takeoff_complete) {
+        if (auto_takeoff.complete) {
             nav_payload_place.state = PayloadPlaceStateType_Done;
         }
         break;
@@ -1974,13 +1974,13 @@ bool ModeAuto::verify_takeoff()
 {
 #if AP_LANDINGGEAR_ENABLED
     // if we have reached our destination
-    if (auto_takeoff_complete) {
+    if (auto_takeoff.complete) {
         // retract the landing gear
         copter.landinggear.retract_after_takeoff();
     }
 #endif
 
-    return auto_takeoff_complete;
+    return auto_takeoff.complete;
 }
 
 // verify_land - returns true if landing has been completed

ArduCopter/mode_guided.cpp

@@ -153,7 +153,7 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
     pos_control->init_z_controller();
 
     // initialise alt for WP_NAVALT_MIN and set completion alt
-    auto_takeoff_start(alt_target_cm, alt_target_terrain);
+    auto_takeoff.start(alt_target_cm, alt_target_terrain);
 
     // record takeoff has not completed
     takeoff_complete = false;
@@ -656,8 +656,8 @@ void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_
 //      called by guided_run at 100hz or more
 void ModeGuided::takeoff_run()
 {
-    auto_takeoff_run();
-    if (auto_takeoff_complete && !takeoff_complete) {
+    auto_takeoff.run();
+    if (auto_takeoff.complete && !takeoff_complete) {
         takeoff_complete = true;
 #if AP_LANDINGGEAR_ENABLED
         // optionally retract landing gear

ArduCopter/takeoff.cpp

@@ -1,13 +1,7 @@
 #include "Copter.h"
 
 Mode::_TakeOff Mode::takeoff;
-
-bool Mode::auto_takeoff_no_nav_active = false;
-float Mode::auto_takeoff_no_nav_alt_cm = 0;
-float Mode::auto_takeoff_complete_alt_cm = 0;
-bool Mode::auto_takeoff_terrain_alt = false;
-bool Mode::auto_takeoff_complete = false;
-Vector3p Mode::auto_takeoff_complete_pos;
+_AutoTakeoff Mode::auto_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
@@ -118,20 +112,27 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm)
 
 // auto_takeoff_run - controls the vertical position controller during the process of taking off in auto modes
 // auto_takeoff_complete set to true when target altitude is within 10% of the take off altitude and less than 50% max climb rate
-void Mode::auto_takeoff_run()
+void _AutoTakeoff::run()
 {
+    const auto &g2 = copter.g2;
+    const auto &inertial_nav = copter.inertial_nav;
+    const auto &wp_nav = copter.wp_nav;
+    auto *motors = copter.motors;
+    auto *pos_control = copter.pos_control;
+    auto *attitude_control = copter.attitude_control;
+
     // if not armed set throttle to zero and exit immediately
     if (!motors->armed() || !copter.ap.auto_armed) {
         // do not spool down tradheli when on the ground with motor interlock enabled
-        make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
+        copter.flightmode->make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
         // update auto_takeoff_no_nav_alt_cm
-        auto_takeoff_no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100;
+        no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100;
         return;
     }
 
     // get terrain offset
     float terr_offset = 0.0f;
-    if (auto_takeoff_terrain_alt && !wp_nav->get_terrain_offset(terr_offset)) {
+    if (terrain_alt && !wp_nav->get_terrain_offset(terr_offset)) {
         // trigger terrain failsafe
         copter.failsafe_terrain_on_event();
         return;
@@ -151,7 +152,7 @@ void Mode::auto_takeoff_run()
         attitude_control->reset_rate_controller_I_terms();
         attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0);
         // update auto_takeoff_no_nav_alt_cm
-        auto_takeoff_no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100;
+        no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100;
         return;
     }
 
@@ -169,7 +170,7 @@ void Mode::auto_takeoff_run()
         if (throttle >= MIN(copter.g2.takeoff_throttle_max, 0.9) || 
             (copter.pos_control->get_z_accel_cmss() >= 0.5 * copter.pos_control->get_max_accel_z_cmss()) ||
             (copter.pos_control->get_vel_desired_cms().z >= 0.1 * copter.pos_control->get_max_speed_up_cms()) || 
-            ( auto_takeoff_no_nav_active && (inertial_nav.get_position_z_up_cm() >= auto_takeoff_no_nav_alt_cm))) {
+            ( no_nav_active && (inertial_nav.get_position_z_up_cm() >= no_nav_alt_cm))) {
             // throttle > 90%
             // acceleration > 50% maximum acceleration
             // velocity > 10% maximum velocity
@@ -180,10 +181,10 @@ void Mode::auto_takeoff_run()
     }
 
     // check if we are not navigating because of low altitude
-    if (auto_takeoff_no_nav_active) {
+    if (no_nav_active) {
         // check if vehicle has reached no_nav_alt threshold
-        if (inertial_nav.get_position_z_up_cm() >= auto_takeoff_no_nav_alt_cm) {
-            auto_takeoff_no_nav_active = false;
+        if (inertial_nav.get_position_z_up_cm() >= no_nav_alt_cm) {
+            no_nav_active = false;
         }
         pos_control->relax_velocity_controller_xy();
     } else {
@@ -194,15 +195,15 @@ void Mode::auto_takeoff_run()
     pos_control->update_xy_controller();
 
     // command the aircraft to the take off altitude
-    float pos_z = auto_takeoff_complete_alt_cm + terr_offset;
+    float pos_z = complete_alt_cm + terr_offset;
     float vel_z = 0.0;
     copter.pos_control->input_pos_vel_accel_z(pos_z, vel_z, 0.0);
 
     // run the vertical position controller and set output throttle
     pos_control->update_z_controller();
 
     // call attitude controller with auto yaw
-    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), copter.flightmode->auto_yaw.get_heading());
 
     // takeoff complete when we are less than 1% of the stopping distance from the target altitude
     // and 10% our maximum climb rate
@@ -211,40 +212,42 @@ void Mode::auto_takeoff_run()
     const float stop_distance = 0.5 * sq(vel_threshold_fraction * copter.pos_control->get_max_speed_up_cms()) / copter.pos_control->get_max_accel_z_cmss();
     bool reached_altitude = copter.pos_control->get_pos_target_z_cm() >= pos_z - stop_distance;
     bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * vel_threshold_fraction;
-    auto_takeoff_complete = reached_altitude && reached_climb_rate;
+    complete = reached_altitude && reached_climb_rate;
 
     // calculate completion for location in case it is needed for a smooth transition to wp_nav
-    if (auto_takeoff_complete) {
-        const Vector3p& complete_pos = copter.pos_control->get_pos_target_cm();
-        auto_takeoff_complete_pos = Vector3p{complete_pos.x, complete_pos.y, pos_z};
+    if (complete) {
+        const Vector3p& _complete_pos = copter.pos_control->get_pos_target_cm();
+        complete_pos = Vector3p{_complete_pos.x, _complete_pos.y, pos_z};
     }
 }
 
-void Mode::auto_takeoff_start(float complete_alt_cm, bool terrain_alt)
+void _AutoTakeoff::start(float _complete_alt_cm, bool _terrain_alt)
 {
     // auto_takeoff_complete_alt_cm is a problem if equal to auto_takeoff_start_alt_cm
-    auto_takeoff_complete_alt_cm = complete_alt_cm;
-    auto_takeoff_terrain_alt = terrain_alt;
-    auto_takeoff_complete = false;
+    complete_alt_cm = _complete_alt_cm;
+    terrain_alt = _terrain_alt;
+    complete = false;
     // initialise auto_takeoff_no_nav_alt_cm
-    auto_takeoff_no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100;
-    if ((g2.wp_navalt_min > 0) && (is_disarmed_or_landed() || !motors->get_interlock())) {
+    const auto &g2 = copter.g2;
+    const auto &inertial_nav = copter.inertial_nav;
+    no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100;
+    if ((g2.wp_navalt_min > 0) && (copter.flightmode->is_disarmed_or_landed() || !copter.motors->get_interlock())) {
         // we are not flying, climb with no navigation to current alt-above-ekf-origin + wp_navalt_min
-        auto_takeoff_no_nav_active = true;
+        no_nav_active = true;
     } else {
-        auto_takeoff_no_nav_active = false;
+        no_nav_active = false;
     }
 }
 
 // return takeoff final position if takeoff has completed successfully
-bool Mode::auto_takeoff_get_position(Vector3p& complete_pos)
+bool _AutoTakeoff::get_position(Vector3p& _complete_pos)
 {
     // only provide location if takeoff has completed
-    if (!auto_takeoff_complete) {
+    if (!complete) {
         return false;
     }
 
-    complete_pos = auto_takeoff_complete_pos;
+    complete_pos = _complete_pos;
     return true;
 }