Copter: FlightMode - convert LAND flight mode

ArduCopter/Copter.h

@@ -840,14 +840,8 @@ private:
     void flip_run();
     bool guided_nogps_init(bool ignore_checks);
     void guided_nogps_run();
-    bool land_init(bool ignore_checks);
-    void land_run();
-    void land_gps_run();
-    void land_nogps_run();
-    int32_t land_get_alt_above_ground(void);
     void land_run_vertical_control(bool pause_descent = false);
     void land_run_horizontal_control();
-    void land_do_not_use_GPS();
     void set_mode_land_with_pause(mode_reason_t reason);
     bool landing_with_GPS();
     bool poshold_init(bool ignore_checks);
@@ -1104,6 +1098,8 @@ private:
 
     Copter::FlightMode_GUIDED flightmode_guided{*this};
 
+    Copter::FlightMode_LAND flightmode_land{*this};
+
     Copter::FlightMode_LOITER flightmode_loiter{*this};
 
 #if FRAME_CONFIG == HELI_FRAME

ArduCopter/FlightMode.h

@@ -475,3 +475,38 @@ private:
     GuidedMode guided_mode = Guided_TakeOff;
 
 };
+
+
+
+class FlightMode_LAND : public FlightMode {
+
+public:
+
+    FlightMode_LAND(Copter &copter) :
+        Copter::FlightMode(copter)
+        { }
+
+    bool init(bool ignore_checks) override;
+    void run() override; // should be called at 100hz or more
+
+    bool requires_GPS() const override { return false; }
+    bool has_manual_throttle() const override { return false; }
+    bool allows_arming(bool from_gcs) const override { return false; };
+    bool is_autopilot() const override { return true; }
+
+    float get_land_descent_speed();
+    bool landing_with_GPS();
+    void do_not_use_GPS();
+
+    int32_t get_alt_above_ground(void);
+
+protected:
+
+    const char *name() const override { return "LAND"; }
+    const char *name4() const override { return "LAND"; }
+
+private:
+
+    void gps_run();
+    void nogps_run();
+};

ArduCopter/control_land.cpp

@@ -1,15 +1,16 @@
 #include "Copter.h"
 
+// FIXME? why are these static?
 static bool land_with_gps;
 
 static uint32_t land_start_time;
 static bool land_pause;
 
 // land_init - initialise land controller
-bool Copter::land_init(bool ignore_checks)
+bool Copter::FlightMode_LAND::init(bool ignore_checks)
 {
     // check if we have GPS and decide which LAND we're going to do
-    land_with_gps = position_ok();
+    land_with_gps = _copter.position_ok();
     if (land_with_gps) {
         // set target to stopping point
         Vector3f stopping_point;
@@ -39,19 +40,19 @@ bool Copter::land_init(bool ignore_checks)
 
 // land_run - runs the land controller
 // should be called at 100hz or more
-void Copter::land_run()
+void Copter::FlightMode_LAND::run()
 {
     if (land_with_gps) {
-        land_gps_run();
+        gps_run();
     }else{
-        land_nogps_run();
+        nogps_run();
     }
 }
 
 // land_gps_run - runs the land controller
 //      horizontal position controlled with loiter controller
 //      should be called at 100hz or more
-void Copter::land_gps_run()
+void Copter::FlightMode_LAND::gps_run()
 {
     // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
     if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
@@ -68,7 +69,7 @@ void Copter::land_gps_run()
 
         // disarm when the landing detector says we've landed
         if (ap.land_complete) {
-            init_disarm_motors();
+            _copter.init_disarm_motors();
         }
         return;
     }
@@ -81,24 +82,24 @@ void Copter::land_gps_run()
         land_pause = false;
     }
     
-    land_run_horizontal_control();
-    land_run_vertical_control(land_pause);
+    _copter.land_run_horizontal_control();
+    _copter.land_run_vertical_control(land_pause);
 }
 
 // land_nogps_run - runs the land controller
 //      pilot controls roll and pitch angles
 //      should be called at 100hz or more
-void Copter::land_nogps_run()
+void Copter::FlightMode_LAND::nogps_run()
 {
     float target_roll = 0.0f, target_pitch = 0.0f;
     float target_yaw_rate = 0;
 
     // process pilot inputs
-    if (!failsafe.radio) {
-        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
+    if (!_copter.failsafe.radio) {
+        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && _copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
             Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
             // exit land if throttle is high
-            set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
+            _copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
         }
 
         if (g.land_repositioning) {
@@ -106,7 +107,7 @@ void Copter::land_nogps_run()
             update_simple_mode();
 
             // get pilot desired lean angles
-            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
@@ -127,7 +128,7 @@ void Copter::land_nogps_run()
 
         // disarm when the landing detector says we've landed
         if (ap.land_complete) {
-            init_disarm_motors();
+            _copter.init_disarm_motors();
         }
         return;
     }
@@ -143,21 +144,21 @@ void Copter::land_nogps_run()
         land_pause = false;
     }
 
-    land_run_vertical_control(land_pause);
+    _copter.land_run_vertical_control(land_pause);
 }
 
 /*
   get a height above ground estimate for landing
  */
-int32_t Copter::land_get_alt_above_ground(void)
+int32_t Copter::FlightMode_LAND::get_alt_above_ground(void)
 {
     int32_t alt_above_ground;
-    if (rangefinder_alt_ok()) {
-        alt_above_ground = rangefinder_state.alt_cm_filt.get();
+    if (_copter.rangefinder_alt_ok()) {
+        alt_above_ground = _copter.rangefinder_state.alt_cm_filt.get();
     } else {
         bool navigating = pos_control->is_active_xy();
-        if (!navigating || !current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) {
-            alt_above_ground = current_loc.alt;
+        if (!navigating || !_copter.current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) {
+            alt_above_ground = _copter.current_loc.alt;
         }
     }
     return alt_above_ground;
@@ -176,7 +177,7 @@ void Copter::land_run_vertical_control(bool pause_descent)
     // compute desired velocity
     const float precland_acceptable_error = 15.0f;
     const float precland_min_descent_speed = 10.0f;
-    int32_t alt_above_ground = land_get_alt_above_ground();
+    int32_t alt_above_ground = flightmode_land.get_alt_above_ground();
 
     float cmb_rate = 0;
     if (!pause_descent) {
@@ -280,7 +281,7 @@ void Copter::land_run_horizontal_control()
         // there is any position estimate drift after touchdown. We
         // limit attitude to 7 degrees below this limit and linearly
         // interpolate for 1m above that
-        int alt_above_ground = land_get_alt_above_ground();
+        int alt_above_ground = flightmode_land.get_alt_above_ground();
         float attitude_limit_cd = linear_interpolate(700, aparm.angle_max, alt_above_ground,
                                                      g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U);
         float total_angle_cd = norm(nav_roll, nav_pitch);
@@ -299,10 +300,10 @@ void Copter::land_run_horizontal_control()
     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate, get_smoothing_gain());
 }
 
-// land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch
+// do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch
 //  called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS
 //  has no effect if we are not already in LAND mode
-void Copter::land_do_not_use_GPS()
+void Copter::FlightMode_LAND::do_not_use_GPS()
 {
     land_with_gps = false;
 }

ArduCopter/ekf_check.cpp

@@ -175,7 +175,7 @@ void Copter::failsafe_ekf_event()
 
     // if flight mode is already LAND ensure it's not the GPS controlled LAND
     if (control_mode == LAND) {
-        land_do_not_use_GPS();
+        flightmode_land.do_not_use_GPS();
     }
 }
 

ArduCopter/flight_mode.cpp

@@ -89,7 +89,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
             break;
 
         case LAND:
-            success = land_init(ignore_checks);
+            success = flightmode_land.init(ignore_checks);
+            if (success) {
+                flightmode = &flightmode_land;
+            }
             break;
 
         case RTL:
@@ -206,10 +209,6 @@ void Copter::update_flight_mode()
 
     switch (control_mode) {
 
-        case LAND:
-            land_run();
-            break;
-
         case RTL:
             rtl_run();
             break;
@@ -377,12 +376,11 @@ void Copter::notify_flight_mode()
         case RTL:
         case AVOID_ADSB:
         case GUIDED_NOGPS:
-        case LAND:
         case SMART_RTL:
             // autopilot modes
             AP_Notify::flags.autopilot_mode = true;
             break;
-        default:
+    default:
             // all other are manual flight modes
             AP_Notify::flags.autopilot_mode = false;
             break;
@@ -393,9 +391,6 @@ void Copter::notify_flight_mode()
         case RTL:
             notify.set_flight_mode_str("RTL ");
             break;
-        case LAND:
-            notify.set_flight_mode_str("LAND");
-            break;
         case DRIFT:
             notify.set_flight_mode_str("DRIF");
             break;