ArduPlane: 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
+    check_takeoff_direction();
+#endif
 }
 
 
@@ -1058,7 +1062,29 @@ void Plane::update_quicktune(void)
     quicktune.update(control_mode->supports_quicktune());
 }
 #endif
+#if MODE_AUTOLAND_ENABLED
+/*
+  In certain pilot controlled modes other than NAV_TAKEOFF or Mode TAKEOFF,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 Plane::check_takeoff_direction()
+{
+    if (takeoff_state.initial_direction.initialized) {
+        return;
+    }
+    //set autoland direction to GPS course over ground
+    if (control_mode->allows_autoland_direction_capture() && (gps.ground_speed() > GPS_GND_CRS_MIN_SPD)) {
+        set_autoland_direction();
+    }      
+}
 
+// Sets autoland direction using ground course + offest parameter
+void Plane::set_autoland_direction()
+{
+    takeoff_state.initial_direction.heading = wrap_360(gps.ground_course() + 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
 /*
   constructor for main Plane class
  */

ArduPlane/Plane.h

@@ -1311,6 +1311,11 @@ class Plane : public AP_Vehicle {
 
     // last target alt we passed to tecs
     int32_t tecs_target_alt_cm;
+#if MODE_AUTOLAND_ENABLED
+    // used to set takeoff direction for AUTOLAND mode in some cases
+    void check_takeoff_direction(void);
+    void set_autoland_direction(void);
+#endif
 
 public:
     void failsafe_check(void);
@@ -1333,6 +1338,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[];
 

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.