Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 00:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: add Q_OPTION to force fw trans on fail instead of QLAND

Browse Source
This commit is contained in:
Hwurzburg 2021-12-10 15:45:28 -06:00 committed by Peter Barker
parent 939407a26f
commit 18e427d27d
3 changed files with 40 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
ArduPlane/quadplane.cpp
View File

@ -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();

1
ArduPlane/quadplane.h
View File

@ -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;

2
ArduPlane/transition.h
View File

@ -113,5 +113,7 @@ protected:
// tiltrotor tilt angle when airspeed wait transition stage completes
float airspeed_reached_tilt;
bool in_forced_transition;
};