From f183b21fc5bb1f3a39226060420f3f0454e5935b Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Thu, 30 Jun 2022 14:43:06 -0700 Subject: [PATCH] Plane: Support vtol landing options on NAV_VTOL_LAND This allows the same mission to contain both circular and straight landing items, and doesn't require on the fly tweaking --- ArduPlane/GCS_Mavlink.cpp | 2 +- ArduPlane/altitude.cpp | 2 +- ArduPlane/commands_logic.cpp | 4 ++-- ArduPlane/navigation.cpp | 2 +- ArduPlane/quadplane.cpp | 15 ++++++++++++--- ArduPlane/quadplane.h | 5 ++++- 6 files changed, 21 insertions(+), 9 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 6d294b1404..547f9ba9b2 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -982,7 +982,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l const bool attempt_go_around = (!plane.quadplane.available()) || ((!plane.quadplane.in_vtol_auto()) && - !plane.quadplane.option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH)); + (!plane.quadplane.landing_with_fixed_wing_spiral_approach())); #else const bool attempt_go_around = true; #endif diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 57f551ed8a..ad7c3fafc7 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -180,7 +180,7 @@ float Plane::relative_ground_altitude(bool use_rangefinder_if_available) #if HAL_QUADPLANE_ENABLED if (quadplane.in_vtol_land_descent() && - !quadplane.option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH)) { + !quadplane.landing_with_fixed_wing_spiral_approach()) { // when doing a VTOL landing we can use the waypoint height as // ground height. We can't do this if using the // LAND_FW_APPROACH as that uses the wp height as the approach diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 4d8d5e40f9..4465ca392b 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -98,7 +98,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) return quadplane.do_vtol_takeoff(cmd); case MAV_CMD_NAV_VTOL_LAND: - if (quadplane.option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH)) { + if (quadplane.landing_with_fixed_wing_spiral_approach()) { // the user wants to approach the landing in a fixed wing flight mode // the waypoint will be used as a loiter_to_alt // after which point the plane will compute the optimal into the wind direction @@ -286,7 +286,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret case MAV_CMD_NAV_VTOL_TAKEOFF: return quadplane.verify_vtol_takeoff(cmd); case MAV_CMD_NAV_VTOL_LAND: - if (quadplane.option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH) && !verify_landing_vtol_approach(cmd)) { + if (quadplane.landing_with_fixed_wing_spiral_approach() && !verify_landing_vtol_approach(cmd)) { // verify_landing_vtol_approach will return true once we have completed the approach, // in which case we fall over to normal vtol landing code return false; diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index e8c2bb88ab..0b1f7cf906 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -113,7 +113,7 @@ void Plane::navigate() float Plane::mode_auto_target_airspeed_cm() { #if HAL_QUADPLANE_ENABLED - if (quadplane.option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH) && + if (quadplane.landing_with_fixed_wing_spiral_approach() && ((vtol_approach_s.approach_stage == Landing_ApproachStage::APPROACH_LINE) || (vtol_approach_s.approach_stage == Landing_ApproachStage::VTOL_LANDING))) { const float land_airspeed = TECS_controller.get_land_airspeed(); diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index e5f8e80b5d..8f6088717c 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -257,8 +257,8 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Param: OPTIONS // @DisplayName: quadplane options - // @Description: Level Transition:Keep wings within LEVEL_ROLL_LIMIT and only use forward motor(s) for climb during transition, Allow FW Takeoff: If bit is not set then NAV_TAKEOFF command on quadplanes will instead perform a NAV_VTOL takeoff, Allow FW Land:If bit is not set then NAV_LAND command on quadplanes will instead perform a NAV_VTOL_LAND, Vtol Takeoff Frame: command NAV_VTOL_TAKEOFF altitude is as set by the command's reference frame rather than a delta above current location, Use FW Approach:Use a fixed wing approach for VTOL landings, USE QRTL:instead of QLAND for rc failsafe when in VTOL modes, Use Governor:Use ICE Idle Governor in MANUAL for forward motor, Force Qassist: on always,Mtrs_Only_Qassist: in tailsitters only, uses VTOL motors and not flying surfaces for QASSIST, Airmode_On_Arm:Airmode enabled when arming by aux switch, Disarmed Yaw Tilt:Enable motor tilt for yaw when disarmed, Delay Spoolup:Delay VTOL spoolup for 2 seconds after arming, ThrLandControl: enable throttle stick control of landing rate, DisableApproach: Disable use of approach and airbrake stages in VTOL landing, EnableLandResposition: enable pilot controlled repositioning in AUTO land. Descent will pause while repositioning. ARMVTOL: Arm only in VTOL or AUTO modes. CompleteTransition: to fixed wing if Q_TRANS_FAIL timer times out instead of QLAND. Force RTL mode: forces RTL mode on rc failsafe in VTOL modes overriding bit 5(USE_QRTL). - // @Bitmask: 0:Level Transition,1:Allow FW Takeoff,2:Allow FW Land,3:Vtol Takeoff Frame,4:Use FW Approach,5:Use QRTL,6:Use Governor,7:Force Qassist,8:Mtrs_Only_Qassist,10:Disarmed Yaw Tilt,11:Delay Spoolup,12:disable Qassist based on synthetic airspeed,13:Disable Ground Effect Compensation,14:Ignore forward flight angle limits in Qmodes,15:ThrLandControl,16:DisableApproach,17:EnableLandReposition,18:ARMVtol, 19: CompleteTransition if Q_TRANS_FAIL, 20: Force RTL mode on VTOL failsafes overriding bit 5(USE QRTL) + // @Description: Level Transition:Keep wings within LEVEL_ROLL_LIMIT and only use forward motor(s) for climb during transition, Allow FW Takeoff: If bit is not set then NAV_TAKEOFF command on quadplanes will instead perform a NAV_VTOL takeoff, Allow FW Land:If bit is not set then NAV_LAND command on quadplanes will instead perform a NAV_VTOL_LAND, Vtol Takeoff Frame: command NAV_VTOL_TAKEOFF altitude is as set by the command's reference frame rather than a delta above current location, Always use FW spiral approach:Always use Use a fixed wing spiral approach for VTOL landings, USE QRTL:instead of QLAND for rc failsafe when in VTOL modes, Use Governor:Use ICE Idle Governor in MANUAL for forward motor, Force Qassist: on always,Mtrs_Only_Qassist: in tailsitters only, uses VTOL motors and not flying surfaces for QASSIST, Airmode_On_Arm:Airmode enabled when arming by aux switch, Disarmed Yaw Tilt:Enable motor tilt for yaw when disarmed, Delay Spoolup:Delay VTOL spoolup for 2 seconds after arming, ThrLandControl: enable throttle stick control of landing rate, DisableApproach: Disable use of approach and airbrake stages in VTOL landing, EnableLandResposition: enable pilot controlled repositioning in AUTO land. Descent will pause while repositioning. ARMVTOL: Arm only in VTOL or AUTO modes. CompleteTransition: to fixed wing if Q_TRANS_FAIL timer times out instead of QLAND. Force RTL mode: forces RTL mode on rc failsafe in VTOL modes overriding bit 5(USE_QRTL). + // @Bitmask: 0:Level Transition,1:Allow FW Takeoff,2:Allow FW Land,3:Vtol Takeoff Frame,4:Always use FW spiral approach,5:Use QRTL,6:Use Governor,7:Force Qassist,8:Mtrs_Only_Qassist,10:Disarmed Yaw Tilt,11:Delay Spoolup,12:disable Qassist based on synthetic airspeed,13:Disable Ground Effect Compensation,14:Ignore forward flight angle limits in Qmodes,15:ThrLandControl,16:DisableApproach,17:EnableLandReposition,18:ARMVtol, 19: CompleteTransition if Q_TRANS_FAIL, 20: Force RTL mode on VTOL failsafes overriding bit 5(USE QRTL) AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0), AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2), @@ -3704,7 +3704,7 @@ bool QuadPlane::is_vtol_takeoff(uint16_t id) const bool QuadPlane::is_vtol_land(uint16_t id) const { if (id == MAV_CMD_NAV_VTOL_LAND) { - if (option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH)) { + if (landing_with_fixed_wing_spiral_approach()) { return plane.vtol_approach_s.approach_stage == Plane::Landing_ApproachStage::VTOL_LANDING; } else { return true; @@ -4299,4 +4299,13 @@ bool QuadPlane::allow_servo_auto_trim() return false; } +bool QuadPlane::landing_with_fixed_wing_spiral_approach(void) const +{ + const AP_Mission::Mission_Command cmd = plane.mission.get_current_nav_cmd(); + + return ((cmd.id == MAV_CMD_NAV_VTOL_LAND) && + (option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH) || + cmd.p1 == NAV_VTOL_LAND_OPTIONS_FW_SPIRAL_APPROACH)); +} + #endif // HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 0295e42a8f..72de5c0897 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -622,7 +622,10 @@ private: are we in the airbrake phase of a VTOL landing? */ bool in_vtol_airbrake(void) const; - + + // returns true if the vehicle should currently be doing a spiral landing + bool landing_with_fixed_wing_spiral_approach(void) const; + // Q assist state, can be enabled, disabled or force. Default to enabled Q_ASSIST_STATE_ENUM q_assist_state = Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED;