diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index 702e8f5f4d..e2b42c0580 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include @@ -195,6 +196,8 @@ public: vehicle_status_flags_s &failsafeFlags() { return _status_flags; } + orb_advert_t *mavlink_log_pub() { return _mavlink_log_pub; } + /** * Whether arming is possible for a given navigation mode */ @@ -333,6 +336,8 @@ private: int _current_result{0}; vehicle_status_flags_s &_status_flags; + + orb_advert_t *_mavlink_log_pub{nullptr}; ///< mavlink log publication for legacy reporting }; template diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp index 66c3a098a5..229276d9a5 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp @@ -57,6 +57,26 @@ bool HealthAndArmingChecks::update(bool force_reporting) if (_reporter.report(_context.isArmed(), force_reporting)) { + // LEGACY start + // Run the checks again, this time with the mavlink publication set. + // We don't expect any change, and rate limitation would prevent the events from being reported again, + // so we only report mavlink_log_*. + _reporter._mavlink_log_pub = &_mavlink_log_pub; + _reporter.reset(); + + for (unsigned i = 0; i < sizeof(_checks) / sizeof(_checks[0]); ++i) { + if (!_checks[i]) { + break; + } + + _checks[i]->checkAndReport(_context, _reporter); + } + + _reporter.finalize(); + _reporter.report(_context.isArmed(), false); + _reporter._mavlink_log_pub = nullptr; + // LEGACY end + health_report_s health_report; _reporter.getHealthReport(health_report); health_report.timestamp = hrt_absolute_time(); diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index 0493a7f576..b001e03a45 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -63,6 +63,7 @@ protected: private: Context _context; Report _reporter; + orb_advert_t _mavlink_log_pub{nullptr}; uORB::Publication _health_report_pub{ORB_ID(health_report)};