From a57e6455abb903a4ecee95f9fa7d2fa66c89e049 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 11 Feb 2023 13:36:33 +1100 Subject: [PATCH] Plane: use is_armed_and_safety_off() this no longer sets soft_armed false when safety is on, and instead uses is_armed_and_safety_off() when appropriate --- ArduPlane/AP_Arming.cpp | 3 +-- ArduPlane/Attitude.cpp | 6 +++--- ArduPlane/commands_logic.cpp | 4 ++-- ArduPlane/failsafe.cpp | 2 +- ArduPlane/is_flying.cpp | 4 ++-- ArduPlane/quadplane.cpp | 18 +++++++++--------- ArduPlane/servos.cpp | 8 ++++---- ArduPlane/tailsitter.cpp | 12 ++++++------ ArduPlane/takeoff.cpp | 2 +- ArduPlane/tiltrotor.cpp | 10 +++++----- 10 files changed, 34 insertions(+), 35 deletions(-) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index d228981224..46e5262087 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -271,7 +271,7 @@ void AP_Arming_Plane::change_arm_state(void) { update_soft_armed(); #if HAL_QUADPLANE_ENABLED - plane.quadplane.set_armed(hal.util->get_soft_armed()); + plane.quadplane.set_armed(is_armed_and_safety_off()); #endif } @@ -372,7 +372,6 @@ void AP_Arming_Plane::update_soft_armed() _armed = true; } #endif - _armed = _armed && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED; hal.util->set_soft_armed(_armed); AP::logger().set_vehicle_armed(hal.util->get_soft_armed()); diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index a8b3ff62d8..54580f8cab 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -9,7 +9,7 @@ float Plane::calc_speed_scaler(void) { float aspeed, speed_scaler; if (ahrs.airspeed_estimate(aspeed)) { - if (aspeed > auto_state.highest_airspeed && hal.util->get_soft_armed()) { + if (aspeed > auto_state.highest_airspeed && arming.is_armed_and_safety_off()) { auto_state.highest_airspeed = aspeed; } // ensure we have scaling over the full configured airspeed @@ -24,7 +24,7 @@ float Plane::calc_speed_scaler(void) speed_scaler = constrain_float(speed_scaler, scale_min, scale_max); #if HAL_QUADPLANE_ENABLED - if (quadplane.in_vtol_mode() && hal.util->get_soft_armed()) { + if (quadplane.in_vtol_mode() && arming.is_armed_and_safety_off()) { // when in VTOL modes limit surface movement at low speed to prevent instability float threshold = airspeed_min * 0.5; if (aspeed < threshold) { @@ -39,7 +39,7 @@ float Plane::calc_speed_scaler(void) } } #endif - } else if (hal.util->get_soft_armed()) { + } else if (arming.is_armed_and_safety_off()) { // scale assumed surface movement using throttle output float throttle_out = MAX(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 1); speed_scaler = sqrtf(THROTTLE_CRUISE / throttle_out); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index cfde46e4ad..3c95fb6646 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -579,7 +579,7 @@ bool Plane::verify_takeoff() if (takeoff_state.start_time_ms != 0 && g2.takeoff_timeout > 0) { const float ground_speed = gps.ground_speed(); const float takeoff_min_ground_speed = 4; - if (!hal.util->get_soft_armed()) { + if (!arming.is_armed_and_safety_off()) { return false; } if (ground_speed >= takeoff_min_ground_speed) { @@ -872,7 +872,7 @@ bool Plane::verify_altitude_wait(const AP_Mission::Mission_Command &cmd) // verify_nav_delay - check if we have waited long enough bool Plane::verify_nav_delay(const AP_Mission::Mission_Command& cmd) { - if (hal.util->get_soft_armed()) { + if (arming.is_armed_and_safety_off()) { // don't delay while armed, we need a nav controller running return true; } diff --git a/ArduPlane/failsafe.cpp b/ArduPlane/failsafe.cpp index 6216126100..154a8c2a00 100644 --- a/ArduPlane/failsafe.cpp +++ b/ArduPlane/failsafe.cpp @@ -68,7 +68,7 @@ void Plane::failsafe_check(void) float throttle = get_throttle_input(true); float rudder = rudder_in_expo(false); - if (!hal.util->get_soft_armed()) { + if (!arming.is_armed_and_safety_off()) { throttle = 0; } diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 904a0c1855..3b2c27396f 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -176,7 +176,7 @@ void Plane::update_is_flying_5Hz(void) set_likely_flying(new_is_flying); // conservative ground mode value for rate D suppression - ground_mode = !is_flying() && !hal.util->get_soft_armed(); + ground_mode = !is_flying() && !arming.is_armed_and_safety_off(); } /* @@ -186,7 +186,7 @@ void Plane::update_is_flying_5Hz(void) */ bool Plane::is_flying(void) { - if (hal.util->get_soft_armed()) { + if (arming.is_armed_and_safety_off()) { #if HAL_QUADPLANE_ENABLED if (quadplane.is_flying_vtol()) { return true; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 749779f59e..67d50f8984 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1410,7 +1410,7 @@ float QuadPlane::desired_auto_yaw_rate_cds(void) const */ bool QuadPlane::should_assist(float aspeed, bool have_airspeed) { - if (!hal.util->get_soft_armed() || (q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_DISABLED) || tailsitter.is_control_surface_tailsitter()) { + if (!plane.arming.is_armed_and_safety_off() || (q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_DISABLED) || tailsitter.is_control_surface_tailsitter()) { // disarmed or disabled by aux switch or because a control surface tailsitter in_angle_assist = false; angle_error_start_ms = 0; @@ -1524,7 +1524,7 @@ void SLT_Transition::update() { const uint32_t now = millis(); - if (!hal.util->get_soft_armed()) { + if (!plane.arming.is_armed_and_safety_off()) { // reset the failure timer if we are disarmed transition_start_ms = now; } @@ -1780,7 +1780,7 @@ void QuadPlane::update(void) attitude_control->reset_rate_controller_I_terms(); } - if (!hal.util->get_soft_armed()) { + if (!plane.arming.is_armed_and_safety_off()) { /* make sure we don't have any residual control from previous flight stages */ @@ -1991,11 +1991,11 @@ void QuadPlane::motors_output(bool run_rate_controller) } #if AP_ADVANCEDFAILSAFE_ENABLED - if (!hal.util->get_soft_armed() || + if (!plane.arming.is_armed_and_safety_off() || (plane.afs.should_crash_vehicle() && !plane.afs.terminating_vehicle_via_landing()) || SRV_Channels::get_emergency_stop()) { #else - if (!hal.util->get_soft_armed() || SRV_Channels::get_emergency_stop()) { + if (!plane.arming.is_armed_and_safety_off() || SRV_Channels::get_emergency_stop()) { #endif set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); motors->output(); @@ -2337,7 +2337,7 @@ void QuadPlane::vtol_position_controller(void) // target speed when we reach position2 threshold const float position2_target_speed = 3.0; - if (hal.util->get_soft_armed()) { + if (plane.arming.is_armed_and_safety_off()) { poscontrol.last_run_ms = now_ms; } @@ -2926,7 +2926,7 @@ void QuadPlane::setup_target_position(void) */ void QuadPlane::takeoff_controller(void) { - if (!hal.util->get_soft_armed()) { + if (!plane.arming.is_armed_and_safety_off()) { return; } @@ -3239,7 +3239,7 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd) const uint32_t now = millis(); // reset takeoff if we aren't armed - if (!hal.util->get_soft_armed()) { + if (!plane.arming.is_armed_and_safety_off()) { do_vtol_takeoff(cmd); return false; } @@ -3748,7 +3748,7 @@ bool QuadPlane::do_user_takeoff(float takeoff_altitude) gcs().send_text(MAV_SEVERITY_INFO, "User Takeoff only in GUIDED mode"); return false; } - if (!hal.util->get_soft_armed()) { + if (!plane.arming.is_armed_and_safety_off()) { gcs().send_text(MAV_SEVERITY_INFO, "Must be armed for takeoff"); return false; } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index db693cff17..11f50fabd8 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -541,7 +541,7 @@ void Plane::set_servos_controlled(void) SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), min_throttle, max_throttle)); - if (!hal.util->get_soft_armed()) { + if (!arming.is_armed_and_safety_off()) { if (arming.arming_required() == AP_Arming::Required::YES_ZERO_PWM) { SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::ZERO_PWM); SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::ZERO_PWM); @@ -697,7 +697,7 @@ void Plane::set_servos_flaps(void) */ void Plane::set_landing_gear(void) { - if (control_mode == &mode_auto && hal.util->get_soft_armed() && is_flying() && gear.last_flight_stage != flight_stage) { + if (control_mode == &mode_auto && arming.is_armed_and_safety_off() && is_flying() && gear.last_flight_stage != flight_stage) { switch (flight_stage) { case AP_FixedWing::FlightStage::LAND: g2.landing_gear.deploy_for_landing(); @@ -743,7 +743,7 @@ void Plane::servos_twin_engine_mix(void) throttle_left = constrain_float(throttle + 50 * rudder_dt, 0, 100); throttle_right = constrain_float(throttle - 50 * rudder_dt, 0, 100); } - if (!hal.util->get_soft_armed()) { + if (!arming.is_armed_and_safety_off()) { if (arming.arming_required() == AP_Arming::Required::YES_ZERO_PWM) { SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::ZERO_PWM); SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::ZERO_PWM); @@ -1033,7 +1033,7 @@ void Plane::servos_auto_trim(void) if (!control_mode->does_auto_throttle() && control_mode != &mode_fbwa) { return; } - if (!hal.util->get_soft_armed()) { + if (!arming.is_armed_and_safety_off()) { return; } if (!is_flying()) { diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 44f0aeb480..815aac3c65 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -296,7 +296,7 @@ void Tailsitter::output(void) // handle forward flight modes and transition to VTOL modes if (!active() || in_vtol_transition()) { // get FW controller throttle demand and mask of motors enabled during forward flight - if (hal.util->get_soft_armed() && in_vtol_transition() && !quadplane.throttle_wait) { + if (plane.arming.is_armed_and_safety_off() && in_vtol_transition() && !quadplane.throttle_wait) { /* during transitions to vtol mode set the throttle to hover thrust, center the rudder */ @@ -417,7 +417,7 @@ void Tailsitter::output(void) SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, (motors->get_pitch()+motors->get_pitch_ff())*SERVO_MAX*VTOL_pitch_scale); SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, (motors->get_roll()+motors->get_roll_ff())*SERVO_MAX*VTOL_roll_scale); - if (hal.util->get_soft_armed()) { + if (plane.arming.is_armed_and_safety_off()) { // scale surfaces for throttle speed_scaling(); } else if (tailsitter_motors != nullptr) { @@ -500,7 +500,7 @@ void Tailsitter::output(void) */ bool Tailsitter::transition_fw_complete(void) { - if (!hal.util->get_soft_armed()) { + if (!plane.arming.is_armed_and_safety_off()) { // instant trainsition when disarmed, no message return true; } @@ -526,7 +526,7 @@ bool Tailsitter::transition_fw_complete(void) */ bool Tailsitter::transition_vtol_complete(void) const { - if (!hal.util->get_soft_armed()) { + if (!plane.arming.is_armed_and_safety_off()) { // instant trainsition when disarmed, no message return true; } @@ -799,7 +799,7 @@ void Tailsitter_Transition::update() case TRANSITION_ANGLE_WAIT_FW: { if (tailsitter.transition_fw_complete()) { transition_state = TRANSITION_DONE; - if (hal.util->get_soft_armed()) { + if (plane.arming.is_armed_and_safety_off()) { fw_limit_start_ms = now; fw_limit_initial_pitch = constrain_float(quadplane.ahrs.pitch_sensor,-8500,8500); plane.nav_pitch_cd = fw_limit_initial_pitch; @@ -856,7 +856,7 @@ void Tailsitter_Transition::VTOL_update() return; } // transition to VTOL complete, if armed set vtol rate limit starting point - if (hal.util->get_soft_armed()) { + if (plane.arming.is_armed_and_safety_off()) { vtol_limit_start_ms = now; vtol_limit_initial_pitch = quadplane.ahrs_view->pitch_sensor; } diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index 0f0ee7f7df..66e4a7a0f2 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -16,7 +16,7 @@ bool Plane::auto_takeoff_check(void) uint16_t wait_time_ms = MIN(uint16_t(g.takeoff_throttle_delay)*100,12700); // reset all takeoff state if disarmed - if (!hal.util->get_soft_armed()) { + if (!arming.is_armed_and_safety_off()) { memset(&takeoff_state, 0, sizeof(takeoff_state)); auto_state.baro_takeoff_alt = barometer.get_altitude(); return false; diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index d2bae63ce9..6e36d193e3 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -167,7 +167,7 @@ float Tiltrotor::tilt_max_change(bool up, bool in_flap_range) const if (plane.control_mode == &plane.mode_manual) { fast_tilt = true; } - if (hal.util->get_soft_armed() && !quadplane.in_vtol_mode() && !quadplane.assisted_flight) { + if (plane.arming.is_armed_and_safety_off() && !quadplane.in_vtol_mode() && !quadplane.assisted_flight) { fast_tilt = true; } if (fast_tilt) { @@ -217,12 +217,12 @@ void Tiltrotor::continuous_update(void) // the maximum rate of throttle change float max_change; - if (!quadplane.in_vtol_mode() && (!hal.util->get_soft_armed() || !quadplane.assisted_flight)) { + if (!quadplane.in_vtol_mode() && (!plane.arming.is_armed_and_safety_off() || !quadplane.assisted_flight)) { // we are in pure fixed wing mode. Move the tiltable motors all the way forward and run them as // a forward motor // option set then if disarmed move to VTOL position to prevent ground strikes, allow tilt forward in manual mode for testing - const bool disarmed_tilt_up = !hal.util->get_soft_armed() && (plane.control_mode != &plane.mode_manual) && quadplane.option_is_set(QuadPlane::OPTION::DISARMED_TILT_UP); + const bool disarmed_tilt_up = !plane.arming.is_armed_and_safety_off() && (plane.control_mode != &plane.mode_manual) && quadplane.option_is_set(QuadPlane::OPTION::DISARMED_TILT_UP); slew(disarmed_tilt_up ? 0.0 : get_forward_flight_tilt()); max_change = tilt_max_change(false); @@ -235,7 +235,7 @@ void Tiltrotor::continuous_update(void) } else { current_throttle = new_throttle; } - if (!hal.util->get_soft_armed()) { + if (!plane.arming.is_armed_and_safety_off()) { current_throttle = 0; } else { // prevent motor shutdown @@ -512,7 +512,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.option_is_set(QuadPlane::OPTION::DISARMED_TILT)) { + if (!plane.arming.is_armed_and_safety_off() && 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()) {