Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

commander/safety: replace safety.msg with Safety class (#19558)

Browse Source
This commit is contained in:
Igor Misic
2022-05-20 16:17:22 +02:00
committed by Beat Küng
parent b800600a6c
commit 08dcc72e1f
15 changed files with 106 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/CMakeLists.txt
-1
View File
@@ -137,7 +137,6 @@ set(msg_files
rc_parameter_map.msg rc_parameter_map.msg
rpm.msg rpm.msg
rtl_time_estimate.msg rtl_time_estimate.msg
safety.msg
satellite_info.msg satellite_info.msg
sensor_accel.msg sensor_accel.msg
sensor_accel_fifo.msg sensor_accel_fifo.msg
msg/safety.msg
-3
View File
@@ -1,3 +0,0 @@
uint64 timestamp # time since system start (microseconds)
bool safety_switch_available # Set to true if a safety switch is connected
bool safety_off # Set to true if safety is off
msg/vehicle_status.msg
+3
View File
@@ -116,3 +116,6 @@ uint8 latest_disarming_reason
uint64 armed_time # Arming timestamp (microseconds) uint64 armed_time # Arming timestamp (microseconds)
uint64 takeoff_time # Takeoff timestamp (microseconds) uint64 takeoff_time # Takeoff timestamp (microseconds)
bool safety_button_available # Set to true if a safety button is connected
bool safety_off # Set to true if safety is off
src/lib/events/enums.json
+7 -11
View File
@@ -146,38 +146,34 @@
"description": "mission start" "description": "mission start"
}, },
"6": { "6": {
"name": "safety_button",
"description": "safety button"
},
"7": {
"name": "auto_disarm_land", "name": "auto_disarm_land",
"description": "landing" "description": "landing"
}, },
"8": { "7": {
"name": "auto_disarm_preflight", "name": "auto_disarm_preflight",
"description": "auto preflight disarming" "description": "auto preflight disarming"
}, },
"9": { "8": {
"name": "kill_switch", "name": "kill_switch",
"description": "kill switch" "description": "kill switch"
}, },
"10": { "9": {
"name": "lockdown", "name": "lockdown",
"description": "lockdown" "description": "lockdown"
}, },
"11": { "10": {
"name": "failure_detector", "name": "failure_detector",
"description": "failure detector" "description": "failure detector"
}, },
"12": { "11": {
"name": "shutdown", "name": "shutdown",
"description": "shutdown request" "description": "shutdown request"
}, },
"13": { "12": {
"name": "unit_test", "name": "unit_test",
"description": "unit tests" "description": "unit tests"
}, },
"14": { "13": {
"name": "rc_button", "name": "rc_button",
"description": "RC (button)" "description": "RC (button)"
} }
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp
+4 -3
View File
@@ -39,7 +39,7 @@ constexpr bool
ArmStateMachine::arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX]; ArmStateMachine::arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX];
transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &status, transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &status,
const vehicle_control_mode_s &control_mode, const safety_s &safety, const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
const arming_state_t new_arming_state, actuator_armed_s &armed, const bool fRunPreArmChecks, const arming_state_t new_arming_state, actuator_armed_s &armed, const bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub, vehicle_status_flags_s &status_flags, orb_advert_t *mavlink_log_pub, vehicle_status_flags_s &status_flags,
const PreFlightCheck::arm_requirements_t &arm_requirements, const PreFlightCheck::arm_requirements_t &arm_requirements,
@@ -112,8 +112,9 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
if (fRunPreArmChecks && preflight_check_ret) { if (fRunPreArmChecks && preflight_check_ret) {
// only bother running prearm if preflight was successful // only bother running prearm if preflight was successful
prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, control_mode, safety, arm_requirements, prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, control_mode,
status); safety_button_available, safety_off,
arm_requirements, status);
} }
if (!preflight_check_ret || !prearm_check_ret) { if (!preflight_check_ret || !prearm_check_ret) {
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp
+2 -3
View File
@@ -37,7 +37,6 @@
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <px4_platform_common/events.h> #include <px4_platform_common/events.h>
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h> #include <uORB/topics/vehicle_status_flags.h>
@@ -55,8 +54,8 @@ public:
void forceArmState(uint8_t new_arm_state) { _arm_state = new_arm_state; } void forceArmState(uint8_t new_arm_state) { _arm_state = new_arm_state; }
transition_result_t transition_result_t
arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode, const safety_s &safety, arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode,
const arming_state_t new_arming_state, const bool safety_button_available, const bool safety_off, const arming_state_t new_arming_state,
actuator_armed_s &armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, actuator_armed_s &armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub,
vehicle_status_flags_s &status_flags, const PreFlightCheck::arm_requirements_t &arm_requirements, vehicle_status_flags_s &status_flags, const PreFlightCheck::arm_requirements_t &arm_requirements,
const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason); const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason);
src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp
+8 -10
View File
@@ -55,7 +55,7 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
hil_state_t hil_state; // Current vehicle_status_s.hil_state hil_state_t hil_state; // Current vehicle_status_s.hil_state
bool bool
system_sensors_initialized; // Current vehicle_status_s.system_sensors_initialized system_sensors_initialized; // Current vehicle_status_s.system_sensors_initialized
bool safety_switch_available; // Current safety_s.safety_switch_available bool safety_button_available; // Current safety_s.safety_button_available
bool safety_off; // Current safety_s.safety_off bool safety_off; // Current safety_s.safety_off
arming_state_t requested_state; // Requested arming state to transition to arming_state_t requested_state; // Requested arming state to transition to
ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition
@@ -166,17 +166,17 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED
}, },
// Safety switch arming tests // Safety button arming tests
{ {
"transition: standby to armed, no safety switch", "transition: standby to armed, no safety button",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
vehicle_status_s::ARMING_STATE_ARMED, vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED
}, },
{ {
"transition: standby to armed, safety switch off", "transition: standby to armed, safety button off",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
vehicle_status_s::ARMING_STATE_ARMED, vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED
@@ -242,10 +242,10 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
// vehicle_status_s::ARMING_STATE_STANDBY, // vehicle_status_s::ARMING_STATE_STANDBY,
// { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, // { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
// Safety switch arming tests // safety button arming tests
{ {
"no transition: init to armed, safety switch on", "no transition: init to armed, safety button on",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED, vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_DENIED { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_DENIED
@@ -254,7 +254,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
struct vehicle_status_s status {}; struct vehicle_status_s status {};
struct vehicle_status_flags_s status_flags {}; struct vehicle_status_flags_s status_flags {};
struct safety_s safety {};
struct actuator_armed_s armed {}; struct actuator_armed_s armed {};
size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]); size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
@@ -270,8 +269,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
status.hil_state = test->hil_state; status.hil_state = test->hil_state;
// The power status of the test unit is not relevant for the unit test // The power status of the test unit is not relevant for the unit test
status_flags.circuit_breaker_engaged_power_check = true; status_flags.circuit_breaker_engaged_power_check = true;
safety.safety_switch_available = test->safety_switch_available;
safety.safety_off = test->safety_off;
vehicle_control_mode_s control_mode{}; vehicle_control_mode_s control_mode{};
@@ -279,7 +276,8 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
transition_result_t result = arm_state_machine.arming_state_transition( transition_result_t result = arm_state_machine.arming_state_transition(
status, status,
control_mode, control_mode,
safety, test->safety_button_available,
test->safety_off,
test->requested_state, test->requested_state,
armed, armed,
true /* enable pre-arm checks */, true /* enable pre-arm checks */,
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp
+4 -4
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2019 PX4 Development Team. All rights reserved. * Copyright (c) 2019 - 2022 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -41,11 +41,11 @@
#pragma once #pragma once
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status_flags.h> #include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include "../../Safety.hpp"
typedef bool (*sens_check_func_t)(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, typedef bool (*sens_check_func_t)(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool is_mandatory, bool &report_fail); const bool is_mandatory, bool &report_fail);
@@ -94,8 +94,8 @@ public:
}; };
static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
const vehicle_control_mode_s &control_mode, const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status, const arm_requirements_t &arm_requirements, vehicle_status_s &status,
bool report_fail = true); bool report_fail = true);
private: private:
src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp
+5 -5
View File
@@ -39,8 +39,8 @@
#include <HealthFlags.h> #include <HealthFlags.h>
bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
const vehicle_control_mode_s &control_mode, const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status, bool report_fail) const arm_requirements_t &arm_requirements, vehicle_status_s &status, bool report_fail)
{ {
bool prearm_ok = true; bool prearm_ok = true;
@@ -156,10 +156,10 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
} }
// safety button // safety button
if (safety.safety_switch_available && !safety.safety_off) { if (safety_button_available && !safety_off) {
// Fail transition if we need safety switch press // Fail transition if we need safety button press
if (prearm_ok) { if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Press safety switch first"); } if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Press safety button first"); }
} }
prearm_ok = false; prearm_ok = false;
src/modules/commander/Commander.cpp
+42 -41
View File
@@ -301,7 +301,10 @@ int Commander::custom_command(int argc, char *argv[])
true, true, 30_s); true, true, 30_s);
PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED"); PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED");
bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, vehicle_status_flags, vehicle_control_mode, safety_s{}, bool dummy_safety_button{false};
bool dummy_safety_off{false};
bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, vehicle_status_flags, vehicle_control_mode,
dummy_safety_button, dummy_safety_off,
PreFlightCheck::arm_requirements_t{}, vehicle_status); PreFlightCheck::arm_requirements_t{}, vehicle_status);
PX4_INFO("Prearm check: %s", prearm_check_res ? "OK" : "FAILED"); PX4_INFO("Prearm check: %s", prearm_check_res ? "OK" : "FAILED");
@@ -493,7 +496,8 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
bool Commander::shutdown_if_allowed() bool Commander::shutdown_if_allowed()
{ {
return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety, return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode,
_safety.isButtonAvailable(), _safety.isSafetyOff(),
vehicle_status_s::ARMING_STATE_SHUTDOWN, vehicle_status_s::ARMING_STATE_SHUTDOWN,
_armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, _arm_requirements, _armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, _arm_requirements,
hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::shutdown); hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::shutdown);
@@ -514,8 +518,6 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r
case arm_disarm_reason_t::mission_start: return "mission start"; case arm_disarm_reason_t::mission_start: return "mission start";
case arm_disarm_reason_t::safety_button: return "safety button";
case arm_disarm_reason_t::auto_disarm_land: return "landing"; case arm_disarm_reason_t::auto_disarm_land: return "landing";
case arm_disarm_reason_t::auto_disarm_preflight: return "auto preflight disarming"; case arm_disarm_reason_t::auto_disarm_preflight: return "auto preflight disarming";
@@ -746,7 +748,8 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
} }
} }
transition_result_t arming_res = _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety, transition_result_t arming_res = _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode,
_safety.isButtonAvailable(), _safety.isSafetyOff(),
vehicle_status_s::ARMING_STATE_ARMED, _armed, run_preflight_checks, vehicle_status_s::ARMING_STATE_ARMED, _armed, run_preflight_checks,
&_mavlink_log_pub, _status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp), &_mavlink_log_pub, _status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
calling_reason); calling_reason);
@@ -788,7 +791,8 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
} }
} }
transition_result_t arming_res = _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety, transition_result_t arming_res = _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode,
_safety.isButtonAvailable(), _safety.isSafetyOff(),
vehicle_status_s::ARMING_STATE_STANDBY, _armed, false, vehicle_status_s::ARMING_STATE_STANDBY, _armed, false,
&_mavlink_log_pub, _status_flags, _arm_requirements, &_mavlink_log_pub, _status_flags, _arm_requirements,
hrt_elapsed_time(&_boot_timestamp), calling_reason); hrt_elapsed_time(&_boot_timestamp), calling_reason);
@@ -799,7 +803,7 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
"Disarmed by {1}", calling_reason); "Disarmed by {1}", calling_reason);
if (_param_com_force_safety.get()) { if (_param_com_force_safety.get()) {
_safety_handler.enableSafety(); _safety.activateSafety();
} }
_status_changed = true; _status_changed = true;
@@ -1393,7 +1397,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else { } else {
/* try to go to INIT/PREFLIGHT arming state */ /* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, safety_s{}, if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode,
_safety.isButtonAvailable(), _safety.isSafetyOff(),
vehicle_status_s::ARMING_STATE_INIT, _armed, vehicle_status_s::ARMING_STATE_INIT, _armed,
false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags,
PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_INIT PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_INIT
@@ -1474,9 +1479,17 @@ Commander::handle_command(const vehicle_command_s &cmd)
/* do esc calibration */ /* do esc calibration */
if (check_battery_disconnected(&_mavlink_log_pub)) { if (check_battery_disconnected(&_mavlink_log_pub)) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
if (_safety.isButtonAvailable() && !_safety.isSafetyOff()) {
mavlink_log_critical(&_mavlink_log_pub, "ESC calibration denied! Press safety button first\t");
events::send(events::ID("commander_esc_calibration_denied"), events::Log::Critical,
"ESCs calibration denied");
} else {
_status_flags.calibration_enabled = true; _status_flags.calibration_enabled = true;
_armed.in_esc_calibration_mode = true; _armed.in_esc_calibration_mode = true;
_worker_thread.startTask(WorkerThread::Request::ESCCalibration); _worker_thread.startTask(WorkerThread::Request::ESCCalibration);
}
} else { } else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
@@ -1630,7 +1643,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
unsigned unsigned
Commander::handle_command_motor_test(const vehicle_command_s &cmd) Commander::handle_command_motor_test(const vehicle_command_s &cmd)
{ {
if (_arm_state_machine.isArmed() || (_safety.safety_switch_available && !_safety.safety_off)) { if (_arm_state_machine.isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
} }
@@ -1678,7 +1691,7 @@ Commander::handle_command_motor_test(const vehicle_command_s &cmd)
unsigned unsigned
Commander::handle_command_actuator_test(const vehicle_command_s &cmd) Commander::handle_command_actuator_test(const vehicle_command_s &cmd)
{ {
if (_arm_state_machine.isArmed() || (_safety.safety_switch_available && !_safety.safety_off)) { if (_arm_state_machine.isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
} }
@@ -2274,29 +2287,18 @@ Commander::run()
} }
} }
_safety_handler.safetyButtonHandler(); /* safety button */
bool safety_updated = _safety.safetyButtonHandler();
/* update safety topic */ _status.safety_button_available = _safety.isButtonAvailable();
const bool safety_updated = _safety_sub.updated(); _status.safety_off = _safety.isSafetyOff();
if (safety_updated) { if (safety_updated) {
const bool previous_safety_valid = (_safety.timestamp != 0);
const bool previous_safety_off = _safety.safety_off;
if (_safety_sub.copy(&_safety)) { set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, _safety.isButtonAvailable(), _safety.isSafetyOff(),
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, _safety.safety_switch_available, _safety.safety_off, _safety.isButtonAvailable(), _status);
_safety.safety_switch_available, _status);
// disarm if safety is now on and still armed // Notify the user if the status of the safety button changes
if (_arm_state_machine.isArmed() && _safety.safety_switch_available && !_safety.safety_off if (_safety.isSafetyOff()) {
&& (_status.hil_state == vehicle_status_s::HIL_STATE_OFF)) {
disarm(arm_disarm_reason_t::safety_button);
}
// Notify the user if the status of the safety switch changes
if (previous_safety_valid && _safety.safety_switch_available && previous_safety_off != _safety.safety_off) {
if (_safety.safety_off) {
set_tune(tune_control_s::TUNE_ID_NOTIFY_POSITIVE); set_tune(tune_control_s::TUNE_ID_NOTIFY_POSITIVE);
} else { } else {
@@ -2305,8 +2307,6 @@ Commander::run()
_status_changed = true; _status_changed = true;
} }
}
}
/* update vtol vehicle status*/ /* update vtol vehicle status*/
if (_vtol_vehicle_status_sub.updated()) { if (_vtol_vehicle_status_sub.updated()) {
@@ -2452,7 +2452,8 @@ Commander::run()
/* If in INIT state, try to proceed to STANDBY state */ /* If in INIT state, try to proceed to STANDBY state */
if (!_status_flags.calibration_enabled && _arm_state_machine.isInit()) { if (!_status_flags.calibration_enabled && _arm_state_machine.isInit()) {
_arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety, _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode,
_safety.isButtonAvailable(), _safety.isSafetyOff(),
vehicle_status_s::ARMING_STATE_STANDBY, _armed, vehicle_status_s::ARMING_STATE_STANDBY, _armed,
true /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, true /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags,
_arm_requirements, hrt_elapsed_time(&_boot_timestamp), _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
@@ -2955,12 +2956,12 @@ Commander::run()
break; break;
case PrearmedMode::SAFETY_BUTTON: case PrearmedMode::SAFETY_BUTTON:
if (_safety.safety_switch_available) { if (_safety.isButtonAvailable()) {
/* safety switch is present, go into prearmed if safety is off */ /* safety button is present, go into prearmed if safety is off */
_armed.prearmed = _safety.safety_off; _armed.prearmed = _safety.isSafetyOff();
} else { } else {
/* safety switch is not present, do not go into prearmed */ /* safety button is not present, do not go into prearmed */
_armed.prearmed = false; _armed.prearmed = false;
} }
@@ -2989,8 +2990,9 @@ Commander::run()
// skip arm authorization check until actual arming attempt // skip arm authorization check until actual arming attempt
PreFlightCheck::arm_requirements_t arm_req = _arm_requirements; PreFlightCheck::arm_requirements_t arm_req = _arm_requirements;
arm_req.arm_authorization = false; arm_req.arm_authorization = false;
bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, _status_flags, _vehicle_control_mode, _safety, arm_req, bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, _status_flags, _vehicle_control_mode,
_status, false); _safety.isButtonAvailable(), _safety.isSafetyOff(),
arm_req, _status, false);
const bool prearm_check_ok = preflight_check_res && prearm_check_res; const bool prearm_check_ok = preflight_check_res && prearm_check_res;
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, prearm_check_ok, _status); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, prearm_check_ok, _status);
@@ -3035,8 +3037,7 @@ Commander::run()
} }
/* play arming and battery warning tunes */ /* play arming and battery warning tunes */
if (!_arm_tune_played && _arm_state_machine.isArmed() && if (!_arm_tune_played && _arm_state_machine.isArmed()) {
(_safety.safety_switch_available || (_safety.safety_switch_available && _safety.safety_off))) {
/* play tune when armed */ /* play tune when armed */
set_tune(tune_control_s::TUNE_ID_ARMING_WARNING); set_tune(tune_control_s::TUNE_ID_ARMING_WARNING);
@@ -3061,7 +3062,7 @@ Commander::run()
} }
/* reset arm_tune_played when disarmed */ /* reset arm_tune_played when disarmed */
if (!_arm_state_machine.isArmed() || (_safety.safety_switch_available && !_safety.safety_off)) { if (!_arm_state_machine.isArmed()) {
// Notify the user that it is safe to approach the vehicle // Notify the user that it is safe to approach the vehicle
if (_arm_tune_played) { if (_arm_tune_played) {
src/modules/commander/Commander.hpp
+1 -4
View File
@@ -80,7 +80,6 @@
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <uORB/topics/power_button_state.h> #include <uORB/topics/power_button_state.h>
#include <uORB/topics/rtl_time_estimate.h> #include <uORB/topics/rtl_time_estimate.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/system_power.h> #include <uORB/topics/system_power.h>
#include <uORB/topics/telemetry_status.h> #include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vehicle_angular_velocity.h> #include <uORB/topics/vehicle_angular_velocity.h>
@@ -392,7 +391,6 @@ private:
geofence_result_s _geofence_result{}; geofence_result_s _geofence_result{};
vehicle_land_detected_s _vehicle_land_detected{}; vehicle_land_detected_s _vehicle_land_detected{};
safety_s _safety{};
vtol_vehicle_status_s _vtol_status{}; vtol_vehicle_status_s _vtol_status{};
hrt_abstime _last_wind_warning{0}; hrt_abstime _last_wind_warning{0};
@@ -404,7 +402,7 @@ private:
vehicle_status_s _status{}; vehicle_status_s _status{};
vehicle_status_flags_s _status_flags{}; vehicle_status_flags_s _status_flags{};
Safety _safety_handler{}; Safety _safety{};
WorkerThread _worker_thread; WorkerThread _worker_thread;
@@ -419,7 +417,6 @@ private:
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _rtl_time_estimate_sub{ORB_ID(rtl_time_estimate)}; uORB::Subscription _rtl_time_estimate_sub{ORB_ID(rtl_time_estimate)};
uORB::Subscription _safety_sub{ORB_ID(safety)};
uORB::Subscription _system_power_sub{ORB_ID(system_power)}; uORB::Subscription _system_power_sub{ORB_ID(system_power)};
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
src/modules/commander/Safety.cpp
+15 -16
View File
@@ -48,36 +48,35 @@ Safety::Safety()
_safety_disabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY); _safety_disabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY);
} }
void Safety::safetyButtonHandler() bool Safety::safetyButtonHandler()
{ {
if (_safety_disabled) { if (_safety_disabled) {
_safety.safety_switch_available = true; _button_available = true;
_safety.safety_off = true; _safety_off = true;
} else { } else {
if (!_safety.safety_switch_available && _safety_button_sub.advertised()) { if (!_button_available && _safety_button_sub.advertised()) {
_safety.safety_switch_available = true; _button_available = true;
} }
button_event_s button_event; button_event_s button_event;
while (_safety_button_sub.update(&button_event)) { while (_safety_button_sub.update(&button_event)) {
_safety.safety_off |= button_event.triggered; // triggered safety button activates safety off _safety_off |= button_event.triggered; // triggered safety button activates safety off
} }
} }
// publish immediately on change, otherwise at 1 Hz for logging bool safety_changed = _previous_safety_off != _safety_off;
if ((hrt_elapsed_time(&_safety.timestamp) >= 1_s) ||
(_safety.safety_off != _previous_safety_off)) { _previous_safety_off = _safety_off;
_safety.timestamp = hrt_absolute_time();
_safety_pub.publish(_safety); return safety_changed;
} }
_previous_safety_off = _safety.safety_off; void Safety::activateSafety()
}
void Safety::enableSafety()
{ {
_safety.safety_off = false; if (!_safety_disabled) {
_safety_off = false;
}
} }
src/modules/commander/Safety.hpp
+8 -7
View File
@@ -40,7 +40,6 @@
#include <uORB/SubscriptionMultiArray.hpp> #include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/Publication.hpp> #include <uORB/Publication.hpp>
#include <uORB/topics/button_event.h> #include <uORB/topics/button_event.h>
#include <uORB/topics/safety.h>
class Safety class Safety
{ {
@@ -50,15 +49,17 @@ public:
Safety(); Safety();
~Safety() = default; ~Safety() = default;
void safetyButtonHandler(); bool safetyButtonHandler();
void enableSafety(); void activateSafety();
bool isButtonAvailable() { return _button_available;}
bool isSafetyOff() { return _safety_off;}
private: private:
uORB::Subscription _safety_button_sub{ORB_ID::safety_button}; uORB::Subscription _safety_button_sub{ORB_ID::safety_button};
uORB::Publication<safety_s> _safety_pub{ORB_ID(safety)};
safety_s _safety{}; bool _button_available{false}; //<! Set to true if a safety button is connected
bool _safety_disabled{false}; bool _safety_off{false}; //<! Set to true if safety is off
bool _previous_safety_off{false}; bool _previous_safety_off{false}; //<! Previous safety value
bool _safety_disabled{false}; //<! Set to true if safety is disabled
}; };
src/modules/commander/esc_calibration.cpp
-10
View File
@@ -52,7 +52,6 @@
#include <uORB/Subscription.hpp> #include <uORB/Subscription.hpp>
#include <uORB/topics/battery_status.h> #include <uORB/topics/battery_status.h>
#include <uORB/topics/actuator_test.h> #include <uORB/topics/actuator_test.h>
#include <uORB/topics/safety.h>
#include <parameters/param.h> #include <parameters/param.h>
using namespace time_literals; using namespace time_literals;
@@ -94,15 +93,6 @@ static void set_motor_actuators(uORB::Publication<actuator_test_s> &publisher, f
int do_esc_calibration_ctrl_alloc(orb_advert_t *mavlink_log_pub) int do_esc_calibration_ctrl_alloc(orb_advert_t *mavlink_log_pub)
{ {
// check safety
uORB::SubscriptionData<safety_s> safety_sub{ORB_ID(safety)};
safety_sub.update();
if (safety_sub.get().safety_switch_available && !safety_sub.get().safety_off) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disable safety first");
return PX4_ERROR;
}
int return_code = PX4_OK; int return_code = PX4_OK;
uORB::Publication<actuator_test_s> actuator_test_pub{ORB_ID(actuator_test)}; uORB::Publication<actuator_test_s> actuator_test_pub{ORB_ID(actuator_test)};
// since we publish multiple at once, make sure the output driver subscribes before we publish // since we publish multiple at once, make sure the output driver subscribes before we publish
src/modules/logger/logged_topics.cpp
-1
View File
@@ -86,7 +86,6 @@ void LoggedTopics::add_default_topics()
add_optional_topic("px4io_status"); add_optional_topic("px4io_status");
add_topic("radio_status"); add_topic("radio_status");
add_topic("rtl_time_estimate", 1000); add_topic("rtl_time_estimate", 1000);
add_topic("safety");
add_topic("sensor_combined"); add_topic("sensor_combined");
add_optional_topic("sensor_correction"); add_optional_topic("sensor_correction");
add_optional_topic("sensor_gyro_fft", 50); add_optional_topic("sensor_gyro_fft", 50);