diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index e638ec956e..d6894ee298 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -173,7 +173,7 @@ bool AP_Arming_Plane::quadplane_checks(bool display_failure) ret = false; } - if ((plane.quadplane.options & QuadPlane::OPTION_ONLY_ARM_IN_QMODE_OR_AUTO) != 0) { + if (plane.quadplane.option_is_set(QuadPlane::OPTION::ONLY_ARM_IN_QMODE_OR_AUTO)) { if (!plane.control_mode->is_vtol_mode() && (plane.control_mode != &plane.mode_auto) && (plane.control_mode != &plane.mode_guided)) { check_failed(display_failure,"not in Q mode"); ret = false; diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index d50062ddfa..6d294b1404 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.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH))); + !plane.quadplane.option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH)); #else const bool attempt_go_around = true; #endif diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index cefe32be21..57f551ed8a 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.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH)) { + !quadplane.option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_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 aa5c20058a..4d8d5e40f9 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.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) { + if (quadplane.option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_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.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) && !verify_landing_vtol_approach(cmd)) { + if (quadplane.option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_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/events.cpp b/ArduPlane/events.cpp index 519464b443..479b0670d3 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -57,9 +57,9 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso case Mode::Number::QAUTOTUNE: #endif case Mode::Number::QACRO: - if (quadplane.options & QuadPlane::OPTION_FS_RTL) { + if (quadplane.option_is_set(QuadPlane::OPTION::FS_RTL)) { set_mode(mode_rtl, reason); - } else if (quadplane.options & QuadPlane::OPTION_FS_QRTL) { + } else if (quadplane.option_is_set(QuadPlane::OPTION::FS_QRTL)) { set_mode(mode_qrtl, reason); } else { set_mode(mode_qland, reason); @@ -152,9 +152,9 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason #if QAUTOTUNE_ENABLED case Mode::Number::QAUTOTUNE: #endif - if (quadplane.options & QuadPlane::OPTION_FS_RTL) { + if (quadplane.option_is_set(QuadPlane::OPTION::FS_RTL)) { set_mode(mode_rtl, reason); - } else if (quadplane.options & QuadPlane::OPTION_FS_QRTL) { + } else if (quadplane.option_is_set(QuadPlane::OPTION::FS_QRTL)) { set_mode(mode_qrtl, reason); } else { set_mode(mode_qland, reason); diff --git a/ArduPlane/mode_qloiter.cpp b/ArduPlane/mode_qloiter.cpp index 7f480ae337..e7fd0dcb00 100644 --- a/ArduPlane/mode_qloiter.cpp +++ b/ArduPlane/mode_qloiter.cpp @@ -99,7 +99,7 @@ void ModeQLoiter::run() float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing); float descent_rate_cms = quadplane.landing_descent_rate_cms(height_above_ground); - if (poscontrol.get_state() == QuadPlane::QPOS_LAND_FINAL && (quadplane.options & QuadPlane::OPTION_DISABLE_GROUND_EFFECT_COMP) == 0) { + if (poscontrol.get_state() == QuadPlane::QPOS_LAND_FINAL && !quadplane.option_is_set(QuadPlane::OPTION::DISABLE_GROUND_EFFECT_COMP)) { quadplane.ahrs.set_touchdown_expected(true); } diff --git a/ArduPlane/mode_qstabilize.cpp b/ArduPlane/mode_qstabilize.cpp index 8a5f9fef65..2c8da2d968 100644 --- a/ArduPlane/mode_qstabilize.cpp +++ b/ArduPlane/mode_qstabilize.cpp @@ -27,7 +27,7 @@ void ModeQStabilize::update() return; } - if ((plane.quadplane.options & QuadPlane::OPTION_INGORE_FW_ANGLE_LIMITS_IN_Q_MODES) == 0) { + if (!plane.quadplane.option_is_set(QuadPlane::OPTION::INGORE_FW_ANGLE_LIMITS_IN_Q_MODES)) { // by default angles are also constrained by forward flight limits set_limited_roll_pitch(roll_input, pitch_input); } else { diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 42010eab8c..e8c2bb88ab 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.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) && + if (quadplane.option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_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 1b53b4e9ae..e5f8e80b5d 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -762,7 +762,7 @@ bool QuadPlane::setup(void) hal.rcout->set_failsafe_pwm(mask, plane.quadplane.motors->get_pwm_output_min()); // default QAssist state as set with Q_OPTIONS - if ((options & OPTION_Q_ASSIST_FORCE_ENABLE) != 0) { + if (option_is_set(QuadPlane::OPTION::Q_ASSIST_FORCE_ENABLE)) { q_assist_state = Q_ASSIST_STATE_ENUM::Q_ASSIST_FORCE; } @@ -1190,7 +1190,7 @@ float QuadPlane::landing_descent_rate_cms(float height_above_ground) height_above_ground, land_final_alt, land_final_alt+6); - if ((options & OPTION_THR_LANDING_CONTROL) != 0) { + if (option_is_set(QuadPlane::OPTION::THR_LANDING_CONTROL)) { // allow throttle control for landing speed const float thr_in = get_pilot_land_throttle(); if (thr_in > THR_CTRL_LAND_THRESH) { @@ -1409,7 +1409,7 @@ bool QuadPlane::should_assist(float aspeed, bool have_airspeed) // 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())) { + (!option_is_set(QuadPlane::OPTION::DISABLE_SYNTHETIC_AIRSPEED_ASSIST) || ahrs.airspeed_sensor_enabled())) { in_angle_assist = false; angle_error_start_ms = 0; return true; @@ -1568,7 +1568,7 @@ void SLT_Transition::update() // if option is set and ground speed> 1/2 arspd_fbw_min for non-tiltrotors, then complete transition, otherwise QLAND. // tiltrotors will immediately transition const bool tiltrotor_with_ground_speed = quadplane.tiltrotor.enabled() && (plane.ahrs.groundspeed() > plane.aparm.airspeed_min * 0.5); - if ((quadplane.options & QuadPlane::OPTION_TRANS_FAIL_TO_FW) && tiltrotor_with_ground_speed) { + if (quadplane.option_is_set(QuadPlane::OPTION::TRANS_FAIL_TO_FW) && tiltrotor_with_ground_speed) { transition_state = TRANSITION_TIMER; in_forced_transition = true; } else { @@ -1604,7 +1604,7 @@ void SLT_Transition::update() // otherwise the plane can end up in high-alpha flight with // low VTOL thrust and may not complete a transition float climb_rate_cms = quadplane.assist_climb_rate_cms(); - if ((quadplane.options & QuadPlane::OPTION_LEVEL_TRANSITION) && !quadplane.tiltrotor.enabled()) { + if (quadplane.option_is_set(QuadPlane::OPTION::LEVEL_TRANSITION) && !quadplane.tiltrotor.enabled()) { climb_rate_cms = MIN(climb_rate_cms, 0.0f); } quadplane.hold_hover(climb_rate_cms); @@ -1957,7 +1957,7 @@ void QuadPlane::motors_output(bool run_rate_controller) 1) for safety (OPTION_DELAY_ARMING) 2) to allow motors to return to vertical (OPTION_DISARMED_TILT) */ - if ((options & OPTION_DISARMED_TILT) || (options & OPTION_DELAY_ARMING)) { + if (option_is_set(QuadPlane::OPTION::DISARMED_TILT) || option_is_set(QuadPlane::OPTION::DELAY_ARMING)) { if (plane.arming.get_delay_arming()) { // delay motor start after arming set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); @@ -2143,7 +2143,7 @@ bool QuadPlane::in_vtol_posvel_mode(void) const */ void QuadPlane::update_land_positioning(void) { - if ((options & OPTION_REPOSITION_LANDING) == 0) { + if (!option_is_set(QuadPlane::OPTION::REPOSITION_LANDING)) { // not enabled poscontrol.pilot_correction_active = false; poscontrol.target_vel_cms.zero(); @@ -2195,7 +2195,7 @@ void QuadPlane::run_xy_controller(float accel_limit) void QuadPlane::poscontrol_init_approach(void) { const float dist = plane.current_loc.get_distance(plane.next_WP_loc); - if ((options & OPTION_DISABLE_APPROACH) != 0) { + if (option_is_set(QuadPlane::OPTION::DISABLE_APPROACH)) { // go straight to QPOS_POSITION1 poscontrol.set_state(QPOS_POSITION1); gcs().send_text(MAV_SEVERITY_INFO,"VTOL Position1 d=%.1f", dist); @@ -2801,7 +2801,7 @@ void QuadPlane::vtol_position_controller(void) case QPOS_LAND_FINAL: { float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing); if (poscontrol.get_state() == QPOS_LAND_FINAL) { - if ((options & OPTION_DISABLE_GROUND_EFFECT_COMP) == 0) { + if (!option_is_set(QuadPlane::OPTION::DISABLE_GROUND_EFFECT_COMP)) { ahrs.set_touchdown_expected(true); } } @@ -3070,7 +3070,7 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd) loc.lat = 0; loc.lng = 0; plane.set_next_WP(loc); - if (options & OPTION_RESPECT_TAKEOFF_FRAME) { + if (option_is_set(QuadPlane::OPTION::RESPECT_TAKEOFF_FRAME)) { if (plane.current_loc.alt >= plane.next_WP_loc.alt) { // we are above the takeoff already, no need to do anything return false; @@ -3164,7 +3164,7 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd) } if (now - takeoff_start_time_ms < 3000 && - (options & OPTION_DISABLE_GROUND_EFFECT_COMP) == 0) { + !option_is_set(QuadPlane::OPTION::DISABLE_GROUND_EFFECT_COMP)) { ahrs.set_takeoff_expected(true); } @@ -3657,7 +3657,7 @@ bool QuadPlane::do_user_takeoff(float takeoff_altitude) guided_start(); guided_takeoff = true; guided_wait_takeoff = false; - if ((options & OPTION_DISABLE_GROUND_EFFECT_COMP) == 0) { + if (!option_is_set(QuadPlane::OPTION::DISABLE_GROUND_EFFECT_COMP)) { ahrs.set_takeoff_expected(true); } return true; @@ -3691,7 +3691,7 @@ bool QuadPlane::is_vtol_takeoff(uint16_t id) const if (id == MAV_CMD_NAV_VTOL_TAKEOFF) { return true; } - if (id == MAV_CMD_NAV_TAKEOFF && available() && (options & OPTION_ALLOW_FW_TAKEOFF) == 0) { + if (id == MAV_CMD_NAV_TAKEOFF && available() && !option_is_set(QuadPlane::OPTION::ALLOW_FW_TAKEOFF)) { // treat fixed wing takeoff as VTOL takeoff return true; } @@ -3704,13 +3704,13 @@ 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 (options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) { + if (option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH)) { return plane.vtol_approach_s.approach_stage == Plane::Landing_ApproachStage::VTOL_LANDING; } else { return true; } } - if (id == MAV_CMD_NAV_LAND && available() && (options & OPTION_ALLOW_FW_LAND) == 0) { + if (id == MAV_CMD_NAV_LAND && available() && !option_is_set(QuadPlane::OPTION::ALLOW_FW_LAND)) { // treat fixed wing land as VTOL land return true; } @@ -4075,7 +4075,7 @@ QuadPlane *QuadPlane::_singleton = nullptr; bool SLT_Transition::set_FW_roll_limit(int32_t& roll_limit_cd) { if (quadplane.assisted_flight && (transition_state == TRANSITION_AIRSPEED_WAIT || transition_state == TRANSITION_TIMER) && - (quadplane.options & QuadPlane::OPTION_LEVEL_TRANSITION)) { + quadplane.option_is_set(QuadPlane::OPTION::LEVEL_TRANSITION)) { // the user wants transitions to be kept level to within LEVEL_ROLL_LIMIT roll_limit_cd = MIN(roll_limit_cd, plane.g.level_roll_limit*100); return true; @@ -4290,7 +4290,7 @@ bool QuadPlane::allow_servo_auto_trim() // In forward flight and VTOL motors not active return true; } - if (tailsitter.enabled() && ((options & QuadPlane::OPTION_TAILSIT_Q_ASSIST_MOTORS_ONLY) != 0)) { + if (tailsitter.enabled() && option_is_set(QuadPlane::OPTION::TAILSIT_Q_ASSIST_MOTORS_ONLY)) { // Tailsitter in forward flight, motors providing active stabalisation with motors only option // Control surfaces are running as normal with I term active, motor I term is zeroed return true; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index ec1591a158..0295e42a8f 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -535,29 +535,32 @@ private: // additional options AP_Int32 options; - enum { - OPTION_LEVEL_TRANSITION=(1<<0), - OPTION_ALLOW_FW_TAKEOFF=(1<<1), - OPTION_ALLOW_FW_LAND=(1<<2), - OPTION_RESPECT_TAKEOFF_FRAME=(1<<3), - OPTION_MISSION_LAND_FW_APPROACH=(1<<4), - OPTION_FS_QRTL=(1<<5), - OPTION_IDLE_GOV_MANUAL=(1<<6), - OPTION_Q_ASSIST_FORCE_ENABLE=(1<<7), - OPTION_TAILSIT_Q_ASSIST_MOTORS_ONLY=(1<<8), - OPTION_AIRMODE_UNUSED=(1<<9), - OPTION_DISARMED_TILT=(1<<10), - OPTION_DELAY_ARMING=(1<<11), - OPTION_DISABLE_SYNTHETIC_AIRSPEED_ASSIST=(1<<12), - OPTION_DISABLE_GROUND_EFFECT_COMP=(1<<13), - OPTION_INGORE_FW_ANGLE_LIMITS_IN_Q_MODES=(1<<14), - OPTION_THR_LANDING_CONTROL=(1<<15), - 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), - OPTION_FS_RTL=(1<<20), + enum class OPTION { + LEVEL_TRANSITION=(1<<0), + ALLOW_FW_TAKEOFF=(1<<1), + ALLOW_FW_LAND=(1<<2), + RESPECT_TAKEOFF_FRAME=(1<<3), + MISSION_LAND_FW_APPROACH=(1<<4), + FS_QRTL=(1<<5), + IDLE_GOV_MANUAL=(1<<6), + Q_ASSIST_FORCE_ENABLE=(1<<7), + TAILSIT_Q_ASSIST_MOTORS_ONLY=(1<<8), + AIRMODE_UNUSED=(1<<9), + DISARMED_TILT=(1<<10), + DELAY_ARMING=(1<<11), + DISABLE_SYNTHETIC_AIRSPEED_ASSIST=(1<<12), + DISABLE_GROUND_EFFECT_COMP=(1<<13), + INGORE_FW_ANGLE_LIMITS_IN_Q_MODES=(1<<14), + THR_LANDING_CONTROL=(1<<15), + DISABLE_APPROACH=(1<<16), + REPOSITION_LANDING=(1<<17), + ONLY_ARM_IN_QMODE_OR_AUTO=(1<<18), + TRANS_FAIL_TO_FW=(1<<19), + FS_RTL=(1<<20), }; + bool option_is_set(OPTION option) const { + return (options.get() & int32_t(option)) != 0; + } AP_Float takeoff_failure_scalar; AP_Float maximum_takeoff_airspeed; diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index dbaa207fc8..03fc39115c 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -388,7 +388,7 @@ void Plane::set_servos_manual_passthrough(void) SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); #if HAL_QUADPLANE_ENABLED - if (quadplane.available() && (quadplane.options & QuadPlane::OPTION_IDLE_GOV_MANUAL)) { + if (quadplane.available() && quadplane.option_is_set(QuadPlane::OPTION::IDLE_GOV_MANUAL)) { // for quadplanes it can be useful to run the idle governor in MANUAL mode // as it prevents the VTOL motors from running int8_t min_throttle = aparm.throttle_min.get(); diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index eac7fb238f..91b96b7455 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -36,7 +36,7 @@ void Plane::init_ardupilot() // initialise rc channels including setting mode #if HAL_QUADPLANE_ENABLED - rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, (quadplane.enabled() && (quadplane.options & QuadPlane::OPTION_AIRMODE_UNUSED) && (rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRMODE) == nullptr)) ? RC_Channel::AUX_FUNC::ARMDISARM_AIRMODE : RC_Channel::AUX_FUNC::ARMDISARM); + rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, (quadplane.enabled() && quadplane.option_is_set(QuadPlane::OPTION::AIRMODE_UNUSED) && (rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRMODE) == nullptr)) ? RC_Channel::AUX_FUNC::ARMDISARM_AIRMODE : RC_Channel::AUX_FUNC::ARMDISARM); #else rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, RC_Channel::AUX_FUNC::ARMDISARM); #endif diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 62c7c9cbdc..2a099c4f68 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -236,7 +236,7 @@ void Tailsitter::setup() // Do not allow arming in forward flight modes // motors will become active due to assisted flight airmode, the vehicle will try very hard to get level - quadplane.options.set(quadplane.options.get() | QuadPlane::OPTION_ONLY_ARM_IN_QMODE_OR_AUTO); + quadplane.options.set(quadplane.options.get() | int32_t(QuadPlane::OPTION::ONLY_ARM_IN_QMODE_OR_AUTO)); } transition = new Tailsitter_Transition(quadplane, motors, *this); @@ -361,7 +361,7 @@ void Tailsitter::output(void) quadplane.hold_stabilize(throttle); quadplane.motors_output(true); - if ((quadplane.options & QuadPlane::OPTION_TAILSIT_Q_ASSIST_MOTORS_ONLY) != 0) { + if (quadplane.option_is_set(QuadPlane::OPTION::TAILSIT_Q_ASSIST_MOTORS_ONLY)) { // only use motors for Q assist, control surfaces remain under plane control // zero copter I terms and use plane quadplane.attitude_control->reset_rate_controller_I_terms(); diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index d9986fb491..bbd1e56cc4 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -508,7 +508,7 @@ void Tiltrotor::vectoring(void) // Wait TILT_DELAY_MS after disarming to allow props to spin down first. constexpr uint32_t TILT_DELAY_MS = 3000; uint32_t now = AP_HAL::millis(); - if (!hal.util->get_soft_armed() && (plane.quadplane.options & QuadPlane::OPTION_DISARMED_TILT)) { + if (!hal.util->get_soft_armed() && plane.quadplane.option_is_set(QuadPlane::OPTION::DISARMED_TILT)) { // this test is subject to wrapping at ~49 days, but the consequences are insignificant if ((now - hal.util->get_last_armed_change()) > TILT_DELAY_MS) { if (quadplane.in_vtol_mode()) {