From d631a5d39f4bdf0acb1ef5973616633e179877d3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Thu, 22 Apr 2021 09:33:31 +0200 Subject: [PATCH] Commander: checking COM_ARM_CHK_ESCS param for ESCs telemetry MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit COM_ARM_CHK_ESCS set to 0. The user will need to enable it manually. Co-authored-by: Beat Küng --- src/modules/commander/Commander.cpp | 20 +++++++++++++++----- src/modules/commander/Commander.hpp | 2 +- src/modules/commander/commander_params.c | 4 ++-- 3 files changed, 18 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index cf9f5f20e9..525c9fedbd 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1961,14 +1961,24 @@ Commander::run() /* ESCs status changed */ esc_status_check(); - } else if (hrt_elapsed_time(&_last_esc_status_updated) > 700_ms) { - // Some DShot ESCs are unresponsive for ~550ms during their initialization, so we use a timeout higher than that + } else if (_param_escs_checks_required.get() != 0) { if (!_status_flags.condition_escs_error) { - mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry timeout"); - } - _status_flags.condition_escs_error = true; + 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"); + _status_flags.condition_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 "); + _status_flags.condition_escs_error = true; + } + } } estimator_check(); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 6e0edb7592..c95313a22f 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -333,7 +333,7 @@ private: hrt_abstime _high_latency_datalink_lost{0}; int _last_esc_online_flags{-1}; - int _last_esc_failure[esc_status_s::CONNECTED_ESC_MAX] {0}; + 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}; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 7fd900931a..cee01392ca 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -902,12 +902,12 @@ PARAM_DEFINE_INT32(COM_FLT_PROFILE, 0); * Enable checks on ESCs that report telemetry. * * If this parameter is set, the system will check ESC's online status and failures. - * This param is specific for ESCs reporting status. Normal ESCs configurations are not affected by the change of this param. + * This param is specific for ESCs reporting status. It shall be used only if ESCs support telemetry. * * @group Commander * @boolean */ -PARAM_DEFINE_INT32(COM_ARM_CHK_ESCS, 1); +PARAM_DEFINE_INT32(COM_ARM_CHK_ESCS, 0); /** * Condition to enter prearmed mode