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 642973dda41..8e41b2ac56d 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 6dddf0777f4..e5bdb947aac 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 796427180fe..f8d3982c5be 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 7fb454cbeb7..3e6a06248d7 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 dbfea11ce88..2ab153ad8a0 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 f0dc8404f29..d04b8d088ec 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 6c0edb58c45..8d633e9f215 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 4cd3ac5202c..a0ff26601be 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 );