mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-06 12:41:36 +08:00
fix(commander): arm throttle check for all vehicle types (#26683)
Extend the arm throttle safety check to all vehicle types including rovers, which were previously excluded. Unify the two separate throttle checks into a single evaluation at arm-time that accounts for vehicle type and control mode: rovers require centered stick, climb-rate modes require stick at or below center, and manual/stab/acro modes require stick at bottom.
This commit is contained in:
@@ -608,27 +608,29 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
|
|||||||
if (run_preflight_checks) {
|
if (run_preflight_checks) {
|
||||||
if (_vehicle_control_mode.flag_control_manual_enabled) {
|
if (_vehicle_control_mode.flag_control_manual_enabled) {
|
||||||
|
|
||||||
if (_vehicle_control_mode.flag_control_climb_rate_enabled &&
|
if (!_failsafe_flags.manual_control_signal_lost) {
|
||||||
!_failsafe_flags.manual_control_signal_lost && _is_throttle_above_center) {
|
bool throttle_safe;
|
||||||
|
|
||||||
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center\t");
|
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) {
|
||||||
events::send(events::ID("commander_arm_denied_throttle_center"), {events::Log::Critical, events::LogInternal::Info},
|
// Rovers: center stick = stop, safe to arm near center
|
||||||
"Arming denied: throttle above center");
|
throttle_safe = (fabsf(_last_manual_throttle) < 0.2f);
|
||||||
tune_negative(true);
|
|
||||||
return TRANSITION_DENIED;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!_vehicle_control_mode.flag_control_climb_rate_enabled &&
|
} else if (_vehicle_control_mode.flag_control_climb_rate_enabled) {
|
||||||
!_failsafe_flags.manual_control_signal_lost && !_is_throttle_low
|
// Climb-rate modes (AltCtl, PosCtl, etc.): center = hover, safe at or below center
|
||||||
&& ((_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)
|
throttle_safe = (_last_manual_throttle <= 0.2f);
|
||||||
|| (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING))
|
|
||||||
) {
|
|
||||||
|
|
||||||
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t");
|
} else {
|
||||||
events::send(events::ID("commander_arm_denied_throttle_high"), {events::Log::Critical, events::LogInternal::Info},
|
// Manual/Stab/Acro on MC/FW: bottom = no thrust, safe at bottom
|
||||||
"Arming denied: high throttle");
|
throttle_safe = (_last_manual_throttle < -0.8f);
|
||||||
tune_negative(true);
|
}
|
||||||
return TRANSITION_DENIED;
|
|
||||||
|
if (!throttle_safe) {
|
||||||
|
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle not in safe position\t");
|
||||||
|
events::send(events::ID("commander_arm_denied_throttle_unsafe"), {events::Log::Critical, events::LogInternal::Info},
|
||||||
|
"Arming denied: throttle not in safe position");
|
||||||
|
tune_negative(true);
|
||||||
|
return TRANSITION_DENIED;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
} else if (calling_reason == arm_disarm_reason_t::stick_gesture
|
} else if (calling_reason == arm_disarm_reason_t::stick_gesture
|
||||||
@@ -3001,8 +3003,7 @@ void Commander::manualControlCheck()
|
|||||||
|
|
||||||
if (manual_control_updated && manual_control_setpoint.valid) {
|
if (manual_control_updated && manual_control_setpoint.valid) {
|
||||||
|
|
||||||
_is_throttle_above_center = (manual_control_setpoint.throttle > 0.2f);
|
_last_manual_throttle = manual_control_setpoint.throttle;
|
||||||
_is_throttle_low = (manual_control_setpoint.throttle < -0.8f);
|
|
||||||
|
|
||||||
if (isArmed()) {
|
if (isArmed()) {
|
||||||
// Abort autonomous mode and switch to position mode if sticks are moved significantly
|
// Abort autonomous mode and switch to position mode if sticks are moved significantly
|
||||||
|
|||||||
@@ -279,8 +279,7 @@ private:
|
|||||||
bool _last_overload{false};
|
bool _last_overload{false};
|
||||||
bool _mode_switch_mapped{false};
|
bool _mode_switch_mapped{false};
|
||||||
|
|
||||||
bool _is_throttle_above_center{false};
|
float _last_manual_throttle{-1.f};
|
||||||
bool _is_throttle_low{false};
|
|
||||||
|
|
||||||
bool _arm_tune_played{false};
|
bool _arm_tune_played{false};
|
||||||
bool _have_taken_off_since_arming{false};
|
bool _have_taken_off_since_arming{false};
|
||||||
|
|||||||
Reference in New Issue
Block a user