From 1b15b2a06539adc14991db53b46fd25ef2af85d6 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 --- ArduPlane/AP_Arming.cpp | 9 ++++ ArduPlane/Plane.cpp | 4 ++ ArduPlane/Plane.h | 5 ++ ArduPlane/commands_logic.cpp | 20 ++++---- ArduPlane/mode.h | 49 ++++++++++++++++++++ ArduPlane/mode_autoland.cpp | 89 +++++++++++++++++++++++++----------- ArduPlane/mode_takeoff.cpp | 13 ------ 7 files changed, 140 insertions(+), 49 deletions(-) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 25a43be25cab3..79cd19d93e60b 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/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; 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..3963293233610 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) { @@ -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); } diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 6e5d0b56327f0..2589645e84563 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? @@ -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[]; @@ -894,11 +930,24 @@ class ModeAutoLand: public Mode 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 diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp index e9ec9e8e96442..2de307f8be09b 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,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 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.