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

@@ -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<Commander *>(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) {

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,

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;
+}

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.

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp

@@ -120,3 +120,8 @@ void HealthAndArmingChecks::updateParams()
 		_checks[i]->updateParams();
 	}
 }
+
+bool HealthAndArmingChecks::reportIfUnreportedDifferences()
+{
+	return _reporter.reportIfUnreportedDifferences(_context.isArmed());
+}

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
 	 */

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

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<px4::params::COM_FAIL_ACT_T>) 	_param_com_fail_act_t
 				       );