From 6bcc2cb500978cfea361a7bacc5e5e653a9c0c60 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Wed, 1 Jan 2025 17:25:12 -0600 Subject: [PATCH] ArduPlane: mode AUTOLAND enhancements Co-authored-by: Andrew Tridgell --- ArduPlane/AP_Arming.cpp | 4 + ArduPlane/Plane.cpp | 4 + ArduPlane/Plane.h | 5 ++ ArduPlane/commands_logic.cpp | 15 ++-- ArduPlane/mode.h | 70 ++++++++++++++- ArduPlane/mode_autoland.cpp | 163 ++++++++++++++++++++++++++--------- ArduPlane/mode_takeoff.cpp | 13 --- 7 files changed, 209 insertions(+), 65 deletions(-) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 25a43be25cab3..f52fd6df10042 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/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; diff --git a/ArduPlane/Plane.cpp b/ArduPlane/Plane.cpp index 2fb6cb1eebdc5..ed451a403dbf8 100644 --- a/ArduPlane/Plane.cpp +++ b/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 } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index b272a13b11ff1..1dda8aa741f40 100644 --- a/ArduPlane/Plane.h +++ b/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; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index af99693635b98..757163e78e206 100644 --- a/ArduPlane/commands_logic.cpp +++ b/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, diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 6e5d0b56327f0..afc60ed1cb5f1 100644 --- a/ArduPlane/mode.h +++ b/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,6 +931,13 @@ 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[]; @@ -894,11 +945,28 @@ class ModeAutoLand: public Mode 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 diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp index e9ec9e8e96442..88840520ca6ab 100644 --- a/ArduPlane/mode_autoland.cpp +++ b/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,33 +52,37 @@ 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 + + // autoland not available for quadplanes as capture of takeoff direction + // doesn't make sense #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; + if (quadplane.available()) { + gcs().send_text(MAV_SEVERITY_WARNING, "autoland not available"); + return false; } #endif + if (!plane.takeoff_state.initial_direction.initialized) { - gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff initial direction not set,must use autotakeoff"); + gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff initial direction not set"); return false; } - plane.prev_WP_loc = plane.current_loc; plane.set_target_altitude_current(); + plane.auto_state.next_wp_crosstrack = true; + + plane.prev_WP_loc = plane.current_loc; /* landing is in 3 steps: - 1) a base leg waypoint - 2) a land start WP, with crosstrack - 3) the landing WP, with crosstrack + 1) a loitering to alt waypoint centered on base leg + 2) exiting and proceeeing to a final approach land start WP, with crosstrack + 3) the landing WP at home, with crosstrack the base leg point is 90 degrees off from the landing leg */ @@ -88,38 +99,37 @@ bool ModeAutoLand::_enter() 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 centered on base leg waypoint. 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; + + // make the "base leg" the smaller of the waypoint loiter radius or 1/3 of the final WP distance + // this results in a neat landing no matter the loiter radius + const float base_leg_length = MIN(final_wp_dist * 0.333, fabsf(plane.aparm.loiter_radius)); + + cmd[0].id = MAV_CMD_NAV_LOITER_TO_ALT; + cmd[0].p1 = base_leg_length; 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; + cmd[0].content.location.loiter_ccw = bearing_err_deg>0? 1 :0; /* - create the WP for the start of the landing + create the WP for the start of the landing final approach */ cmd[1].content.location = land_start_WP; cmd[1].id = MAV_CMD_NAV_WAYPOINT; - // and the land WP + // and the land WP at home cmd[2].id = MAV_CMD_NAV_LAND; cmd[2].content.location = home; - // start first leg - stage = 0; + // start first leg toward the base leg loiter to alt point + stage = AutoLandStage::LOITER; plane.start_command(cmd[0]); - - // don't crosstrack initially - plane.auto_state.crosstrack = false; - plane.auto_state.next_wp_crosstrack = true; - return true; } @@ -129,30 +139,97 @@ 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 AutoLandStage::LOITER: + // check if we have arrived and completed loiter at base leg waypoint + if (plane.verify_loiter_to_alt(cmd[0])) { + stage = AutoLandStage::BASE_LEG; + plane.start_command(cmd[1]); + // setup a predictable turn angle for the base leg + plane.auto_state.next_turn_angle = 90; + plane.auto_state.crosstrack = false; + } + break; + + case AutoLandStage::BASE_LEG: + // check if we have reached the final approach waypoint + if (plane.verify_nav_wp(cmd[1])) { + stage = AutoLandStage::LANDING; + plane.start_command(cmd[2]); + } + break; + + case AutoLandStage::LANDING: plane.set_flight_stage(AP_FixedWing::FlightStage::LAND); - plane.verify_command(cmd[stage]); + plane.verify_command(cmd[2]); + // make sure we line up + plane.auto_state.crosstrack = true; + break; + } +} + +/* + 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 HAL_QUADPLANE_ENABLED + // we don't allow fixed wing autoland for quadplanes + if (quadplane.available()) { return; } +#endif - // 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; + 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.is_flying() && + hal.util->get_soft_armed() && + plane.gps.ground_speed() > GPS_GND_CRS_MIN_SPD) { + set_autoland_direction(); } } -#endif + +// 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)); +} + +/* + return true when the LOITER_TO_ALT is lined up ready for the landing, used in commands_logic verify loiter to alt + */ +bool ModeAutoLand::landing_lined_up(void) +{ + // use the line between the center of the LOITER_TO_ALT on the base leg and the + // start of the landing leg (land_start_WP) + return plane.mode_loiter.isHeadingLinedUp_cd(cmd[0].content.location.get_bearing(cmd[1].content.location)*100); +} + +// possibly capture heading on arming for autoland mode if option is set and using a compass +void ModeAutoLand::arm_check(void) +{ + if (plane.ahrs.use_compass() && 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 // MODE_AUTOLAND_ENABLED + diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 8f18f6d1b2eeb..9eebcd1c119ad 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/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.