Plane:mode AUTOLAND enhancements

ArduPlane/AP_Arming.cpp

@@ -321,6 +321,15 @@ 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
+    //capture heading for autoland mode if option is set and using a compass
+    if (plane.ahrs.use_compass() && plane.tkoff_option_is_set(AP_FixedWing::TakeoffOption::AUTOLAND_DIR_ON_ARM)) {
+        plane.takeoff_state.initial_direction.heading = wrap_360(plane.ahrs.yaw_sensor * 0.01f);
+        plane.takeoff_state.initial_direction.initialized = true;
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.initial_direction.heading));
+    }
+#endif
+
     send_arm_disarm_statustext("Throttle armed");
 
     return true;

ArduPlane/Parameters.cpp

@@ -170,7 +170,7 @@ const AP_Param::Info Plane::var_info[] = {
     // @Param: TKOFF_OPTIONS
     // @DisplayName: Takeoff options
     // @Description: This selects the mode of the takeoff in AUTO and TAKEOFF flight modes. 
-    // @Bitmask: 0: When unset the maximum allowed throttle is always used (THR_MAX or TKOFF_THR_MAX) during takeoff. When set TECS is allowed to operate between a minimum (THR_MIN or TKOFF_THR_MIN) and a maximum (THR_MAX or TKOFF_THR_MAX) limit. Applicable only when using an airspeed sensor.
+    // @Bitmask: 0: When unset the maximum allowed throttle is always used (THR_MAX or TKOFF_THR_MAX) during takeoff. When set TECS is allowed to operate between a minimum (THR_MIN or TKOFF_THR_MIN) and a maximum (THR_MAX or TKOFF_THR_MAX) limit. Applicable only when using an airspeed sensor., 1: When set if there is a healthy compass in use the compass heading will be captured at arming and used for the AUTOLAND mode's initial takeoff direction instead of capturing ground course in NAV_TAKEOFF or Mode TAKEOFF or other modes.
     // @User: Advanced
     ASCALAR(takeoff_options,               "TKOFF_OPTIONS",       0),
 

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) {

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?
@@ -262,6 +267,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
@@ -449,6 +459,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 +510,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();
 
@@ -552,6 +572,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
@@ -788,6 +813,11 @@ class ModeQAcro : public Mode
     bool is_vtol_man_throttle() const override { return true; }
     virtual bool is_vtol_man_mode() 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
+
     // methods that affect movement of the vehicle in this mode
     void update() override;
 
@@ -844,6 +874,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,6 +922,7 @@ class ModeAutoLand: public Mode
 
     bool does_auto_throttle() const override { return true; }
 
+    void check_takeoff_direction(void);
 
     // var_info for holding parameter information
     static const struct AP_Param::GroupInfo var_info[];
@@ -899,6 +935,7 @@ class ModeAutoLand: public Mode
     bool _enter() override;
     AP_Mission::Mission_Command cmd[3];
     uint8_t stage;
+    void set_autoland_direction(void);
 };
 #endif
 #if HAL_SOARING_ENABLED

ArduPlane/mode_autoland.cpp

@@ -28,7 +28,7 @@ const AP_Param::GroupInfo ModeAutoLand::var_info[] = {
 
     // @Param: DIR_OFF
     // @DisplayName: Landing direction offset from takeoff
-    // @Description: The captured takeoff direction (at arming,if TKOFF_OPTION bit1 is set, or after ground course is established in autotakeoffs)is offset by this amount to create a different landing direction and approach.
+    // @Description: The captured takeoff direction after ground course is established in autotakeoffsis offset by this amount to create a different landing direction and approach.However,if TKOFF_OPTION bit1 is set, the takeoff(landing) direction is captured immediately via compass heading upon arming, then this offset is NOT applied.
     // @Range: -360 360
     // @Increment: 1
     // @Units: deg
@@ -155,4 +155,26 @@ void ModeAutoLand::navigate()
         plane.auto_state.next_wp_crosstrack = true;
     }
 }
+
+/*
+  Takeoff direction is initialized after arm when sufficient altitude and ground speed is obtained, then captured takeoff direction + offset used as landing direction in AUTOLAND
+*/
+void ModeAutoLand::check_takeoff_direction()
+{
+    if (plane.takeoff_state.initial_direction.initialized) {
+        return;
+    }
+    //set autoland direction to GPS course over ground
+    if (plane.control_mode->allows_autoland_direction_capture() && (plane.gps.ground_speed() > GPS_GND_CRS_MIN_SPD)) {
+        set_autoland_direction();
+    }      
+}
+
+// Sets autoland direction using ground course + offest parameter
+void ModeAutoLand::set_autoland_direction()
+{
+    plane.takeoff_state.initial_direction.heading = wrap_360(plane.gps.ground_course() + landing_dir_off);
+    plane.takeoff_state.initial_direction.initialized = true;
+    gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.initial_direction.heading));
+}
 #endif

ArduPlane/mode_takeoff.cpp

@@ -134,9 +134,6 @@ void ModeTakeoff::update()
                 plane.takeoff_state.throttle_max_timer_ms = millis();
                 takeoff_mode_setup = true;
                 plane.steer_state.hold_course_cd = wrap_360_cd(direction*100); // Necessary to allow Plane::takeoff_calc_roll() to function.
-#if MODE_AUTOLAND_ENABLED
-                plane.takeoff_state.initial_direction.initialized = false;
-#endif
             }
         }
     }
@@ -145,16 +142,6 @@ void ModeTakeoff::update()
         plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
         takeoff_mode_setup = false;
     }
-#if MODE_AUTOLAND_ENABLED
-    // set initial_direction.heading
-    const float min_gps_speed = GPS_GND_CRS_MIN_SPD;
-    if (!(plane.takeoff_state.initial_direction.initialized) && (plane.gps.ground_speed() > min_gps_speed)
-       && (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF)) {
-       plane.takeoff_state.initial_direction.heading = wrap_360(plane.gps.ground_course() + plane.mode_autoland.landing_dir_off);
-       plane.takeoff_state.initial_direction.initialized = true;
-       gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.initial_direction.heading));
-    }
-#endif
     // We update the waypoint to follow once we're past TKOFF_LVL_ALT or we
     // pass the target location. The check for target location prevents us
     // flying towards a wrong location if we can't climb.

Tools/autotest/arduplane.py

@@ -4241,17 +4241,28 @@ def FlyEachFrame(self):
 
     def AutoLandMode(self):
         '''Test AUTOLAND mode'''
+        self.set_parameters({
+            "AUTOLAND_DIR_OFF": 45,
+        })
         self.customise_SITL_commandline(["--home", "-35.362938,149.165085,585,173"])
         self.context_collect('STATUSTEXT')
         self.takeoff(alt=80, mode='TAKEOFF')
         self.wait_text("Autoland direction", check_context=True)
         self.change_mode(26)
         self.wait_disarmed(120)
-        self.progress("Check the landed heading matches takeoff")
-        self.wait_heading(173, accuracy=5, timeout=1)
-        loc = mavutil.location(-35.362938, 149.165085, 585, 173)
+        self.progress("Check the landed heading matches takeoff plus offset")
+        self.wait_heading(218, accuracy=5, timeout=1)
+        loc = mavutil.location(-35.362938, 149.165085, 585, 218)
         if self.get_distance(loc, self.mav.location()) > 35:
             raise NotAchievedException("Did not land close to home")
+        self.set_parameters({
+            "TKOFF_OPTIONS": 2,
+        })
+        self.wait_ready_to_arm()
+        self.set_autodisarm_delay(0)
+        self.arm_vehicle()
+        self.progress("Check the set dir on arm option")
+        self.wait_text("Autoland direction", check_context=True)
 
     def RCDisableAirspeedUse(self):
         '''Test RC DisableAirspeedUse option'''

libraries/AP_Vehicle/AP_FixedWing.h

@@ -56,5 +56,6 @@ struct AP_FixedWing {
     // Bitfields of TKOFF_OPTIONS
     enum class TakeoffOption {
         THROTTLE_RANGE          = (1U << 0), // Unset: Max throttle. Set: Throttle range.
+        AUTOLAND_DIR_ON_ARM     = (1U << 1), // set dir for autoland on arm if compass in use.
     };
 };