commander: move ownership of vehicle_status_flags_s to HealthAndArmingChecks

src/modules/commander/HealthAndArmingChecks/Common.cpp

@@ -172,6 +172,7 @@ void Report::reset()
 	_results[_current_result].reset();
 	_next_buffer_idx = 0;
 	_buffer_overflowed = false;
+	_results_changed = false;
 }
 
 void Report::prepare(uint8_t vehicle_type)
@@ -186,16 +187,18 @@ NavModes Report::getModeGroup(uint8_t nav_state) const
 	return (NavModes)(1u << nav_state);
 }
 
-void Report::finalize()
+bool Report::finalize()
 {
 	_results[_current_result].arming_checks.valid = true;
 	_already_reported = false;
+	_results_changed = _results[0] != _results[1];
+	return _results_changed;
 }
 
 bool Report::report(bool is_armed, bool force)
 {
 	const hrt_abstime now = hrt_absolute_time();
-	const bool has_difference = _had_unreported_difference || _results[0] != _results[1];
+	const bool has_difference = _had_unreported_difference || _results_changed;
 
 	if (now - _last_report < _min_reporting_interval && !force) {
 		if (has_difference) {

src/modules/commander/HealthAndArmingChecks/Common.hpp

@@ -323,7 +323,10 @@ private:
 	 */
 	void reset();
 	void prepare(uint8_t vehicle_type);
-	void finalize();
+	/**
+	 * Called after all checks are run. Returns true if the results changed
+	 */
+	bool finalize();
 
 	bool report(bool is_armed, bool force);
 
@@ -337,6 +340,7 @@ private:
 
 	bool _already_reported{false};
 	bool _had_unreported_difference{false}; ///< true if there was a difference not reported yet (due to rate limitation)
+	bool _results_changed{false};
 	hrt_abstime _last_report{0};
 
 	Results _results[2]; ///< Previous and current results to check for changes

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp

@@ -33,11 +33,10 @@
 
 #include "HealthAndArmingChecks.hpp"
 
-HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_status_flags_s &status_flags,
-		vehicle_status_s &status)
+HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_status_s &status)
 	: ModuleParams(parent),
 	  _context(status),
-	  _reporter(status_flags)
+	  _reporter(_failsafe_flags)
 {
 	// Initialize mode requirements to invalid
 	_failsafe_flags.angular_velocity_invalid = true;
@@ -67,9 +66,10 @@ bool HealthAndArmingChecks::update(bool force_reporting)
 		_checks[i]->checkAndReport(_context, _reporter);
 	}
 
-	_reporter.finalize();
+	const bool results_changed = _reporter.finalize();
+	const bool reported = _reporter.report(_context.isArmed(), force_reporting);
 
-	if (_reporter.report(_context.isArmed(), force_reporting)) {
+	if (reported) {
 
 		// LEGACY start
 		// Run the checks again, this time with the mavlink publication set.
@@ -97,10 +97,17 @@ bool HealthAndArmingChecks::update(bool force_reporting)
 		_reporter.getHealthReport(health_report);
 		health_report.timestamp = hrt_absolute_time();
 		_health_report_pub.publish(health_report);
-		return true;
 	}
 
-	return false;
+	// Check if we need to publish the failsafe flags
+	const hrt_abstime now = hrt_absolute_time();
+
+	if (now - _failsafe_flags.timestamp > 500_ms || results_changed) {
+		_failsafe_flags.timestamp = now;
+		_failsafe_flags_pub.publish(_failsafe_flags);
+	}
+
+	return reported;
 }
 
 void HealthAndArmingChecks::updateParams()

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp

@@ -38,6 +38,7 @@
 #include <px4_platform_common/module_params.h>
 #include <uORB/Publication.hpp>
 #include <uORB/topics/health_report.h>
+#include <uORB/topics/vehicle_status_flags.h>
 
 #include "checks/accelerometerCheck.hpp"
 #include "checks/airspeedCheck.hpp"
@@ -71,7 +72,7 @@
 class HealthAndArmingChecks : public ModuleParams
 {
 public:
-	HealthAndArmingChecks(ModuleParams *parent, vehicle_status_flags_s &status_flags, vehicle_status_s &status);
+	HealthAndArmingChecks(ModuleParams *parent, vehicle_status_s &status);
 	~HealthAndArmingChecks() = default;
 
 	/**
@@ -97,6 +98,8 @@ public:
 	 */
 	bool modePreventsArming(uint8_t nav_state) const { return _reporter.modePreventsArming(nav_state); }
 
+	const vehicle_status_flags_s &failsafeFlags() const { return _failsafe_flags; }
+
 protected:
 	void updateParams() override;
 private:
@@ -104,7 +107,10 @@ private:
 	Report _reporter;
 	orb_advert_t _mavlink_log_pub{nullptr};
 
+	vehicle_status_flags_s _failsafe_flags{};
+
 	uORB::Publication<health_report_s> _health_report_pub{ORB_ID(health_report)};
+	uORB::Publication<vehicle_status_flags_s> _failsafe_flags_pub{ORB_ID(vehicle_status_flags)};
 
 	// all checks
 	AccelerometerChecks _accelerometer_checks;