mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Rename manual_lockdown to kill for clarity
This commit is contained in:
parent
32531c870e
commit
79b46e08a8
@ -4,6 +4,6 @@ bool armed # Set to true if system is armed
|
|||||||
bool prearmed # Set to true if the actuator safety is disabled but motors are not armed
|
bool prearmed # Set to true if the actuator safety is disabled but motors are not armed
|
||||||
bool ready_to_arm # Set to true if system is ready to be armed
|
bool ready_to_arm # Set to true if system is ready to be armed
|
||||||
bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL)
|
bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL)
|
||||||
bool manual_lockdown # Set to true if manual throttle kill switch is engaged
|
bool kill # Set to true if manual throttle kill switch is engaged
|
||||||
bool termination # Send out failsafe (by default same as disarmed) output
|
bool termination # Send out failsafe (by default same as disarmed) output
|
||||||
bool in_esc_calibration_mode # IO/FMU should ignore messages from the actuator controls topics
|
bool in_esc_calibration_mode # IO/FMU should ignore messages from the actuator controls topics
|
||||||
|
|||||||
@ -791,11 +791,11 @@ PX4IO::io_set_arming_state()
|
|||||||
clear |= PX4IO_P_SETUP_ARMING_FMU_PREARMED;
|
clear |= PX4IO_P_SETUP_ARMING_FMU_PREARMED;
|
||||||
}
|
}
|
||||||
|
|
||||||
if ((armed.lockdown || armed.manual_lockdown) && !_lockdown_override) {
|
if ((armed.lockdown || armed.kill) && !_lockdown_override) {
|
||||||
set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
|
set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
|
||||||
_lockdown_override = true;
|
_lockdown_override = true;
|
||||||
|
|
||||||
} else if (!(armed.lockdown || armed.manual_lockdown) && _lockdown_override) {
|
} else if (!(armed.lockdown || armed.kill) && _lockdown_override) {
|
||||||
clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
|
clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
|
||||||
_lockdown_override = false;
|
_lockdown_override = false;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -66,7 +66,7 @@ void UavcanArmingStatus::periodic_update(const uavcan::TimerEvent &)
|
|||||||
if (_actuator_armed_sub.update(&actuator_armed)) {
|
if (_actuator_armed_sub.update(&actuator_armed)) {
|
||||||
uavcan::equipment::safety::ArmingStatus cmd;
|
uavcan::equipment::safety::ArmingStatus cmd;
|
||||||
|
|
||||||
if (actuator_armed.lockdown || actuator_armed.manual_lockdown) {
|
if (actuator_armed.lockdown || actuator_armed.kill) {
|
||||||
cmd.status = cmd.STATUS_DISARMED;
|
cmd.status = cmd.STATUS_DISARMED;
|
||||||
|
|
||||||
} else if (actuator_armed.armed) {
|
} else if (actuator_armed.armed) {
|
||||||
|
|||||||
@ -458,7 +458,7 @@ bool MixingOutput::update()
|
|||||||
|
|
||||||
// Send output if any function mapped or one last disabling sample
|
// Send output if any function mapped or one last disabling sample
|
||||||
if (!all_disabled || !_was_all_disabled) {
|
if (!all_disabled || !_was_all_disabled) {
|
||||||
if (!_armed.armed && !_armed.manual_lockdown) {
|
if (!_armed.armed && !_armed.kill) {
|
||||||
_actuator_test.overrideValues(outputs, _max_num_outputs);
|
_actuator_test.overrideValues(outputs, _max_num_outputs);
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -473,7 +473,7 @@ bool MixingOutput::update()
|
|||||||
void
|
void
|
||||||
MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updates)
|
MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updates)
|
||||||
{
|
{
|
||||||
if (_armed.lockdown || _armed.manual_lockdown) {
|
if (_armed.lockdown || _armed.kill) {
|
||||||
// overwrite outputs in case of lockdown with disarmed values
|
// overwrite outputs in case of lockdown with disarmed values
|
||||||
for (size_t i = 0; i < _max_num_outputs; i++) {
|
for (size_t i = 0; i < _max_num_outputs; i++) {
|
||||||
_current_output_value[i] = _disarmed_value[i];
|
_current_output_value[i] = _disarmed_value[i];
|
||||||
|
|||||||
@ -148,13 +148,13 @@ public:
|
|||||||
_actuator_test_pub.publish(actuator_test);
|
_actuator_test_pub.publish(actuator_test);
|
||||||
}
|
}
|
||||||
|
|
||||||
void sendActuatorArmed(bool armed, bool termination = false, bool manual_lockdown = false, bool prearm = false)
|
void sendActuatorArmed(bool armed, bool termination = false, bool kill = false, bool prearm = false)
|
||||||
{
|
{
|
||||||
actuator_armed_s actuator_armed{};
|
actuator_armed_s actuator_armed{};
|
||||||
actuator_armed.timestamp = hrt_absolute_time();
|
actuator_armed.timestamp = hrt_absolute_time();
|
||||||
actuator_armed.armed = armed;
|
actuator_armed.armed = armed;
|
||||||
actuator_armed.termination = termination;
|
actuator_armed.termination = termination;
|
||||||
actuator_armed.manual_lockdown = manual_lockdown;
|
actuator_armed.kill = kill;
|
||||||
actuator_armed.prearmed = prearm;
|
actuator_armed.prearmed = prearm;
|
||||||
_actuator_armed_pub.publish(actuator_armed);
|
_actuator_armed_pub.publish(actuator_armed);
|
||||||
}
|
}
|
||||||
|
|||||||
@ -93,7 +93,7 @@ static constexpr bool operator ==(const actuator_armed_s &a, const actuator_arme
|
|||||||
a.prearmed == b.prearmed &&
|
a.prearmed == b.prearmed &&
|
||||||
a.ready_to_arm == b.ready_to_arm &&
|
a.ready_to_arm == b.ready_to_arm &&
|
||||||
a.lockdown == b.lockdown &&
|
a.lockdown == b.lockdown &&
|
||||||
a.manual_lockdown == b.manual_lockdown &&
|
a.kill == b.kill &&
|
||||||
a.termination == b.termination &&
|
a.termination == b.termination &&
|
||||||
a.in_esc_calibration_mode == b.in_esc_calibration_mode);
|
a.in_esc_calibration_mode == b.in_esc_calibration_mode);
|
||||||
}
|
}
|
||||||
@ -1662,17 +1662,17 @@ void Commander::executeActionRequest(const action_request_s &action_request)
|
|||||||
break;
|
break;
|
||||||
|
|
||||||
case action_request_s::ACTION_UNKILL:
|
case action_request_s::ACTION_UNKILL:
|
||||||
if (_actuator_armed.manual_lockdown) {
|
if (_actuator_armed.kill) {
|
||||||
mavlink_log_info(&_mavlink_log_pub, "Kill disengaged\t");
|
mavlink_log_info(&_mavlink_log_pub, "Kill disengaged\t");
|
||||||
events::send(events::ID("commander_kill_sw_disengaged"), events::Log::Info, "Kill disengaged");
|
events::send(events::ID("commander_kill_sw_disengaged"), events::Log::Info, "Kill disengaged");
|
||||||
_status_changed = true;
|
_status_changed = true;
|
||||||
_actuator_armed.manual_lockdown = false;
|
_actuator_armed.kill = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case action_request_s::ACTION_KILL:
|
case action_request_s::ACTION_KILL:
|
||||||
if (!_actuator_armed.manual_lockdown) {
|
if (!_actuator_armed.kill) {
|
||||||
const char kill_switch_string[] = "Kill engaged\t";
|
const char kill_switch_string[] = "Kill engaged\t";
|
||||||
events::LogLevels log_levels{events::Log::Info};
|
events::LogLevels log_levels{events::Log::Info};
|
||||||
|
|
||||||
@ -1687,7 +1687,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
|
|||||||
events::send(events::ID("commander_kill_sw_engaged"), log_levels, "Kill engaged");
|
events::send(events::ID("commander_kill_sw_engaged"), log_levels, "Kill engaged");
|
||||||
|
|
||||||
_status_changed = true;
|
_status_changed = true;
|
||||||
_actuator_armed.manual_lockdown = true;
|
_actuator_armed.kill = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
@ -1897,7 +1897,7 @@ void Commander::run()
|
|||||||
_actuator_armed.ready_to_arm = _vehicle_status.pre_flight_checks_pass || isArmed();
|
_actuator_armed.ready_to_arm = _vehicle_status.pre_flight_checks_pass || isArmed();
|
||||||
_actuator_armed.lockdown = ((_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON)
|
_actuator_armed.lockdown = ((_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON)
|
||||||
|| _multicopter_throw_launch.isThrowLaunchInProgress());
|
|| _multicopter_throw_launch.isThrowLaunchInProgress());
|
||||||
// _actuator_armed.manual_lockdown // action_request_s::ACTION_KILL
|
// _actuator_armed.kill // action_request_s::ACTION_KILL
|
||||||
_actuator_armed.termination = (_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION);
|
_actuator_armed.termination = (_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION);
|
||||||
// _actuator_armed.in_esc_calibration_mode // VEHICLE_CMD_PREFLIGHT_CALIBRATION
|
// _actuator_armed.in_esc_calibration_mode // VEHICLE_CMD_PREFLIGHT_CALIBRATION
|
||||||
|
|
||||||
@ -2285,7 +2285,7 @@ void Commander::handleAutoDisarm()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Auto disarm after 5 seconds if kill switch is engaged
|
// Auto disarm after 5 seconds if kill switch is engaged
|
||||||
bool auto_disarm = _actuator_armed.manual_lockdown;
|
bool auto_disarm = _actuator_armed.kill;
|
||||||
|
|
||||||
// auto disarm if locked down to avoid user confusion
|
// auto disarm if locked down to avoid user confusion
|
||||||
// skipped in HITL where lockdown is enabled for safety
|
// skipped in HITL where lockdown is enabled for safety
|
||||||
@ -2299,7 +2299,7 @@ void Commander::handleAutoDisarm()
|
|||||||
_auto_disarm_killed.set_state_and_update(auto_disarm, hrt_absolute_time());
|
_auto_disarm_killed.set_state_and_update(auto_disarm, hrt_absolute_time());
|
||||||
|
|
||||||
if (_auto_disarm_killed.get_state()) {
|
if (_auto_disarm_killed.get_state()) {
|
||||||
if (_actuator_armed.manual_lockdown) {
|
if (_actuator_armed.kill) {
|
||||||
disarm(arm_disarm_reason_t::kill_switch, true);
|
disarm(arm_disarm_reason_t::kill_switch, true);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
@ -42,7 +42,7 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter)
|
|||||||
actuator_armed_s actuator_armed;
|
actuator_armed_s actuator_armed;
|
||||||
|
|
||||||
if (_actuator_armed_sub.copy(&actuator_armed)) {
|
if (_actuator_armed_sub.copy(&actuator_armed)) {
|
||||||
if (actuator_armed.termination || actuator_armed.manual_lockdown) {
|
if (actuator_armed.termination || actuator_armed.kill) {
|
||||||
/* EVENT
|
/* EVENT
|
||||||
*/
|
*/
|
||||||
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_flight_term_active"),
|
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_flight_term_active"),
|
||||||
|
|||||||
@ -119,9 +119,8 @@ private:
|
|||||||
}
|
}
|
||||||
|
|
||||||
// system_status overrides
|
// system_status overrides
|
||||||
if (actuator_armed.termination
|
if (actuator_armed.termination || actuator_armed.kill
|
||||||
|| (actuator_armed.lockdown && vehicle_status.hil_state == vehicle_status_s::HIL_STATE_OFF)
|
|| (actuator_armed.lockdown && vehicle_status.hil_state == vehicle_status_s::HIL_STATE_OFF)) {
|
||||||
|| actuator_armed.manual_lockdown) {
|
|
||||||
|
|
||||||
system_status = MAV_STATE_FLIGHT_TERMINATION;
|
system_status = MAV_STATE_FLIGHT_TERMINATION;
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user