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 );