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.mode_autoland.autoland_option_is_set(ModeAutoLand::AutoLandOption::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/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) {
@@ -1172,11 +1164,14 @@ bool Plane::verify_loiter_heading(bool init)
         return true;
     }
 #endif
-
     //Get the lat/lon of next Nav waypoint after this one:
     AP_Mission::Mission_Command next_nav_cmd;
+    bool in_autoland_mode = false;
+#if MODE_AUTOLAND_ENABLED
+    in_autoland_mode = plane.control_mode-> mode_number() == Mode::Number::AUTOLAND;
+#endif
     if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1,
-                                   next_nav_cmd)) {
+                                   next_nav_cmd) && !in_autoland_mode) {
         //no next waypoint to shoot for -- go ahead and break out of loiter
         return true;
     }
@@ -1185,6 +1180,11 @@ bool Plane::verify_loiter_heading(bool init)
         loiter.sum_cd = 0;
     }
 
+#if MODE_AUTOLAND_ENABLED
+    if (plane.control_mode-> mode_number() == Mode::Number::AUTOLAND){
+        return plane.mode_loiter.isHeadingLinedUp( plane.mode_autoland.loiter_WP,plane.mode_autoland.land_start_WP);
+    } 
+#endif
     return plane.mode_loiter.isHeadingLinedUp(next_WP_loc, next_nav_cmd.content.location);
 }
 

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,18 +922,32 @@ 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[];
 
     AP_Int16 final_wp_alt;
     AP_Int16 final_wp_dist;
     AP_Int16 landing_dir_off;
+    AP_Int8  options;
+    Location land_start_WP;
+    Location loiter_WP;
+
+    // Bitfields of AUTOLAND_OPTIONS
+    enum class AutoLandOption {
+        AUTOLAND_DIR_ON_ARM     = (1U << 0), // set dir for autoland on arm if compass in use.
+    };
+
+    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;
+    void set_autoland_direction(void);
 };
 #endif
 #if HAL_SOARING_ENABLED

ArduPlane/mode_autoland.cpp

@@ -28,13 +28,20 @@ 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
     // @User: Standard
     AP_GROUPINFO("DIR_OFF", 3, ModeAutoLand, landing_dir_off, 0),
 
+    // @Param: OPTIONS
+    // @DisplayName: Autoland mode options
+    // @Description: Enables optional autoland mode behaviors
+    // @Bitmask: 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: Standard
+    AP_GROUPINFO("OPTIONS", 4, ModeAutoLand, options, 0),
+
     AP_GROUPEND
 };
 
@@ -45,18 +52,18 @@ ModeAutoLand::ModeAutoLand() :
 }
 
 bool ModeAutoLand::_enter()
-{   
+{
     //must be flying to enter
-    if (!plane.is_flying()) { 
-        gcs().send_text(MAV_SEVERITY_WARNING, "Must already be flying!"); 
+    if (!plane.is_flying()) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Must already be flying!");
         return false;
     }
-    
+
     //takeoff direction must be set and must not be a quadplane, otherwise since flying switch to RTL so this can be used as FS action
 #if HAL_QUADPLANE_ENABLED
     if (quadplane.available() && !quadplane.option_is_set(QuadPlane::OPTION::ALLOW_FW_LAND)) {
-        gcs().send_text(MAV_SEVERITY_WARNING, "Option not set to allow fixed wing autoland"); 
-        return false; 
+        gcs().send_text(MAV_SEVERITY_WARNING, "Option not set to allow fixed wing autoland");
+        return false;
     }
 #endif
     if (!plane.takeoff_state.initial_direction.initialized) {
@@ -82,25 +89,25 @@ bool ModeAutoLand::_enter()
       NAV_LAND. This is offset by final_wp_dist from home, in a
       direction 180 degrees from takeoff direction
      */
-    Location land_start_WP = home;
+    land_start_WP = home;
     land_start_WP.offset_bearing(wrap_360(plane.takeoff_state.initial_direction.heading + 180), final_wp_dist);
     land_start_WP.set_alt_m(final_wp_alt, Location::AltFrame::ABOVE_HOME);
     land_start_WP.change_alt_frame(Location::AltFrame::ABSOLUTE);
 
     /*
-      now create the initial target waypoint for the base leg. We
+      now create the initial target waypoint for the loitering to alt base leg. We
       choose if we will do a right or left turn onto the landing based
       on where we are when we enter the landing mode
      */
     const float bearing_to_current_deg = wrap_180(degrees(land_start_WP.get_bearing(plane.current_loc)));
     const float bearing_err_deg = wrap_180(wrap_180(plane.takeoff_state.initial_direction.heading) - bearing_to_current_deg);
     const float bearing_offset_deg = bearing_err_deg > 0? -90 : 90;
-    const float base_leg_length = final_wp_dist * 0.333;
-    cmd[0].id = MAV_CMD_NAV_WAYPOINT;
+    const float base_leg_length = plane.aparm.loiter_radius; //make the "base leg" the loiter radius so its a tangent exit always
+    cmd[0].id = MAV_CMD_NAV_LOITER_TO_ALT;
     cmd[0].content.location = land_start_WP;
     cmd[0].content.location.offset_bearing(plane.takeoff_state.initial_direction.heading + bearing_offset_deg, base_leg_length);
-    // set a 1m acceptance radius, we want to fly all the way to this waypoint
-    cmd[0].p1 = 1;
+    loiter_WP = cmd[0].content.location;
+    cmd[0].content.location.loiter_ccw = bearing_err_deg>0? 1 :0;
 
     /*
       create the WP for the start of the landing
@@ -115,6 +122,7 @@ bool ModeAutoLand::_enter()
     // start first leg
     stage = 0;
     plane.start_command(cmd[0]);
+    plane.target_altitude.amsl_cm = cmd[1].content.location.alt;
 
     // don't crosstrack initially
     plane.auto_state.crosstrack = false;
@@ -129,30 +137,59 @@ void ModeAutoLand::update()
     plane.calc_nav_pitch();
     plane.set_offset_altitude_location(plane.prev_WP_loc, plane.next_WP_loc);
     if (plane.landing.is_throttle_suppressed()) {
-       // if landing is considered complete throttle is never allowed, regardless of landing type
-       SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
+        // if landing is considered complete throttle is never allowed, regardless of landing type
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
     } else {
-       plane.calc_throttle();
+        plane.calc_throttle();
     }
 }
 
 void ModeAutoLand::navigate()
 {
-    if (stage == 2) {
-        // we are on final landing leg
+    switch (stage) {
+    case 0:
+        if (plane.verify_loiter_to_alt(cmd[0])) {
+            break;
+        }
+        return;
+    case 1:
+        if (plane.verify_nav_wp(cmd[stage])) {
+            break;
+        }
+        return;
+    case 2:
         plane.set_flight_stage(AP_FixedWing::FlightStage::LAND);
         plane.verify_command(cmd[stage]);
         return;
     }
+    stage++;
+    plane.prev_WP_loc = plane.next_WP_loc;
+    plane.auto_state.next_turn_angle = 90;
+    plane.start_command(cmd[stage]);
+    plane.auto_state.crosstrack = true;
+    plane.auto_state.next_wp_crosstrack = true;
+    return;
+}
 
-    // see if we have completed the leg
-    if (plane.verify_nav_wp(cmd[stage])) {
-        stage++;
-        plane.prev_WP_loc = plane.next_WP_loc;
-        plane.auto_state.next_turn_angle = 90;
-        plane.start_command(cmd[stage]);
-        plane.auto_state.crosstrack = true;
-        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.