ArduPlane: mode AUTOLAND enhancements

Co-authored-by: Andrew Tridgell <[email protected]>

ArduPlane/AP_Arming.cpp

@@ -321,6 +321,10 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c
     // rising edge of delay_arming oneshot
     delay_arming = true;
 
+#if MODE_AUTOLAND_ENABLED
+    plane.mode_autoland.arm_check();
+#endif
+
     send_arm_disarm_statustext("Throttle armed");
 
     return true;

ArduPlane/Plane.cpp

@@ -514,6 +514,10 @@ void Plane::update_control_mode(void)
     update_fly_forward();
 
     control_mode->update();
+
+#if MODE_AUTOLAND_ENABLED
+    mode_autoland.check_takeoff_direction();
+#endif
 }
 
 

ArduPlane/Plane.h

@@ -1333,6 +1333,11 @@ class Plane : public AP_Vehicle {
 
 #endif // AP_SCRIPTING_ENABLED
 
+    bool tkoff_option_is_set(AP_FixedWing::TakeoffOption option) const {
+        return (aparm.takeoff_options & int32_t(option)) != 0;
+    }
+
+
 };
 
 extern Plane plane;

ArduPlane/commands_logic.cpp

@@ -382,9 +382,6 @@ 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();
 }
 
@@ -559,11 +556,6 @@ bool Plane::verify_takeoff()
             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) {
@@ -1173,6 +1165,13 @@ bool Plane::verify_loiter_heading(bool init)
     }
 #endif
 
+#if MODE_AUTOLAND_ENABLED
+    if (control_mode == &mode_autoland) {
+        // autoland mode has its own lineup criterion
+        return mode_autoland.landing_lined_up();
+    }
+#endif
+
     //Get the lat/lon of next Nav waypoint after this one:
     AP_Mission::Mission_Command next_nav_cmd;
     if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1,

ArduPlane/mode.h

@@ -156,6 +156,11 @@ class Mode
 
     // true if voltage correction should be applied to throttle
     virtual bool use_battery_compensation() const;
+
+#if MODE_AUTOLAND_ENABLED   
+    // true if mode allows landing direction to be set on first takeoff after arm in this mode 
+    virtual bool allows_autoland_direction_capture() const { return false; }
+#endif
 
 #if AP_QUICKTUNE_ENABLED
     // does this mode support VTOL quicktune?
@@ -212,6 +217,11 @@ friend class ModeQAcro;
 
     void stabilize_quaternion();
 
+#if MODE_AUTOLAND_ENABLED   
+    // true if mode allows landing direction to be set on first takeoff after arm in this mode 
+    bool allows_autoland_direction_capture() const override { return true; }
+#endif
+
 protected:
 
     // ACRO controller state
@@ -262,6 +272,11 @@ class ModeAuto : public Mode
 
     void run() override;
 
+#if MODE_AUTOLAND_ENABLED   
+    // true if mode allows landing direction to be set on first takeoff after arm in this mode 
+    bool allows_autoland_direction_capture() const override { return true; }
+#endif
+
 #if AP_PLANE_GLIDER_PULLUP_ENABLED
     bool in_pullup() const { return pullup.in_pullup(); }
 #endif
@@ -308,6 +323,11 @@ class ModeAutoTune : public Mode
 
     void run() override;
 
+#if MODE_AUTOLAND_ENABLED   
+    // true if mode allows landing direction to be set on first takeoff after arm in this mode 
+    bool allows_autoland_direction_capture() const override { return true; }
+#endif
+
 protected:
 
     bool _enter() override;
@@ -449,6 +469,11 @@ class ModeManual : public Mode
     // true if voltage correction should be applied to throttle
     bool use_battery_compensation() const override { return false; }
 
+#if MODE_AUTOLAND_ENABLED   
+    // true if mode allows landing direction to be set on first takeoff after arm in this mode 
+    bool allows_autoland_direction_capture() const override { return true; }
+#endif
+
 };
 
 
@@ -495,6 +520,11 @@ class ModeStabilize : public Mode
 
     void run() override;
 
+#if MODE_AUTOLAND_ENABLED   
+    // true if mode allows landing direction to be set on first takeoff after arm in this mode 
+    bool allows_autoland_direction_capture() const override { return true; }
+#endif
+
 private:
     void stabilize_stick_mixing_direct();
 
@@ -513,6 +543,10 @@ class ModeTraining : public Mode
 
     void run() override;
 
+#if MODE_AUTOLAND_ENABLED   
+    // true if mode allows landing direction to be set on first takeoff after arm in this mode 
+    bool allows_autoland_direction_capture() const override { return true; }
+#endif
 };
 
 class ModeInitializing : public Mode
@@ -552,6 +586,11 @@ class ModeFBWA : public Mode
 
     void run() override;
 
+#if MODE_AUTOLAND_ENABLED   
+    // true if mode allows landing direction to be set on first takeoff after arm in this mode 
+    bool allows_autoland_direction_capture() const override { return true; }
+#endif
+
 };
 
 class ModeFBWB : public Mode
@@ -844,6 +883,11 @@ class ModeTakeoff: public Mode
 
     bool does_auto_throttle() const override { return true; }
 
+#if MODE_AUTOLAND_ENABLED   
+    // true if mode allows landing direction to be set on first takeoff after arm in this mode 
+    bool allows_autoland_direction_capture() const override { return true; }
+#endif
+
     // var_info for holding parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -887,18 +931,42 @@ class ModeAutoLand: public Mode
 
     bool does_auto_throttle() const override { return true; }
 
+    void check_takeoff_direction(void);
+
+    // return true when lined up correctly from the LOITER_TO_ALT
+    bool landing_lined_up(void);
+
+    // see if we should capture the direction
+    void arm_check(void);
 
     // 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;
+    AP_Int8  options;
+
+    // Bitfields of AUTOLAND_OPTIONS
+    enum class AutoLandOption {
+        AUTOLAND_DIR_ON_ARM     = (1U << 0), // set dir for autoland on arm if compass in use.
+    };
+
+    enum class AutoLandStage {
+        LOITER,
+        BASE_LEG,
+        LANDING
+    };
+
+    bool autoland_option_is_set(AutoLandOption option) const {
+        return (options & int8_t(option)) != 0;
+    }
 
 protected:
     bool _enter() override;
     AP_Mission::Mission_Command cmd[3];
-    uint8_t stage;
+    AutoLandStage stage;
+    void set_autoland_direction(void);
 };
 #endif
 #if HAL_SOARING_ENABLED