From be300b767d779f237a6448ea980a503eae06d6a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 19 Nov 2024 11:37:35 +0100 Subject: [PATCH] commander: ensure health report is always sent out before failsafe notificaation As the failsafe message can reference the health report, the health report needs to be sent out first. This is generally the case, except there is a rate limiter set to 2 seconds. So if the report changes quickly, it is sent out delayed (potentially after the failsafe report). --- src/modules/commander/Commander.cpp | 16 ++++++++++++++++ src/modules/commander/Commander.hpp | 3 +++ .../commander/HealthAndArmingChecks/Common.cpp | 9 +++++++++ .../commander/HealthAndArmingChecks/Common.hpp | 5 +++++ .../HealthAndArmingChecks.cpp | 5 +++++ .../HealthAndArmingChecks.hpp | 2 ++ src/modules/commander/failsafe/framework.cpp | 4 ++++ src/modules/commander/failsafe/framework.h | 14 ++++++++++++++ 8 files changed, 58 insertions(+) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 642973dda4..8e41b2ac56 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -716,6 +716,8 @@ Commander::Commander() : } updateParameters(); + + _failsafe.setOnNotifyUserCallback(&Commander::onFailsafeNotifyUserTrampoline, this); } Commander::~Commander() @@ -3000,6 +3002,20 @@ void Commander::send_parachute_command() set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE); } +void Commander::onFailsafeNotifyUserTrampoline(void *arg) +{ + Commander *commander = static_cast(arg); + commander->onFailsafeNotifyUser(); +} + +void Commander::onFailsafeNotifyUser() +{ + // If we are about to inform about a failsafe, we need to ensure any pending health report is sent out first, + // as the failsafe message might reference that. This is only needed in case the report is currently rate-limited, + // i.e. it had a recent previous change already. + _health_and_arming_checks.reportIfUnreportedDifferences(); +} + int Commander::print_usage(const char *reason) { if (reason) { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 6dddf0777f..e5bdb947aa 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -200,6 +200,9 @@ private: void modeManagementUpdate(); + static void onFailsafeNotifyUserTrampoline(void *arg); + void onFailsafeNotifyUser(); + enum class PrearmedMode { DISABLED = 0, SAFETY_BUTTON = 1, diff --git a/src/modules/commander/HealthAndArmingChecks/Common.cpp b/src/modules/commander/HealthAndArmingChecks/Common.cpp index 796427180f..f8d3982c5b 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.cpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.cpp @@ -317,3 +317,12 @@ bool Report::report(bool is_armed, bool force) current_results.health.error, current_results.health.warning); return true; } + +bool Report::reportIfUnreportedDifferences(bool is_armed) +{ + if (_had_unreported_difference) { + return report(is_armed, true); + } + + return false; +} diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index 7fb454cbeb..3e6a06248d 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -339,6 +339,11 @@ private: bool report(bool is_armed, bool force); + /** + * Send out any unreported changes if there are any + */ + bool reportIfUnreportedDifferences(bool is_armed); + const hrt_abstime _min_reporting_interval; /// event buffer: stores current events + arguments. diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp index dbfea11ce8..2ab153ad8a 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp @@ -120,3 +120,8 @@ void HealthAndArmingChecks::updateParams() _checks[i]->updateParams(); } } + +bool HealthAndArmingChecks::reportIfUnreportedDifferences() +{ + return _reporter.reportIfUnreportedDifferences(_context.isArmed()); +} diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index f0dc8404f2..d04b8d088e 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -88,6 +88,8 @@ public: */ bool update(bool force_reporting = false, bool is_arming_request = false); + bool reportIfUnreportedDifferences(); + /** * Whether arming is possible for a given navigation mode */ diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index 6c0edb58c4..8d633e9f21 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -170,6 +170,10 @@ void FailsafeBase::removeActions(ClearCondition condition) void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action delayed_action, Cause cause) { + if (_on_notify_user_cb) { + _on_notify_user_cb(_on_notify_user_arg); + } + int delay_s = (_current_delay + 500_ms) / 1_s; PX4_DEBUG("User notification: failsafe triggered (action=%i, delayed_action=%i, cause=%i, delay=%is)", (int)action, (int)delayed_action, (int)cause, delay_s); diff --git a/src/modules/commander/failsafe/framework.h b/src/modules/commander/failsafe/framework.h index 4cd3ac5202..a0ff26601b 100644 --- a/src/modules/commander/failsafe/framework.h +++ b/src/modules/commander/failsafe/framework.h @@ -165,6 +165,17 @@ public: bool getDeferFailsafes() const { return _defer_failsafes; } bool failsafeDeferred() const { return _failsafe_defer_started != 0; } + using UserCallback = void(*)(void *); + + /** + * Register a callback that is called before notifying the user. + */ + void setOnNotifyUserCallback(UserCallback callback, void *arg) + { + _on_notify_user_cb = callback; + _on_notify_user_arg = arg; + } + protected: enum class UserTakeoverAllowed { Always, ///< allow takeover (immediately) @@ -278,6 +289,9 @@ private: orb_advert_t _mavlink_log_pub{nullptr}; + UserCallback _on_notify_user_cb{nullptr}; + void *_on_notify_user_arg{nullptr}; + DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams, (ParamFloat) _param_com_fail_act_t );