From 18e427d27deaa7c0a39f149dfae3306f60d9a94e Mon Sep 17 00:00:00 2001 From: Hwurzburg Date: Fri, 10 Dec 2021 15:45:28 -0600 Subject: [PATCH] Plane: add Q_OPTION to force fw trans on fail instead of QLAND --- ArduPlane/quadplane.cpp | 54 ++++++++++++++++++++++++++++------------- ArduPlane/quadplane.h | 1 + ArduPlane/transition.h | 2 ++ 3 files changed, 40 insertions(+), 17 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 9ae9773272..7230289ea3 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -268,8 +268,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, EnableLandResponsition: enable pilot controlled repositioning in AUTO land. Descent will pause while repositioning. - // @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:EnableLandResponsition,18:Only allow arming in Qmodes or auto + // @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. + // @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:EnableLandResponsition,18:ARMVtol, 19: CompleteTransition if Q_TRANS_FAIL AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0), AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2), @@ -327,7 +327,7 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @Param: TRANS_FAIL // @DisplayName: Quadplane transition failure time - // @Description: Maximum time allowed for forward transitions, exceeding this time will cancel the transition and the aircraft will immediately change to QLAND. 0 for no limit. + // @Description: Maximum time allowed for forward transitions, exceeding this time will cancel the transition and the aircraft will immediately change to QLAND or finish the transition depending on Q_OPTIONS bit 19. 0 for no limit. // @Units: s // @Range: 0 20 // @Increment: 1 @@ -1401,16 +1401,10 @@ bool QuadPlane::should_assist(float aspeed, bool have_airspeed) void SLT_Transition::update() { const uint32_t now = millis(); - + if (!hal.util->get_soft_armed()) { - // reset the failure timer if we haven't started transitioning + // reset the failure timer if we are disarmed transition_start_ms = now; - } else if ((transition_state != TRANSITION_DONE) && - (transition_start_ms != 0) && - (quadplane.transition_failure > 0) && - ((now - transition_start_ms) > ((uint32_t)quadplane.transition_failure * 1000))) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "Transition failed, exceeded time limit"); - plane.set_mode(plane.mode_qland, ModeReason::VTOL_FAILED_TRANSITION); } float aspeed; @@ -1423,12 +1417,14 @@ void SLT_Transition::update() // the quad should provide some assistance to the plane quadplane.assisted_flight = true; // update transition state for vehicles using airspeed wait - if (transition_state != TRANSITION_AIRSPEED_WAIT) { - gcs().send_text(MAV_SEVERITY_INFO, "Transition started airspeed %.1f", (double)aspeed); - } - transition_state = TRANSITION_AIRSPEED_WAIT; - if (transition_start_ms == 0) { - transition_start_ms = now; + if (!in_forced_transition) { + if (transition_state != TRANSITION_AIRSPEED_WAIT) { + gcs().send_text(MAV_SEVERITY_INFO, "Transition started airspeed %.1f", (double)aspeed); + } + transition_state = TRANSITION_AIRSPEED_WAIT; + if (transition_start_ms == 0) { + transition_start_ms = now; + } } } else { quadplane.assisted_flight = false; @@ -1474,6 +1470,26 @@ void SLT_Transition::update() transition_start_ms = now; } + // check if we have failed to transition while in TRANSITION_AIRSPEED_WAIT + if (transition_start_ms != 0 && + (quadplane.transition_failure > 0) && + ((now - transition_start_ms) > ((uint32_t)quadplane.transition_failure * 1000))) { + gcs().send_text(MAV_SEVERITY_CRITICAL, "Transition failed, exceeded time limit"); + // if option is set and ground speed> 1/2 arspd_fbw_min for non-tiltrotors, then complete transition, otherwise QLAND. + // tiltrotors will immediately transition + if (quadplane.options & QuadPlane::OPTION_TRANS_FAIL_TO_FW) { + if (!plane.quadplane.tiltrotor.enabled() && + plane.ahrs.groundspeed() < plane.aparm.airspeed_min * 0.5) { + plane.set_mode(plane.mode_qland, ModeReason::VTOL_FAILED_TRANSITION); + } else { + transition_state = TRANSITION_TIMER; + in_forced_transition = true; + } + } else { + plane.set_mode(plane.mode_qland, ModeReason::VTOL_FAILED_TRANSITION); + } + } + transition_low_airspeed_ms = now; if (have_airspeed && aspeed > plane.aparm.airspeed_min && !quadplane.assisted_flight) { transition_state = TRANSITION_TIMER; @@ -1529,6 +1545,7 @@ void SLT_Transition::update() const uint32_t transition_timer_ms = now - transition_low_airspeed_ms; if (transition_timer_ms > (unsigned)quadplane.transition_time_ms) { transition_state = TRANSITION_DONE; + in_forced_transition = false; transition_start_ms = 0; transition_low_airspeed_ms = 0; gcs().send_text(MAV_SEVERITY_INFO, "Transition done"); @@ -1578,6 +1595,7 @@ void SLT_Transition::update() } last_fw_mode_ms = now; last_fw_nav_pitch_cd = plane.nav_pitch_cd; + in_forced_transition = false; return; } @@ -1595,6 +1613,7 @@ void SLT_Transition::VTOL_update() transition_start_ms = 0; transition_low_airspeed_ms = 0; if (quadplane.throttle_wait && !plane.is_flying()) { + in_forced_transition = false; transition_state = TRANSITION_DONE; } else { /* @@ -3764,6 +3783,7 @@ bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_ void SLT_Transition::force_transistion_complete() { transition_state = TRANSITION_DONE; + in_forced_transition = false; transition_start_ms = 0; transition_low_airspeed_ms = 0; last_fw_mode_ms = AP_HAL::millis(); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 57ef9173a7..de58ae3766 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -505,6 +505,7 @@ private: OPTION_DISABLE_APPROACH=(1<<16), OPTION_REPOSITION_LANDING=(1<<17), OPTION_ONLY_ARM_IN_QMODE_OR_AUTO=(1<<18), + OPTION_TRANS_FAIL_TO_FW=(1<<19), }; AP_Float takeoff_failure_scalar; diff --git a/ArduPlane/transition.h b/ArduPlane/transition.h index 8b00fd6088..adc8106bf0 100644 --- a/ArduPlane/transition.h +++ b/ArduPlane/transition.h @@ -113,5 +113,7 @@ protected: // tiltrotor tilt angle when airspeed wait transition stage completes float airspeed_reached_tilt; + bool in_forced_transition; + };