From 0201692eebc1e56636c0f7046ee888fa00282103 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 4 Oct 2020 23:22:50 +0100 Subject: [PATCH] Plane: Q_OPTION to disable synthetic airspeed assist --- ArduPlane/quadplane.cpp | 8 +++++--- ArduPlane/quadplane.h | 1 + 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index c26ac15fe9..eddc98b90b 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -329,7 +329,7 @@ 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. - // @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,9:Airmode_On_Arm,10:Disarmed Yaw Tilt,11:Delay Spoolup + // @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,9:Airmode_On_Arm,10:Disarmed Yaw Tilt,11:Delay Spoolup,12:disable Qassist based on synthetic airspeed AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0), AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2), @@ -1479,8 +1479,10 @@ bool QuadPlane::assistance_needed(float aspeed, bool have_airspeed) return false; } - if (have_airspeed && aspeed < assist_speed) { - // assistance due to Q_ASSIST_SPEED + // assistance due to Q_ASSIST_SPEED + // if option bit is enabled only allow assist with real airspeed sensor + if ((have_airspeed && aspeed < assist_speed) && + (((options & OPTION_DISABLE_SYNTHETIC_AIRSPEED_ASSIST) == 0) || ahrs.airspeed_sensor_enabled())) { in_angle_assist = false; angle_error_start_ms = 0; return true; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 713328ca31..68e6a9f457 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -568,6 +568,7 @@ private: OPTION_AIRMODE=(1<<9), OPTION_DISARMED_TILT=(1<<10), OPTION_DELAY_ARMING=(1<<11), + OPTION_DISABLE_SYNTHETIC_AIRSPEED_ASSIST=(1<<12), }; AP_Float takeoff_failure_scalar;