From 13c93db11a8b686843c4f05431e72a535fdb3ecc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 8 Aug 2022 13:47:07 +0200 Subject: [PATCH] commander: use circuit breaker params directly in arming checks --- msg/vehicle_status.msg | 6 ------ src/lib/circuit_breaker/circuit_breaker.cpp | 5 ----- src/lib/circuit_breaker/circuit_breaker.h | 6 +++++- src/modules/commander/Commander.cpp | 16 +++------------- src/modules/commander/Commander.hpp | 1 + .../checks/airspeedCheck.cpp | 3 ++- .../checks/airspeedCheck.hpp | 1 + .../HealthAndArmingChecks/checks/powerCheck.cpp | 3 ++- .../HealthAndArmingChecks/checks/powerCheck.hpp | 1 + .../HealthAndArmingChecks/checks/systemCheck.cpp | 5 +++-- .../HealthAndArmingChecks/checks/systemCheck.hpp | 2 ++ 11 files changed, 20 insertions(+), 29 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 716fcc9d23..dc733c517e 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -120,9 +120,3 @@ bool avoidance_system_valid # Status of the obstacle avoid bool battery_healthy # set if battery is available and not low -bool circuit_breaker_engaged_power_check -bool circuit_breaker_engaged_airspd_check -bool circuit_breaker_flight_termination_disabled -bool circuit_breaker_engaged_usb_check -bool circuit_breaker_vtol_fw_arming_check # set to true if for VTOLs arming in fixed-wing mode should be allowed - diff --git a/src/lib/circuit_breaker/circuit_breaker.cpp b/src/lib/circuit_breaker/circuit_breaker.cpp index f3b81e7849..5d658e023a 100644 --- a/src/lib/circuit_breaker/circuit_breaker.cpp +++ b/src/lib/circuit_breaker/circuit_breaker.cpp @@ -54,8 +54,3 @@ bool circuit_breaker_enabled(const char *breaker, int32_t magic) return (param_get(param_find(breaker), &val) == 0) && (val == magic); } - -bool circuit_breaker_enabled_by_val(int32_t breaker_val, int32_t magic) -{ - return (breaker_val == magic); -} diff --git a/src/lib/circuit_breaker/circuit_breaker.h b/src/lib/circuit_breaker/circuit_breaker.h index 01cad483f6..9487278bc8 100644 --- a/src/lib/circuit_breaker/circuit_breaker.h +++ b/src/lib/circuit_breaker/circuit_breaker.h @@ -63,7 +63,11 @@ __BEGIN_DECLS extern "C" __EXPORT bool circuit_breaker_enabled(const char *breaker, int32_t magic); -extern "C" __EXPORT bool circuit_breaker_enabled_by_val(int32_t breaker_val, int32_t magic); + +static inline bool circuit_breaker_enabled_by_val(int32_t breaker_val, int32_t magic) +{ + return breaker_val == magic; +} __END_DECLS diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index e2e5a79fed..18f684226b 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2607,7 +2607,7 @@ Commander::run() /* Check for mission flight termination */ if (_arm_state_machine.isArmed() && _mission_result_sub.get().flight_termination && - !_vehicle_status.circuit_breaker_flight_termination_disabled) { + !_circuit_breaker_flight_termination_disabled) { if (!_flight_termination_triggered && !_lockdown_triggered) { @@ -2717,7 +2717,7 @@ Commander::run() events::send(events::ID("commander_fd_lockdown"), {events::Log::Emergency, events::LogInternal::Warning}, "Critical failure detected: lockdown"); - } else if (!_vehicle_status.circuit_breaker_flight_termination_disabled && + } else if (!_circuit_breaker_flight_termination_disabled && !_flight_termination_triggered && !_lockdown_triggered) { _actuator_armed.force_failsafe = true; @@ -3086,19 +3086,9 @@ Commander::run() void Commander::get_circuit_breaker_params() { - _vehicle_status.circuit_breaker_engaged_power_check = circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(), - CBRK_SUPPLY_CHK_KEY); - _vehicle_status.circuit_breaker_engaged_usb_check = circuit_breaker_enabled_by_val(_param_cbrk_usb_chk.get(), - CBRK_USB_CHK_KEY); - _vehicle_status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled_by_val( - _param_cbrk_airspd_chk.get(), - CBRK_AIRSPD_CHK_KEY); - _vehicle_status.circuit_breaker_flight_termination_disabled = circuit_breaker_enabled_by_val( + _circuit_breaker_flight_termination_disabled = circuit_breaker_enabled_by_val( _param_cbrk_flightterm.get(), CBRK_FLIGHTTERM_KEY); - _vehicle_status.circuit_breaker_vtol_fw_arming_check = circuit_breaker_enabled_by_val( - _param_cbrk_vtolarming.get(), - CBRK_VTOLARMING_KEY); } void Commander::check_and_inform_ready_for_takeoff() diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 5753212165..1861425963 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -314,6 +314,7 @@ private: bool _geofence_warning_action_on{false}; bool _geofence_violated_prev{false}; + bool _circuit_breaker_flight_termination_disabled{false}; bool _rtl_time_actions_done{false}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp index 0a54a3aca4..a69cd26826 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp @@ -32,6 +32,7 @@ ****************************************************************************/ #include "airspeedCheck.hpp" +#include using namespace time_literals; @@ -42,7 +43,7 @@ AirspeedChecks::AirspeedChecks() void AirspeedChecks::checkAndReport(const Context &context, Report &reporter) { - if (context.status().circuit_breaker_engaged_airspd_check || + if (circuit_breaker_enabled_by_val(_param_cbrk_airspd_chk.get(), CBRK_AIRSPD_CHK_KEY) || (context.status().vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !context.status().is_vtol)) { return; } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.hpp index 41549aee2e..866ae54250 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.hpp @@ -53,6 +53,7 @@ private: const param_t _param_fw_airspd_trim_handle; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamInt) _param_cbrk_airspd_chk, (ParamBool) _param_com_arm_arsp_en ) }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp index 77fd33c2bd..8b40191c42 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp @@ -32,12 +32,13 @@ ****************************************************************************/ #include "powerCheck.hpp" +#include using namespace time_literals; void PowerChecks::checkAndReport(const Context &context, Report &reporter) { - if (context.status().circuit_breaker_engaged_power_check) { + if (circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(), CBRK_SUPPLY_CHK_KEY)) { return; } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp index 94d753fd3a..72ed928391 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp @@ -50,6 +50,7 @@ private: uORB::Subscription _system_power_sub{ORB_ID(system_power)}; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamInt) _param_cbrk_supply_chk, (ParamInt) _param_com_power_count ) }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp index 8da933093c..3618cd4730 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp @@ -34,6 +34,7 @@ #include "systemCheck.hpp" #include "../../Arming/ArmAuthorization/ArmAuthorization.h" +#include #include void SystemChecks::checkAndReport(const Context &context, Report &reporter) @@ -54,7 +55,7 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter) } // USB not connected - if (!context.status().circuit_breaker_engaged_usb_check && context.status().usb_connected) { + if (!circuit_breaker_enabled_by_val(_param_cbrk_usb_chk.get(), CBRK_USB_CHK_KEY) && context.status().usb_connected) { /* EVENT * @description * Flying with USB is not safe. Disconnect it and reboot the FMU. @@ -206,7 +207,7 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter) } } - if (!context.status().circuit_breaker_vtol_fw_arming_check + if (!circuit_breaker_enabled_by_val(_param_cbrk_vtolarming.get(), CBRK_VTOLARMING_KEY) && context.status().vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { /* EVENT * @description diff --git a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp index daf31fe5da..e9a966302f 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp @@ -50,6 +50,8 @@ private: uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamInt) _param_cbrk_vtolarming, + (ParamInt) _param_cbrk_usb_chk, (ParamBool) _param_com_arm_mis_req, (ParamBool) _param_com_arm_wo_gps, (ParamBool) _param_com_arm_chk_escs,