Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Andrew Tridgell 2023-02-11 13:36:33 +11:00
parent bddd716cf9
commit a57e6455ab
10 changed files with 34 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/AP_Arming.cpp
View File

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

6
ArduPlane/Attitude.cpp
View File

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

4
ArduPlane/commands_logic.cpp
View File

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

2
ArduPlane/failsafe.cpp
View File

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

4
ArduPlane/is_flying.cpp
View File

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

18
ArduPlane/quadplane.cpp
View File

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

8
ArduPlane/servos.cpp
View File

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

12
ArduPlane/tailsitter.cpp
View File

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

2
ArduPlane/takeoff.cpp
View File

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

10
ArduPlane/tiltrotor.cpp
View File

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