From f197c8884d985eb9dc0fe43c504d55b26e4cff28 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 12 Aug 2022 13:44:39 +0200 Subject: [PATCH] commander: move esc checks to arming check --- msg/vehicle_status_flags.msg | 2 - src/modules/commander/Commander.cpp | 154 --------------- src/modules/commander/Commander.hpp | 10 - .../HealthAndArmingChecks/CMakeLists.txt | 1 + .../HealthAndArmingChecks.hpp | 3 + .../HealthAndArmingChecks/checks/escCheck.cpp | 186 ++++++++++++++++++ .../HealthAndArmingChecks/checks/escCheck.hpp | 59 ++++++ .../checks/systemCheck.cpp | 33 ---- .../checks/systemCheck.hpp | 1 - 9 files changed, 249 insertions(+), 200 deletions(-) create mode 100644 src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp create mode 100644 src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index fa756273ef..f6eb4df696 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -29,8 +29,6 @@ bool local_velocity_valid # set to true by the commander app if the quality of bool global_position_valid # set to true by the commander app if the quality of the global position estimate is good enough to use for navigation bool gps_position_valid bool home_position_valid # indicates a valid home position (a valid home position is not always a valid launch) -bool escs_error # set to true if one or more ESCs reporting esc_status are offline -bool escs_failure # set to true if one or more ESCs reporting esc_status has a failure bool offboard_control_signal_lost diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 8e7c0658b3..48a0d605f9 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -532,38 +532,6 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r return ""; }; -using esc_fault_reason_t = events::px4::enums::esc_fault_reason_t; -static_assert(esc_report_s::ESC_FAILURE_COUNT == (static_cast(esc_fault_reason_t::_max) + 1) - , "ESC fault flags mismatch!"); -static constexpr const char *esc_fault_reason_str(esc_fault_reason_t esc_fault_reason) -{ - switch (esc_fault_reason) { - case esc_fault_reason_t::over_current: return "over current"; - - case esc_fault_reason_t::over_voltage: return "over voltage"; - - case esc_fault_reason_t::motor_over_temp: return "motor critical temperature"; - - case esc_fault_reason_t::over_rpm: return "over RPM"; - - case esc_fault_reason_t::inconsistent_cmd: return "control failure"; - - case esc_fault_reason_t::motor_stuck: return "motor stall"; - - case esc_fault_reason_t::failure_generic: return "hardware failure"; - - case esc_fault_reason_t::motor_warn_temp: return "motor over temperature"; - - case esc_fault_reason_t::esc_warn_temp: return "over temperature"; - - case esc_fault_reason_t::esc_over_temp: return "critical temperature"; - - } - - return ""; -}; - - using navigation_mode_t = events::px4::enums::navigation_mode_t; @@ -2014,34 +1982,6 @@ Commander::run() } } - if (_esc_status_sub.updated()) { - /* ESCs status changed */ - esc_status_check(); - - } else if (_param_escs_checks_required.get() != 0) { - - if (!_vehicle_status_flags.escs_error) { - - if ((_last_esc_status_updated != 0) && (hrt_elapsed_time(&_last_esc_status_updated) > 700_ms)) { - /* Detect timeout after first telemetry packet received - * Some DShot ESCs are unresponsive for ~550ms during their initialization, so we use a timeout higher than that - */ - - mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry timeout\t"); - events::send(events::ID("commander_esc_telemetry_timeout"), events::Log::Critical, - "ESCs telemetry timeout"); - _vehicle_status_flags.escs_error = true; - - } else if (_last_esc_status_updated == 0 && hrt_elapsed_time(&_boot_timestamp) > 5000_ms) { - /* Detect if esc telemetry is not connected after reboot */ - mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry not connected\t"); - events::send(events::ID("commander_esc_telemetry_not_con"), events::Log::Critical, - "ESCs telemetry not connected"); - _vehicle_status_flags.escs_error = true; - } - } - } - _home_position.update(_param_com_home_en.get(), !_arm_state_machine.isArmed() && _vehicle_land_detected.landed); _vehicle_status_flags.home_position_valid = _home_position.valid(); @@ -3578,100 +3518,6 @@ Commander::offboard_control_update() } } -void Commander::esc_status_check() -{ - esc_status_s esc_status{}; - - _esc_status_sub.copy(&esc_status); - - if (esc_status.esc_count > 0) { - - char esc_fail_msg[50]; - esc_fail_msg[0] = '\0'; - - int online_bitmask = (1 << esc_status.esc_count) - 1; - - // Check if ALL the ESCs are online - if (online_bitmask == esc_status.esc_online_flags) { - - _vehicle_status_flags.escs_error = false; - _last_esc_online_flags = esc_status.esc_online_flags; - - } else if (_last_esc_online_flags == esc_status.esc_online_flags) { - - // Avoid checking the status if the flags are the same or if the mixer has not yet been loaded in the ESC driver - - _vehicle_status_flags.escs_error = true; - - } else if (esc_status.esc_online_flags < _last_esc_online_flags) { - - // Only warn the user when an ESC goes from ONLINE to OFFLINE. This is done to prevent showing Offline ESCs warnings at boot - - for (int index = 0; index < esc_status.esc_count; index++) { - if ((esc_status.esc_online_flags & (1 << index)) == 0) { - snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", index + 1); - esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0'; - events::px4::enums::suggested_action_t action = _arm_state_machine.isArmed() ? - events::px4::enums::suggested_action_t::land : - events::px4::enums::suggested_action_t::none; - uint8_t motor_index = esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + 1; - events::send(events::ID("commander_esc_offline"), - events::Log::Critical, "ESC{1} offline. {2}", motor_index, action); - } - } - - mavlink_log_critical(&_mavlink_log_pub, "%soffline. %s\t", esc_fail_msg, - _arm_state_machine.isArmed() ? "Land now!" : ""); - - _last_esc_online_flags = esc_status.esc_online_flags; - _vehicle_status_flags.escs_error = true; - } - - _vehicle_status_flags.escs_failure = false; - - for (int index = 0; index < esc_status.esc_count; index++) { - - _vehicle_status_flags.escs_failure |= esc_status.esc[index].failures > 0; - - if (esc_status.esc[index].failures != _last_esc_failure[index]) { - - for (uint8_t fault_index = 0; fault_index <= static_cast(esc_fault_reason_t::_max); - fault_index++) { - if (esc_status.esc[index].failures & (1 << fault_index)) { - - esc_fault_reason_t fault_reason_index = static_cast(fault_index); - - const char *user_action = nullptr; - events::px4::enums::suggested_action_t action = events::px4::enums::suggested_action_t::none; - - if (fault_reason_index == esc_fault_reason_t::motor_warn_temp - || fault_reason_index == esc_fault_reason_t::esc_warn_temp) { - user_action = "Reduce throttle"; - action = events::px4::enums::suggested_action_t::reduce_throttle; - - } else { - user_action = "Land now!"; - action = events::px4::enums::suggested_action_t::land; - } - - mavlink_log_emergency(&_mavlink_log_pub, "ESC%d: %s. %s \t", index + 1, - esc_fault_reason_str(fault_reason_index), _arm_state_machine.isArmed() ? user_action : ""); - - events::send - (events::ID("commander_esc_fault"), {events::Log::Emergency, events::LogInternal::Warning}, - "ESC {1}: {2}. {3}", index + 1, fault_reason_index, action); - } - } - } - - _last_esc_failure[index] = esc_status.esc[index].failures; - } - - } - - _last_esc_status_updated = esc_status.timestamp; -} - void Commander::send_parachute_command() { vehicle_command_s vcmd{}; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 082eb06439..d8c84aef72 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -69,7 +69,6 @@ #include #include #include -#include #include #include #include @@ -136,8 +135,6 @@ private: */ void data_link_check(); - void esc_status_check(); - void manual_control_check(); bool handle_command(const vehicle_command_s &cmd); @@ -217,8 +214,6 @@ private: // Engine failure (ParamInt) _param_com_actuator_failure_act, - (ParamBool) _param_escs_checks_required, - (ParamInt) _param_flight_uuid, (ParamInt) _param_takeoff_finished_action, @@ -293,10 +288,6 @@ private: hrt_abstime _high_latency_datalink_heartbeat{0}; hrt_abstime _high_latency_datalink_lost{0}; - int _last_esc_online_flags{-1}; - int _last_esc_failure[esc_status_s::CONNECTED_ESC_MAX] {}; - hrt_abstime _last_esc_status_updated{0}; - uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE}; hrt_abstime _battery_failsafe_timestamp{0}; Hysteresis _auto_disarm_landed{false}; @@ -348,7 +339,6 @@ private: // Subscriptions uORB::Subscription _action_request_sub {ORB_ID(action_request)}; uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; - uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)}; uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index 7b53acfbbe..c631e19f06 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -42,6 +42,7 @@ px4_add_library(health_and_arming_checks checks/imuConsistencyCheck.cpp checks/airspeedCheck.cpp checks/distanceSensorChecks.cpp + checks/escCheck.cpp checks/rcCalibrationCheck.cpp checks/powerCheck.cpp checks/estimatorCheck.cpp diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index dcce2f9bff..64247a7ce4 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -44,6 +44,7 @@ #include "checks/baroCheck.hpp" #include "checks/cpuResourceCheck.hpp" #include "checks/distanceSensorChecks.hpp" +#include "checks/escCheck.hpp" #include "checks/estimatorCheck.hpp" #include "checks/failureDetectorCheck.hpp" #include "checks/gyroCheck.hpp" @@ -98,6 +99,7 @@ private: BaroChecks _baro_checks; CpuResourceChecks _cpu_resource_checks; DistanceSensorChecks _distance_sensor_checks; + EscChecks _esc_checks; EstimatorChecks _estimator_checks; FailureDetectorChecks _failure_detector_checks; GyroChecks _gyro_checks; @@ -118,6 +120,7 @@ private: &_baro_checks, &_cpu_resource_checks, &_distance_sensor_checks, + &_esc_checks, &_estimator_checks, &_failure_detector_checks, &_gyro_checks, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp new file mode 100644 index 0000000000..cd7d716b31 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp @@ -0,0 +1,186 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "escCheck.hpp" +#include +#include + +using namespace time_literals; + +using esc_fault_reason_t = events::px4::enums::esc_fault_reason_t; +static_assert(esc_report_s::ESC_FAILURE_COUNT == (static_cast(esc_fault_reason_t::_max) + 1) + , "ESC fault flags mismatch!"); +static constexpr const char *esc_fault_reason_str(esc_fault_reason_t esc_fault_reason) +{ + switch (esc_fault_reason) { + case esc_fault_reason_t::over_current: return "over current"; + + case esc_fault_reason_t::over_voltage: return "over voltage"; + + case esc_fault_reason_t::motor_over_temp: return "motor critical temperature"; + + case esc_fault_reason_t::over_rpm: return "over RPM"; + + case esc_fault_reason_t::inconsistent_cmd: return "control failure"; + + case esc_fault_reason_t::motor_stuck: return "motor stall"; + + case esc_fault_reason_t::failure_generic: return "hardware failure"; + + case esc_fault_reason_t::motor_warn_temp: return "motor over temperature"; + + case esc_fault_reason_t::esc_warn_temp: return "over temperature"; + + case esc_fault_reason_t::esc_over_temp: return "critical temperature"; + + } + + return ""; +}; + + +void EscChecks::checkAndReport(const Context &context, Report &reporter) +{ + const hrt_abstime now = hrt_absolute_time(); + const hrt_abstime esc_telemetry_timeout = + 700_ms; // Some DShot ESCs are unresponsive for ~550ms during their initialization, so we use a timeout higher than that + + esc_status_s esc_status; + + if (_esc_status_sub.copy(&esc_status) && now - esc_status.timestamp < esc_telemetry_timeout) { + + checkEscStatus(context, reporter, esc_status); + reporter.setIsPresent(health_component_t::motors_escs); + + } else if (_param_escs_checks_required.get() + && now - _start_time > 5_s) { // Wait a bit after startup to allow esc's to init + + /* EVENT + * @description + * + * This check can be configured via COM_ARM_CHK_ESCS parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_escs_telem_missing"), + events::Log::Critical, "ESC telemetry missing"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: ESC telemetry missing"); + } + } +} + +void EscChecks::checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status) +{ + const NavModes required_modes = _param_escs_checks_required.get() ? NavModes::All : NavModes::None; + + if (esc_status.esc_count > 0) { + + char esc_fail_msg[50]; + esc_fail_msg[0] = '\0'; + + int online_bitmask = (1 << esc_status.esc_count) - 1; + + // Check if one or more the ESCs are offline + if (online_bitmask != esc_status.esc_online_flags) { + + for (int index = 0; index < esc_status.esc_count; index++) { + if ((esc_status.esc_online_flags & (1 << index)) == 0) { + uint8_t motor_index = esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + 1; + /* EVENT + * @description + * + * This check can be configured via COM_ARM_CHK_ESCS parameter. + * + */ + reporter.healthFailure(required_modes, health_component_t::motors_escs, events::ID("check_escs_offline"), + events::Log::Critical, "ESC {1} offline", motor_index); + snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", motor_index); + esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0'; + } + } + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "%soffline. %s\t", esc_fail_msg, context.isArmed() ? "Land now!" : ""); + } + } + + for (int index = 0; index < esc_status.esc_count; index++) { + + if (esc_status.esc[index].failures != 0) { + + for (uint8_t fault_index = 0; fault_index <= static_cast(esc_fault_reason_t::_max); fault_index++) { + if (esc_status.esc[index].failures & (1 << fault_index)) { + + esc_fault_reason_t fault_reason_index = static_cast(fault_index); + + const char *user_action = ""; + events::px4::enums::suggested_action_t action = events::px4::enums::suggested_action_t::none; + + if (context.isArmed()) { + if (fault_reason_index == esc_fault_reason_t::motor_warn_temp + || fault_reason_index == esc_fault_reason_t::esc_warn_temp) { + user_action = "Reduce throttle"; + action = events::px4::enums::suggested_action_t::reduce_throttle; + + } else { + user_action = "Land now!"; + action = events::px4::enums::suggested_action_t::land; + } + } + + uint8_t motor_index = esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + 1; + + /* EVENT + * @description + * {3} + * + * + * This check can be configured via COM_ARM_CHK_ESCS parameter. + * + */ + reporter.healthFailure( + required_modes, health_component_t::motors_escs, events::ID("check_escs_fault"), + events::Log::Critical, "ESC {1}: {2}", motor_index, fault_reason_index, action); + + if (reporter.mavlink_log_pub()) { + mavlink_log_emergency(reporter.mavlink_log_pub(), "ESC%d: %s. %s \t", motor_index, + esc_fault_reason_str(fault_reason_index), user_action); + } + } + } + } + } + } +} + diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp new file mode 100644 index 0000000000..7e32563e46 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include + +class EscChecks : public HealthAndArmingCheckBase +{ +public: + EscChecks() = default; + ~EscChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + void checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status); + + uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; + + const hrt_abstime _start_time{hrt_absolute_time()}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamBool) _param_escs_checks_required + ) +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp index 3618cd4730..ec7aaa2905 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp @@ -161,39 +161,6 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter) } } - // ESC checks - if (_param_com_arm_chk_escs.get() && reporter.failsafeFlags().escs_error) { - /* EVENT - * @description - * - * This check can be configured via COM_ARM_CHK_ESCS parameter. - * - */ - reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_system_escs_offline"), - events::Log::Error, "One or more ESCs are offline"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: One or more ESCs are offline"); - } - } - - if (_param_com_arm_chk_escs.get() && reporter.failsafeFlags().escs_failure) { - /* EVENT - * @description - * - * This check can be configured via COM_ARM_CHK_ESCS parameter. - * - */ - reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_system_escs_failure"), - events::Log::Error, "One or more ESCs have a failure"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: One or more ESCs have a failure"); - } - } - - reporter.setIsPresent(health_component_t::motors_escs); // TODO: based on telemetry - // VTOL in transition if (context.status().is_vtol && !context.isArmed()) { if (context.status().in_transition_mode) { diff --git a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp index e9a966302f..e4dd148912 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp @@ -54,7 +54,6 @@ private: (ParamInt) _param_cbrk_usb_chk, (ParamBool) _param_com_arm_mis_req, (ParamBool) _param_com_arm_wo_gps, - (ParamBool) _param_com_arm_chk_escs, (ParamInt) _param_com_arm_auth_req, (ParamInt) _param_gf_action )