fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -137,7 +137,6 @@ set(msg_files
rc_parameter_map.msg
rpm.msg
rtl_time_estimate.msg
safety.msg
satellite_info.msg
sensor_accel.msg
sensor_accel_fifo.msg

3
msg/safety.msg
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

3
msg/vehicle_status.msg
View File

@ -116,3 +116,6 @@ uint8 latest_disarming_reason
uint64 armed_time # Arming 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

18
src/lib/events/enums.json
View File

@ -146,38 +146,34 @@
"description": "mission start"
},
"6": {
"name": "safety_button",
"description": "safety button"
},
"7": {
"name": "auto_disarm_land",
"description": "landing"
},
"8": {
"7": {
"name": "auto_disarm_preflight",
"description": "auto preflight disarming"
},
"9": {
"8": {
"name": "kill_switch",
"description": "kill switch"
},
"10": {
"9": {
"name": "lockdown",
"description": "lockdown"
},
"11": {
"10": {
"name": "failure_detector",
"description": "failure detector"
},
"12": {
"11": {
"name": "shutdown",
"description": "shutdown request"
},
"13": {
"12": {
"name": "unit_test",
"description": "unit tests"
},
"14": {
"13": {
"name": "rc_button",
"description": "RC (button)"
}

7
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp
View File

@ -39,7 +39,7 @@ constexpr bool
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,
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,
orb_advert_t *mavlink_log_pub, vehicle_status_flags_s &status_flags,
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) {
// only bother running prearm if preflight was successful
prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, control_mode, safety, arm_requirements,
status);
prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, control_mode,
safety_button_available, safety_off,
arm_requirements, status);
}
if (!preflight_check_ret || !prearm_check_ret) {

5
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp
View File

@ -37,7 +37,6 @@
#include <drivers/drv_hrt.h>
#include <px4_platform_common/events.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.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; }
transition_result_t
arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode, const safety_s &safety,
const arming_state_t new_arming_state,
arming_state_transition(vehicle_status_s &status, 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, orb_advert_t *mavlink_log_pub,
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);

18
src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp
View File

@ -55,7 +55,7 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
hil_state_t hil_state; // Current vehicle_status_s.hil_state
bool
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
arming_state_t requested_state; // Requested arming state to transition to
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
},
// 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_ARMED,
{ 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_ARMED,
{ 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_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_ARMED,
{ 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_flags_s status_flags {};
struct safety_s safety {};
struct actuator_armed_s armed {};
size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
@ -270,8 +269,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
status.hil_state = test->hil_state;
// The power status of the test unit is not relevant for the unit test
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{};
@ -279,7 +276,8 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
transition_result_t result = arm_state_machine.arming_state_transition(
status,
control_mode,
safety,
test->safety_button_available,
test->safety_off,
test->requested_state,
armed,
true /* enable pre-arm checks */,

8
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp
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
* modification, are permitted provided that the following conditions
@ -41,11 +41,11 @@
#pragma once
#include <uORB/uORB.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/vehicle_status.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,
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,
const vehicle_control_mode_s &control_mode,
const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status,
const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
const arm_requirements_t &arm_requirements, vehicle_status_s &status,
bool report_fail = true);
private:

10
src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp
View File

@ -39,8 +39,8 @@
#include <HealthFlags.h>
bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
const vehicle_control_mode_s &control_mode,
const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status, bool report_fail)
const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
const arm_requirements_t &arm_requirements, vehicle_status_s &status, bool report_fail)
{
bool prearm_ok = true;
@ -156,10 +156,10 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
}
// safety button
if (safety.safety_switch_available && !safety.safety_off) {
// Fail transition if we need safety switch press
if (safety_button_available && !safety_off) {
// Fail transition if we need safety button press
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;

99
src/modules/commander/Commander.cpp
View File

@ -301,7 +301,10 @@ int Commander::custom_command(int argc, char *argv[])
true, true, 30_s);
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);
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()
{
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,
_armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, _arm_requirements,
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::safety_button: return "safety button";
case arm_disarm_reason_t::auto_disarm_land: return "landing";
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,
&_mavlink_log_pub, _status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
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,
&_mavlink_log_pub, _status_flags, _arm_requirements,
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);
if (_param_com_force_safety.get()) {
_safety_handler.enableSafety();
_safety.activateSafety();
}
_status_changed = true;
@ -1393,7 +1397,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else {
/* 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,
false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags,
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 */
if (check_battery_disconnected(&_mavlink_log_pub)) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
_status_flags.calibration_enabled = true;
_armed.in_esc_calibration_mode = true;
_worker_thread.startTask(WorkerThread::Request::ESCCalibration);
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;
_armed.in_esc_calibration_mode = true;
_worker_thread.startTask(WorkerThread::Request::ESCCalibration);
}
} else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
@ -1630,7 +1643,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
unsigned
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;
}
@ -1678,7 +1691,7 @@ Commander::handle_command_motor_test(const vehicle_command_s &cmd)
unsigned
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;
}
@ -2274,38 +2287,25 @@ Commander::run()
}
}
_safety_handler.safetyButtonHandler();
/* update safety topic */
const bool safety_updated = _safety_sub.updated();
/* safety button */
bool safety_updated = _safety.safetyButtonHandler();
_status.safety_button_available = _safety.isButtonAvailable();
_status.safety_off = _safety.isSafetyOff();
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.safety_switch_available, _safety.safety_off,
_safety.safety_switch_available, _status);
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, _safety.isButtonAvailable(), _safety.isSafetyOff(),
_safety.isButtonAvailable(), _status);
// disarm if safety is now on and still armed
if (_arm_state_machine.isArmed() && _safety.safety_switch_available && !_safety.safety_off
&& (_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 button changes
if (_safety.isSafetyOff()) {
set_tune(tune_control_s::TUNE_ID_NOTIFY_POSITIVE);
// 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);
} else {
tune_neutral(true);
}
_status_changed = true;
}
} else {
tune_neutral(true);
}
_status_changed = true;
}
/* update vtol vehicle status*/
@ -2452,7 +2452,8 @@ Commander::run()
/* If in INIT state, try to proceed to STANDBY state */
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,
true /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags,
_arm_requirements, hrt_elapsed_time(&_boot_timestamp),
@ -2955,12 +2956,12 @@ Commander::run()
break;
case PrearmedMode::SAFETY_BUTTON:
if (_safety.safety_switch_available) {
/* safety switch is present, go into prearmed if safety is off */
_armed.prearmed = _safety.safety_off;
if (_safety.isButtonAvailable()) {
/* safety button is present, go into prearmed if safety is off */
_armed.prearmed = _safety.isSafetyOff();
} else {
/* safety switch is not present, do not go into prearmed */
/* safety button is not present, do not go into prearmed */
_armed.prearmed = false;
}
@ -2989,8 +2990,9 @@ Commander::run()
// skip arm authorization check until actual arming attempt
PreFlightCheck::arm_requirements_t arm_req = _arm_requirements;
arm_req.arm_authorization = false;
bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, _status_flags, _vehicle_control_mode, _safety, arm_req,
_status, false);
bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, _status_flags, _vehicle_control_mode,
_safety.isButtonAvailable(), _safety.isSafetyOff(),
arm_req, _status, false);
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);
@ -3035,8 +3037,7 @@ Commander::run()
}
/* play arming and battery warning tunes */
if (!_arm_tune_played && _arm_state_machine.isArmed() &&
(_safety.safety_switch_available || (_safety.safety_switch_available && _safety.safety_off))) {
if (!_arm_tune_played && _arm_state_machine.isArmed()) {
/* play tune when armed */
set_tune(tune_control_s::TUNE_ID_ARMING_WARNING);
@ -3061,7 +3062,7 @@ Commander::run()
}
/* 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
if (_arm_tune_played) {

5
src/modules/commander/Commander.hpp
View File

@ -80,7 +80,6 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/power_button_state.h>
#include <uORB/topics/rtl_time_estimate.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/system_power.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vehicle_angular_velocity.h>
@ -392,7 +391,6 @@ private:
geofence_result_s _geofence_result{};
vehicle_land_detected_s _vehicle_land_detected{};
safety_s _safety{};
vtol_vehicle_status_s _vtol_status{};
hrt_abstime _last_wind_warning{0};
@ -404,7 +402,7 @@ private:
vehicle_status_s _status{};
vehicle_status_flags_s _status_flags{};
Safety _safety_handler{};
Safety _safety{};
WorkerThread _worker_thread;
@ -419,7 +417,6 @@ private:
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
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 _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};

29
src/modules/commander/Safety.cpp
View File

@ -48,36 +48,35 @@ Safety::Safety()
_safety_disabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY);
}
void Safety::safetyButtonHandler()
bool Safety::safetyButtonHandler()
{
if (_safety_disabled) {
_safety.safety_switch_available = true;
_safety.safety_off = true;
_button_available = true;
_safety_off = true;
} else {
if (!_safety.safety_switch_available && _safety_button_sub.advertised()) {
_safety.safety_switch_available = true;
if (!_button_available && _safety_button_sub.advertised()) {
_button_available = true;
}
button_event_s 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
if ((hrt_elapsed_time(&_safety.timestamp) >= 1_s) ||
(_safety.safety_off != _previous_safety_off)) {
_safety.timestamp = hrt_absolute_time();
_safety_pub.publish(_safety);
}
bool safety_changed = _previous_safety_off != _safety_off;
_previous_safety_off = _safety.safety_off;
_previous_safety_off = _safety_off;
return safety_changed;
}
void Safety::enableSafety()
void Safety::activateSafety()
{
_safety.safety_off = false;
if (!_safety_disabled) {
_safety_off = false;
}
}

15
src/modules/commander/Safety.hpp
View File

@ -40,7 +40,6 @@
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/button_event.h>
#include <uORB/topics/safety.h>
class Safety
{
@ -50,15 +49,17 @@ public:
Safety();
~Safety() = default;
void safetyButtonHandler();
void enableSafety();
bool safetyButtonHandler();
void activateSafety();
bool isButtonAvailable() { return _button_available;}
bool isSafetyOff() { return _safety_off;}
private:
uORB::Subscription _safety_button_sub{ORB_ID::safety_button};
uORB::Publication<safety_s> _safety_pub{ORB_ID(safety)};
safety_s _safety{};
bool _safety_disabled{false};
bool _previous_safety_off{false};
bool _button_available{false}; //<! Set to true if a safety button is connected
bool _safety_off{false}; //<! Set to true if safety is off
bool _previous_safety_off{false}; //<! Previous safety value
bool _safety_disabled{false}; //<! Set to true if safety is disabled
};

10
src/modules/commander/esc_calibration.cpp
View File

@ -52,7 +52,6 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/actuator_test.h>
#include <uORB/topics/safety.h>
#include <parameters/param.h>
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)
{
// 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;
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

1
src/modules/logger/logged_topics.cpp
View File

@ -86,7 +86,6 @@ void LoggedTopics::add_default_topics()
add_optional_topic("px4io_status");
add_topic("radio_status");
add_topic("rtl_time_estimate", 1000);
add_topic("safety");
add_topic("sensor_combined");
add_optional_topic("sensor_correction");
add_optional_topic("sensor_gyro_fft", 50);