diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 84045809a0..0709971fd2 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -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 diff --git a/msg/safety.msg b/msg/safety.msg deleted file mode 100644 index 429b76b726..0000000000 --- a/msg/safety.msg +++ /dev/null @@ -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 diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index a53449ade0..19a9171aa6 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -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 diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index 1dd476076d..95efd34331 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -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)" } diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp index 15400161d4..70c06166bd 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp @@ -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) { diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp index 999bcd864a..217367f9d1 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp @@ -37,7 +37,6 @@ #include #include #include -#include #include #include #include @@ -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); diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp index 5da49a9a3a..9511d1276e 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp @@ -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 */, diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index fae89e005a..58432d425b 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -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 -#include #include #include #include #include +#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: diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp index e6552084e0..df7eea0d1a 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp @@ -39,8 +39,8 @@ #include 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; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 1cd1bee39a..24c7ad603a 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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); - // 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); - } + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, _safety.isButtonAvailable(), _safety.isSafetyOff(), + _safety.isButtonAvailable(), _status); - // 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) { + // Notify the user if the status of the safety button changes + if (_safety.isSafetyOff()) { + set_tune(tune_control_s::TUNE_ID_NOTIFY_POSITIVE); - 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) { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 2d4f2b3db7..9b7eb05d06 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -80,7 +80,6 @@ #include #include #include -#include #include #include #include @@ -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)}; diff --git a/src/modules/commander/Safety.cpp b/src/modules/commander/Safety.cpp index 652b4148f4..49ca905a04 100644 --- a/src/modules/commander/Safety.cpp +++ b/src/modules/commander/Safety.cpp @@ -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_off; - _previous_safety_off = _safety.safety_off; + return safety_changed; } -void Safety::enableSafety() +void Safety::activateSafety() { - _safety.safety_off = false; + if (!_safety_disabled) { + _safety_off = false; + } } diff --git a/src/modules/commander/Safety.hpp b/src/modules/commander/Safety.hpp index 3a78aeeefa..4b06d7e26d 100644 --- a/src/modules/commander/Safety.hpp +++ b/src/modules/commander/Safety.hpp @@ -40,7 +40,6 @@ #include #include #include -#include 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_pub{ORB_ID(safety)}; - safety_s _safety{}; - bool _safety_disabled{false}; - bool _previous_safety_off{false}; + bool _button_available{false}; // #include #include -#include #include using namespace time_literals; @@ -94,15 +93,6 @@ static void set_motor_actuators(uORB::Publication &publisher, f int do_esc_calibration_ctrl_alloc(orb_advert_t *mavlink_log_pub) { - // check safety - uORB::SubscriptionData 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_pub{ORB_ID(actuator_test)}; // since we publish multiple at once, make sure the output driver subscribes before we publish diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 0d2bfce453..1a0840006d 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -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);