ArduPlane: add AutoLand fixed-wing mode

Hwurzburg · Dec 24, 2024 · d087a82 · d087a82
1 parent 494ff18
commit d087a82
Show file tree

Hide file tree

Showing 17 changed files with 266 additions and 14 deletions.
diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp
@@ -378,8 +378,11 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_chec
     // DO_CHANGE_SPEED commands
     plane.new_airspeed_cm = -1;
 
+#if MODE_AUTOLAND_ENABLED
+    // takeoff direction always cleared on disarm
+    plane.takeoff_state.initial_direction.initialized = false;
+#endif
     send_arm_disarm_statustext("Throttle disarmed");
-
     return true;
 }
 

diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
@@ -59,6 +59,9 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const
     case Mode::Number::GUIDED:
     case Mode::Number::CIRCLE:
     case Mode::Number::TAKEOFF:
+#if MODE_AUTOLAND_ENABLED
+    case Mode::Number::AUTOLAND:
+#endif
 #if HAL_QUADPLANE_ENABLED
     case Mode::Number::QRTL:
     case Mode::Number::LOITER_ALT_QLAND:

diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp
@@ -70,6 +70,9 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
     case Mode::Number::GUIDED:
     case Mode::Number::CIRCLE:
     case Mode::Number::TAKEOFF:
+#if MODE_AUTOLAND_ENABLED
+    case Mode::Number::AUTOLAND:
+#endif
 #if HAL_QUADPLANE_ENABLED
     case Mode::Number::QRTL:
     case Mode::Number::LOITER_ALT_QLAND:

diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
@@ -1028,6 +1028,12 @@ const AP_Param::Info Plane::var_info[] = {
     // @Path: mode_takeoff.cpp
     GOBJECT(mode_takeoff, "TKOFF_", ModeTakeoff),
 
+#if MODE_AUTOLAND_ENABLED
+    // @Group: AUTOLAND_
+    // @Path: mode_autoland.cpp
+    GOBJECT(mode_autoland, "AUTOLAND_", ModeAutoLand),
+#endif
+
 #if AP_PLANE_GLIDER_PULLUP_ENABLED
     // @Group: PUP_
     // @Path: pullup.cpp

diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
@@ -363,6 +363,8 @@ class Parameters {
 
         k_param_pullup = 270,
         k_param_quicktune,
+        k_param_mode_autoland, 
+
     };
 
     AP_Int16 format_version;

diff --git a/ArduPlane/Plane.cpp b/ArduPlane/Plane.cpp
@@ -690,7 +690,11 @@ void Plane::update_flight_stage(void)
             }
 #endif
             set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
-        } else if (control_mode != &mode_takeoff) {
+        } else if ((control_mode != &mode_takeoff)
+#if MODE_AUTOLAND_ENABLED
+             && (control_mode != &mode_autoland)
+#endif
+             ) {
             // If not in AUTO then assume normal operation for normal TECS operation.
             // This prevents TECS from being stuck in the wrong stage if you switch from
             // AUTO to, say, FBWB during a landing, an aborted landing or takeoff.

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
@@ -172,7 +172,9 @@ class Plane : public AP_Vehicle {
     friend class ModeTakeoff;
     friend class ModeThermal;
     friend class ModeLoiterAltQLand;
-
+#if MODE_AUTOLAND_ENABLED
+    friend class ModeAutoLand;
+#endif
 #if AP_EXTERNAL_CONTROL_ENABLED
     friend class AP_ExternalControl_Plane;
 #endif
@@ -326,6 +328,9 @@ class Plane : public AP_Vehicle {
 #endif  // QAUTOTUNE_ENABLED
 #endif  // HAL_QUADPLANE_ENABLED
     ModeTakeoff mode_takeoff;
+#if MODE_AUTOLAND_ENABLED
+    ModeAutoLand mode_autoland;
+#endif
 #if HAL_SOARING_ENABLED
     ModeThermal mode_thermal;
 #endif
@@ -454,6 +459,13 @@ class Plane : public AP_Vehicle {
         float throttle_lim_min;
         uint32_t throttle_max_timer_ms;
         uint32_t level_off_start_time_ms;
+        // Good candidate for keeping the initial time for TKOFF_THR_MAX_T.
+#if MODE_AUTOLAND_ENABLED
+       struct {
+            float heading; // deg
+            bool initialized;
+        } initial_direction;
+#endif
     } takeoff_state;
 
     // ground steering controller state

diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
@@ -382,6 +382,9 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd)
     // zero locked course
     steer_state.locked_course_err = 0;
     steer_state.hold_course_cd = -1;
+#if MODE_AUTOLAND_ENABLED
+    takeoff_state.initial_direction.initialized = false;
+#endif
     auto_state.baro_takeoff_alt = barometer.get_altitude();
 }
 
@@ -550,12 +553,17 @@ bool Plane::verify_takeoff()
     trust_ahrs_yaw |= ahrs.dcm_yaw_initialised();
 #endif
     if (trust_ahrs_yaw && steer_state.hold_course_cd == -1) {
-        const float min_gps_speed = 5;
+        const float min_gps_speed = GPS_GND_CRS_MIN_SPD;
         if (auto_state.takeoff_speed_time_ms == 0 && 
             gps.status() >= AP_GPS::GPS_OK_FIX_3D && 
             gps.ground_speed() > min_gps_speed &&
             hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
             auto_state.takeoff_speed_time_ms = millis();
+#if MODE_AUTOLAND_ENABLED
+            plane.takeoff_state.initial_direction.heading = wrap_360(plane.gps.ground_course() + plane.mode_autoland.landing_dir_off);
+            takeoff_state.initial_direction.initialized = true;
+            gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(takeoff_state.initial_direction.heading));
+#endif
         }
         if (auto_state.takeoff_speed_time_ms != 0 &&
             millis() - auto_state.takeoff_speed_time_ms >= 2000) {

diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp
@@ -83,6 +83,11 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num)
     case Mode::Number::TAKEOFF:
         ret = &mode_takeoff;
         break;
+#if MODE_AUTOLAND_ENABLED
+    case Mode::Number::AUTOLAND:
+        ret = &mode_autoland;
+        break;
+#endif //MODE_AUTOLAND_ENABLED
     case Mode::Number::THERMAL:
 #if HAL_SOARING_ENABLED
         ret = &mode_thermal;

diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h
@@ -10,6 +10,8 @@
 
 #define TAKEOFF_RUDDER_WARNING_TIMEOUT 3000 //ms that GCS warning about not returning arming rudder to neutral repeats
 
+#define GPS_GND_CRS_MIN_SPD 5 // m/s, used to set when intial_direction.heading is captured in NAV_TAKEOFF and Mode TAKEOFF
+
 // failsafe
 // ----------------------
 enum failsafe_state {
@@ -45,6 +47,7 @@ enum failsafe_action_long {
     FS_ACTION_LONG_GLIDE = 2,
     FS_ACTION_LONG_PARACHUTE = 3,
     FS_ACTION_LONG_AUTO = 4,
+    FS_ACTION_LONG_AUTOLAND = 5,
 };
 
 // type of stick mixing enabled

diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp
@@ -65,7 +65,11 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
         break;
 #endif // HAL_QUADPLANE_ENABLED
 
-    case Mode::Number::AUTO: {
+    case Mode::Number::AUTO:
+#if MODE_AUTOLAND_ENABLED
+    case Mode::Number::AUTOLAND:
+#endif
+        {
         if (failsafe_in_landing_sequence()) {
             // don't failsafe in a landing sequence
             break;
@@ -145,6 +149,12 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
             set_mode(mode_fbwa, reason);
         } else if (g.fs_action_long == FS_ACTION_LONG_AUTO) {
             set_mode(mode_auto, reason);
+#if MODE_AUTOLAND_ENABLED
+        } else if (g.fs_action_long == FS_ACTION_LONG_AUTOLAND) {
+            if (!set_mode(mode_autoland, reason)) {
+               set_mode(mode_rtl, reason);
+            }
+#endif
         } else {
             set_mode(mode_rtl, reason);
         }
@@ -194,14 +204,23 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
             set_mode(mode_fbwa, reason);
         } else if (g.fs_action_long == FS_ACTION_LONG_AUTO) {
             set_mode(mode_auto, reason);
+#if MODE_AUTOLAND_ENABLED
+        } else if (g.fs_action_long == FS_ACTION_LONG_AUTOLAND) {
+            if (!set_mode(mode_autoland, reason)) {
+               set_mode(mode_rtl, reason);
+            } 
+#endif           
         } else if (g.fs_action_long == FS_ACTION_LONG_RTL) {
             set_mode(mode_rtl, reason);
         }
         break;
-
     case Mode::Number::RTL:
         if (g.fs_action_long == FS_ACTION_LONG_AUTO) {
             set_mode(mode_auto, reason);
+#if MODE_AUTOLAND_ENABLED
+        } else if (g.fs_action_long == FS_ACTION_LONG_AUTOLAND) {
+            set_mode(mode_autoland, reason);
+#endif
         }
         break;
 #if HAL_QUADPLANE_ENABLED
@@ -210,6 +229,9 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
     case Mode::Number::LOITER_ALT_QLAND:
 #endif
     case Mode::Number::INITIALISING:
+#if MODE_AUTOLAND_ENABLED
+    case Mode::Number::AUTOLAND:
+#endif
         break;
     }
     gcs().send_text(MAV_SEVERITY_WARNING, "%s Failsafe On: %s", (reason == ModeReason:: GCS_FAILSAFE) ? "GCS" : "RC Long", control_mode->name());

diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp
@@ -141,6 +141,10 @@ bool Mode::enter()
 
         // Make sure the flight stage is correct for the new mode
         plane.update_flight_stage();
+
+        // reset landing state
+        plane.landing.reset();
+
 
 #if HAL_QUADPLANE_ENABLED
         if (quadplane.enabled()) {

diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
@@ -15,6 +15,10 @@
 #define AP_QUICKTUNE_ENABLED HAL_QUADPLANE_ENABLED
 #endif
 
+#ifndef MODE_AUTOLAND_ENABLED
+#define MODE_AUTOLAND_ENABLED 1
+#endif
+
 #include <AP_Quicktune/AP_Quicktune.h>
 
 class AC_PosControl;
@@ -60,6 +64,9 @@ class Mode
         THERMAL       = 24,
 #if HAL_QUADPLANE_ENABLED
         LOITER_ALT_QLAND = 25,
+#endif
+#if MODE_AUTOLAND_ENABLED
+        AUTOLAND      = 26,
 #endif
     };
 
@@ -857,7 +864,41 @@ class ModeTakeoff: public Mode
     bool have_autoenabled_fences;
 
 };
+#if MODE_AUTOLAND_ENABLED
+class ModeAutoLand: public Mode
+{
+public:
+    ModeAutoLand();
+
+    Number mode_number() const override { return Number::AUTOLAND; }
+    const char *name() const override { return "AUTOLAND"; }
+    const char *name4() const override { return "ALND"; }
+
+    // methods that affect movement of the vehicle in this mode
+    void update() override;
+
+    void navigate() override;
+
+    bool allows_throttle_nudging() const override { return true; }
 
+    bool does_auto_navigation() const override { return true; }
+
+    bool does_auto_throttle() const override { return true; }
+
+
+    // var_info for holding parameter information
+    static const struct AP_Param::GroupInfo var_info[];
+
+    AP_Int16 final_wp_alt;
+    AP_Int16 final_wp_dist;
+    AP_Int16 landing_dir_off;
+
+protected:
+    bool _enter() override;
+    AP_Mission::Mission_Command cmd;
+    bool land_started;
+};
+#endif
 #if HAL_SOARING_ENABLED
 
 class ModeThermal: public Mode