From 010e6ba0b0f777d53999c62bbaa19c019a07ea1e Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Wed, 15 Jan 2025 16:05:26 -0600 Subject: [PATCH] ArduPlane: mode AUTOLAND enhancements Co-authored-by: Andrew Tridgell Co-authored-by: Pete Hall --- ArduPlane/AP_Arming.cpp | 4 + ArduPlane/Plane.cpp | 4 + ArduPlane/Plane.h | 5 + ArduPlane/commands_logic.cpp | 32 +++--- ArduPlane/mode.h | 73 +++++++++++- ArduPlane/mode_autoland.cpp | 211 ++++++++++++++++++++++++----------- ArduPlane/mode_takeoff.cpp | 13 --- 7 files changed, 248 insertions(+), 94 deletions(-) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 25a43be25cab36..f52fd6df100422 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 2fb6cb1eebdc51..ed451a403dbf89 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 b272a13b11ff12..1dda8aa741f40e 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 af99693635b98a..b5f91f89d5b421 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -26,15 +26,18 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) // reset loiter start time. New command is a new loiter loiter.start_time_ms = 0; - AP_Mission::Mission_Command next_nav_cmd; - const uint16_t next_index = mission.get_current_nav_index() + 1; - const bool have_next_cmd = mission.get_next_nav_cmd(next_index, next_nav_cmd); - auto_state.wp_is_land_approach = have_next_cmd && (next_nav_cmd.id == MAV_CMD_NAV_LAND); + // Mission lookahead is only valid in auto + if (control_mode == &mode_auto) { + AP_Mission::Mission_Command next_nav_cmd; + const uint16_t next_index = mission.get_current_nav_index() + 1; + const bool have_next_cmd = mission.get_next_nav_cmd(next_index, next_nav_cmd); + auto_state.wp_is_land_approach = have_next_cmd && (next_nav_cmd.id == MAV_CMD_NAV_LAND); #if HAL_QUADPLANE_ENABLED - if (have_next_cmd && quadplane.is_vtol_land(next_nav_cmd.id)) { - auto_state.wp_is_land_approach = false; - } + if (have_next_cmd && quadplane.is_vtol_land(next_nav_cmd.id)) { + auto_state.wp_is_land_approach = false; + } #endif + } } switch(cmd.id) { @@ -382,9 +385,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 +559,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 +1168,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 6e5d0b56327f0a..c5cf09fb4988a2 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,29 @@ 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, + 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; + AP_Mission::Mission_Command cmd_loiter; + AP_Mission::Mission_Command cmd_land; + Location land_start; + AutoLandStage stage; + void set_autoland_direction(const float heading); }; #endif #if HAL_SOARING_ENABLED diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp index e9ec9e8e964425..4727cc585015af 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,81 +52,100 @@ 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; + + // In flight stage normal for approach + plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); + + const Location &home = ahrs.get_home(); + +#ifndef HAL_LANDING_DEEPSTALL_ENABLED + if (plane.landing.get_type() == AP_Landing::TYPE_DEEPSTALL) { + // Deep stall landings require only a landing location, they do there own loiter to alt and approach + cmd_land.id = MAV_CMD_NAV_LAND; + cmd_land.content.location = home; + + // p1 gives the altitude from which to start the deep stall above the location alt + cmd_land.p1 = final_wp_alt; + plane.start_command(cmd_land); + + stage = AutoLandStage::LANDING; + return true; + } +#endif // HAL_LANDING_DEEPSTALL_ENABLED /* - landing is in 3 steps: - 1) a base leg waypoint - 2) a land start WP, with crosstrack - 3) the landing WP, with crosstrack + Glide slope landing is in 3 steps: + 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 */ - const Location &home = ahrs.get_home(); /* first calculate the starting waypoint we will use when doing the 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.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); + land_start = home; + land_start.offset_bearing(plane.takeoff_state.initial_direction.heading, -final_wp_dist); + land_start.set_alt_m(final_wp_alt, Location::AltFrame::ABOVE_HOME); + land_start.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; - 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; - - /* - create the WP for the start of the landing - */ - cmd[1].content.location = land_start_WP; - cmd[1].id = MAV_CMD_NAV_WAYPOINT; - - // and the land WP - cmd[2].id = MAV_CMD_NAV_LAND; - cmd[2].content.location = home; - - // start first leg - stage = 0; - plane.start_command(cmd[0]); - - // don't crosstrack initially - plane.auto_state.crosstrack = false; - plane.auto_state.next_wp_crosstrack = true; - + const float bearing_to_current_deg = degrees(land_start.get_bearing(plane.current_loc)); + const float bearing_err_deg = wrap_180(plane.takeoff_state.initial_direction.heading - bearing_to_current_deg); + const float bearing_offset_deg = (bearing_err_deg > 0) ? -90 : 90; + + // Try and minimize loiter radius by using the smaller of the waypoint loiter radius or 1/3 of the final WP distance + const float loiter_radius = MIN(final_wp_dist * 0.333, fabsf(plane.aparm.loiter_radius)); + + // corrected_loiter_radius is the radius the vehicle will actually fly, this gets larger as altitude increases. + // Strictly this gets the loiter radius at the current altitude, really we want the loiter radius at final_wp_alt. + const float corrected_loiter_radius = plane.nav_controller->loiter_radius(loiter_radius); + + cmd_loiter.id = MAV_CMD_NAV_LOITER_TO_ALT; + cmd_loiter.p1 = loiter_radius; + cmd_loiter.content.location = land_start; + cmd_loiter.content.location.offset_bearing(plane.takeoff_state.initial_direction.heading + bearing_offset_deg, corrected_loiter_radius); + cmd_loiter.content.location.loiter_ccw = bearing_err_deg>0? 1 :0; + + // land WP at home + cmd_land.id = MAV_CMD_NAV_LAND; + cmd_land.content.location = home; + + // start first leg toward the base leg loiter to alt point + stage = AutoLandStage::LOITER; + plane.start_command(cmd_loiter); return true; } @@ -129,30 +155,87 @@ 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_loiter)) { + stage = AutoLandStage::LANDING; + plane.start_command(cmd_land); + // Crosstrack from the land start location + plane.prev_WP_loc = land_start; + + } + break; + + case AutoLandStage::LANDING: plane.set_flight_stage(AP_FixedWing::FlightStage::LAND); - plane.verify_command(cmd[stage]); + plane.verify_command(cmd_land); + // 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(plane.gps.ground_course() + landing_dir_off); } } -#endif + +// Sets autoland direction using ground course + offest parameter +void ModeAutoLand::set_autoland_direction(const float heading) +{ + plane.takeoff_state.initial_direction.heading = wrap_360(heading); + 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(cmd_loiter.content.location, cmd_land.content.location); +} + +// 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)) { + set_autoland_direction(plane.ahrs.yaw_sensor * 0.01); + } +} + +#endif // MODE_AUTOLAND_ENABLED + diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 8f18f6d1b2eeb8..9eebcd1c119ade 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.