commander: ensure health report is always sent out before failsafe no…

…tificaation

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 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 
 	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 @@ class Report
 
 	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 @@ class HealthAndArmingChecks : public ModuleParams
 	 */
 	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 @@ class FailsafeBase: public ModuleParams
 	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 @@ class FailsafeBase: public ModuleParams
 
 	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
 				       );