Copter: Support Prec Land State Machine

ArduCopter/Copter.h

@@ -113,6 +113,7 @@
 #endif
 #if PRECISION_LANDING == ENABLED
  # include <AC_PrecLand/AC_PrecLand.h>
+ # include <AC_PrecLand/AC_PrecLand_StateMachine.h>
 #endif
 #if MODE_FOLLOW_ENABLED == ENABLED
  # include <AP_Follow/AP_Follow.h>
@@ -523,6 +524,7 @@ private:
     // Precision Landing
 #if PRECISION_LANDING == ENABLED
     AC_PrecLand precland;
+    AC_PrecLand_StateMachine precland_statemachine;
 #endif
 
     // Pilot Input Management Library

ArduCopter/mode.cpp

@@ -699,6 +699,100 @@ void Mode::land_run_horizontal_control()
     }
 }
 
+#if PRECISION_LANDING == ENABLED
+// Go towards a position commanded by prec land state machine in order to retry landing
+// The passed in location is expected to be NED and in m
+void Mode::land_retry_position(const Vector3f &retry_loc)
+{
+    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){
+            AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT);
+            // exit land if throttle is high
+            if (!set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) {
+                set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE);
+            }
+        }
+
+        // allow user to take control during repositioning. Note: copied from land_run_horizontal_control()
+        // To-Do: this code exists at several different places in slightly diffrent forms and that should be fixed
+        if (g.land_repositioning) {
+            float target_roll = 0.0f;
+            float target_pitch = 0.0f;
+            // convert pilot input to lean angles
+            get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
+
+            // record if pilot has overridden roll or pitch
+            if (!is_zero(target_roll) || !is_zero(target_pitch)) {
+                if (!copter.ap.land_repo_active) {
+                    AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE);
+                }
+                // this flag will be checked by prec land state machine later and any further landing retires will be cancelled
+                copter.ap.land_repo_active = true;
+            }
+        }
+    }
+
+    Vector3p retry_loc_NEU{retry_loc.x, retry_loc.y, retry_loc.z * -1.0f};
+    //pos contoller expects input in NEU cm's
+    retry_loc_NEU = retry_loc_NEU * 100.0f;
+    pos_control->input_pos_xyz(retry_loc_NEU, 0.0f, 1000.0f);
+
+    // run position controllers
+    pos_control->update_xy_controller();
+    pos_control->update_z_controller();
+
+    const Vector3f thrust_vector{pos_control->get_thrust_vector()};
+
+    // roll, pitch from position controller, yaw heading from auto_heading()
+    attitude_control->input_thrust_vector_heading(thrust_vector, auto_yaw.yaw());
+}
+
+// Run precland statemachine. This function should be called from any mode that wants to do precision landing.
+// This handles everything from prec landing, to prec landing failures, to retries and failsafe measures
+void Mode::run_precland()
+{
+    // if user is taking control, we will not run the statemachine, and simply land (may or may not be on target)
+    if (!copter.ap.land_repo_active) {
+        // This will get updated later to a retry pos if needed
+        Vector3f retry_pos;
+
+        switch (copter.precland_statemachine.update(retry_pos)) {
+        case AC_PrecLand_StateMachine::Status::RETRYING:
+            // we want to retry landing by going to another position
+            land_retry_position(retry_pos);
+            break;
+
+        case AC_PrecLand_StateMachine::Status::FAILSAFE: {
+            // we have hit a failsafe. Failsafe can only mean two things, we either want to stop permanently till user takes over or land
+            switch (copter.precland_statemachine.get_failsafe_actions()) {
+            case AC_PrecLand_StateMachine::FailSafeAction::DESCEND:
+                // descend normally, prec land target is definitely not in sight
+                run_land_controllers();
+                break;
+            case AC_PrecLand_StateMachine::FailSafeAction::HOLD_POS:
+                // sending "true" in this argument will stop the descend
+                run_land_controllers(true);
+                break;
+            }
+            break;
+        }
+        case AC_PrecLand_StateMachine::Status::ERROR:
+            // should never happen, is certainly a bug. Report then descend
+            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+            FALLTHROUGH;
+        case AC_PrecLand_StateMachine::Status::DESCEND:
+            // run land controller. This will descend towards the target if prec land target is in sight
+            // else it will just descend vertically
+            run_land_controllers();
+            break;
+        }
+    } else {
+        // just land, since user has taken over controls, it does not make sense to run any retries or failsafe measures
+        run_land_controllers();
+    }
+}
+#endif
+
 float Mode::throttle_hover() const
 {
     return motors->get_throttle_hover();

ArduCopter/mode.h

@@ -115,6 +115,10 @@ protected:
     // in modes that support landing
     void land_run_horizontal_control();
     void land_run_vertical_control(bool pause_descent = false);
+    void run_land_controllers(bool pause_descent = false) {
+        land_run_horizontal_control();
+        land_run_vertical_control(pause_descent);
+    }
 
     // return expected input throttle setting to hover:
     virtual float throttle_hover() const;
@@ -240,6 +244,17 @@ public:
     };
     static AutoYaw auto_yaw;
 
+#if PRECISION_LANDING == ENABLED
+    // Go towards a position commanded by prec land state machine in order to retry landing
+    // The passed in location is expected to be NED and in meters
+    void land_retry_position(const Vector3f &retry_loc);
+
+    // Run precland statemachine. This function should be called from any mode that wants to do precision landing.
+    // This handles everything from prec landing, to prec landing failures, to retries and failsafe measures
+    void run_precland();
+
+#endif
+
     // pass-through functions to reduce code churn on conversion;
     // these are candidates for moving into the Mode base
     // class.
@@ -251,7 +266,6 @@ public:
     GCS_Copter &gcs();
     void set_throttle_takeoff(void);
     uint16_t get_pilot_speed_dn(void);
-
     // end pass-through functions
 };
 

ArduCopter/mode_auto.cpp

@@ -50,6 +50,11 @@ bool ModeAuto::init(bool ignore_checks)
         // clear guided limits
         copter.mode_guided.limit_clear();
 
+#if PRECISION_LANDING == ENABLED
+        // initialise precland state machine
+        copter.precland_statemachine.init();
+#endif
+
         return true;
     } else {
         return false;
@@ -883,9 +888,13 @@ void ModeAuto::land_run()
 
     // set motors to full range
     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
-    
+
-    land_run_horizontal_control();
+#if PRECISION_LANDING == ENABLED
-    land_run_vertical_control();
+        // the state machine takes care of the entire landing procedure
+        run_precland();
+#else
+        run_land_controllers();
+#endif
 }
 
 // auto_rtl_run - rtl in AUTO flight mode

ArduCopter/mode_land.cpp

@@ -15,6 +15,7 @@ bool ModeLand::init(bool ignore_checks)
     // set vertical speed and acceleration limits
     pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
     pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
+    pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
 
     // initialise the vertical position controller
     if (!pos_control->is_active_z()) {
@@ -40,6 +41,11 @@ bool ModeLand::init(bool ignore_checks)
     copter.fence.auto_disable_fence_for_landing();
 #endif
 
+#if PRECISION_LANDING == ENABLED
+    // initialise precland state machine
+    copter.precland_statemachine.init();
+#endif
+
     return true;
 }
 
@@ -77,9 +83,17 @@ void ModeLand::gps_run()
         if (land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
             land_pause = false;
         }
-
+#if PRECISION_LANDING == ENABLED
-        land_run_horizontal_control();
+        // the state machine takes care of the entire landing procedure except for land_pause.
-        land_run_vertical_control(land_pause);
+        if (land_pause) {
+            // we don't want to start descending immediately
+            run_land_controllers(true);
+        } else {
+            run_precland();
+        }
+#else
+        run_land_controllers(land_pause);
+#endif
     }
 }
 

ArduCopter/mode_rtl.cpp

@@ -24,6 +24,12 @@ bool ModeRTL::init(bool ignore_checks)
     terrain_following_allowed = !copter.failsafe.terrain;
     // reset flag indicating if pilot has applied roll or pitch inputs during landing
     copter.ap.land_repo_active = false;
+
+#if PRECISION_LANDING == ENABLED
+    // initialise precland state machine
+    copter.precland_statemachine.init();
+#endif
+
     return true;
 }
 
@@ -407,8 +413,12 @@ void ModeRTL::land_run(bool disarm_on_land)
     // set motors to full range
     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
-    land_run_horizontal_control();
+#if PRECISION_LANDING == ENABLED
-    land_run_vertical_control();
+        // the state machine takes care of the entire landing procedure
+        run_precland();
+#else
+        run_land_controllers();
+#endif
 }
 
 void ModeRTL::build_path()