From 6fda555cba4361d98d8a3ac382cb3a9d1e9516f8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 6 Sep 2022 09:24:00 +0200 Subject: [PATCH] commander: move ownership of vehicle_status_flags_s to HealthAndArmingChecks --- .../ArmStateMachine/ArmStateMachineTest.cpp | 3 +-- src/modules/commander/Commander.cpp | 4 ---- src/modules/commander/Commander.hpp | 7 +++---- .../HealthAndArmingChecks/Common.cpp | 7 +++++-- .../HealthAndArmingChecks/Common.hpp | 6 +++++- .../HealthAndArmingChecks.cpp | 21 ++++++++++++------- .../HealthAndArmingChecks.hpp | 8 ++++++- .../microbench/test_microbench_uorb.cpp | 14 ++++++------- 8 files changed, 42 insertions(+), 28 deletions(-) diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp index 2867efc149..f141f2ddcf 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp @@ -242,7 +242,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) }; struct vehicle_status_s status {}; - struct vehicle_status_flags_s status_flags {}; struct actuator_armed_s armed {}; size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]); @@ -254,7 +253,7 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) arm_state_machine.forceArmState(test->current_state.arming_state); status.hil_state = test->hil_state; - HealthAndArmingChecks health_and_arming_checks(nullptr, status_flags, status); + HealthAndArmingChecks health_and_arming_checks(nullptr, status); // Attempt transition transition_result_t result = arm_state_machine.arming_state_transition( diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 69334360fa..f39dae63d6 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1805,10 +1805,6 @@ Commander::run() _vehicle_status.timestamp = hrt_absolute_time(); _vehicle_status_pub.publish(_vehicle_status); - // publish vehicle_status_flags (after prearm/preflight updates above) - _vehicle_status_flags.timestamp = hrt_absolute_time(); - _vehicle_status_flags_pub.publish(_vehicle_status_flags); - // failure_detector_status publish failure_detector_status_s fd_status{}; fd_status.fd_roll = _failure_detector.getStatusFlags().roll; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index d0227f891c..90dd689c0d 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -58,7 +58,6 @@ #include #include #include -#include // subscriptions #include @@ -277,7 +276,6 @@ private: actuator_armed_s _actuator_armed{}; vehicle_control_mode_s _vehicle_control_mode{}; vehicle_status_s _vehicle_status{}; - vehicle_status_flags_s _vehicle_status_flags{}; Safety _safety; @@ -309,7 +307,6 @@ private: uORB::Publication _failure_detector_status_pub{ORB_ID(failure_detector_status)}; uORB::Publication _actuator_test_pub{ORB_ID(actuator_test)}; uORB::Publication _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)}; - uORB::Publication _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)}; uORB::Publication _vehicle_status_pub{ORB_ID(vehicle_status)}; uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; @@ -319,7 +316,9 @@ private: perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _preflight_check_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": preflight check")}; - HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status_flags, _vehicle_status}; + HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status}; + const vehicle_status_flags_s &_vehicle_status_flags{_health_and_arming_checks.failsafeFlags()}; + HomePosition _home_position{_vehicle_status_flags}; Failsafe _failsafe_instance{this}; FailsafeBase &_failsafe{_failsafe_instance}; diff --git a/src/modules/commander/HealthAndArmingChecks/Common.cpp b/src/modules/commander/HealthAndArmingChecks/Common.cpp index 91b1fc3a16..9849944744 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.cpp +++ b/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) { diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index b976cf5cd3..519857c557 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/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 diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp index c4088173ea..038673d82a 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp +++ b/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() diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index 3ef959ccb4..ccb089b637 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -38,6 +38,7 @@ #include #include #include +#include #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_pub{ORB_ID(health_report)}; + uORB::Publication _failsafe_flags_pub{ORB_ID(vehicle_status_flags)}; // all checks AccelerometerChecks _accelerometer_checks; diff --git a/src/systemcmds/microbench/test_microbench_uorb.cpp b/src/systemcmds/microbench/test_microbench_uorb.cpp index 02c7cdf303..d198b3b303 100644 --- a/src/systemcmds/microbench/test_microbench_uorb.cpp +++ b/src/systemcmds/microbench/test_microbench_uorb.cpp @@ -52,7 +52,7 @@ #include #include #include -#include +#include namespace MicroBenchORB { @@ -105,7 +105,7 @@ private: void reset(); - vehicle_status_s status; + vehicle_status_flags_s status; vehicle_local_position_s lpos; sensor_gyro_s gyro; sensor_gyro_fifo_s gyro_fifo; @@ -146,7 +146,7 @@ ut_declare_test_c(test_microbench_uorb, MicroBenchORB) bool MicroBenchORB::time_px4_uorb() { - int fd_status = orb_subscribe(ORB_ID(vehicle_status)); + int fd_status = orb_subscribe(ORB_ID(vehicle_status_flags)); int fd_lpos = orb_subscribe(ORB_ID(vehicle_local_position)); int fd_gyro = orb_subscribe(ORB_ID(sensor_gyro)); int fd_gyro_fifo = orb_subscribe(ORB_ID(sensor_gyro_fifo)); @@ -155,7 +155,7 @@ bool MicroBenchORB::time_px4_uorb() bool updated = false; PERF("orb_check vehicle_status", ret = orb_check(fd_status, &updated), 100); - PERF("orb_copy vehicle_status", ret = orb_copy(ORB_ID(vehicle_status), fd_status, &status), 100); + PERF("orb_copy vehicle_status", ret = orb_copy(ORB_ID(vehicle_status_flags), fd_status, &status), 100); printf("\n"); @@ -198,9 +198,9 @@ bool MicroBenchORB::time_px4_uorb_direct() { bool ret = false; - uORB::Subscription vstatus{ORB_ID(vehicle_status)}; - PERF("uORB::Subscription orb_check vehicle_status", ret = vstatus.updated(), 100); - PERF("uORB::Subscription orb_copy vehicle_status", ret = vstatus.copy(&status), 100); + uORB::Subscription vehicle_status_flags{ORB_ID(vehicle_status_flags)}; + PERF("uORB::Subscription orb_check vehicle_status", ret = vehicle_status_flags.updated(), 100); + PERF("uORB::Subscription orb_copy vehicle_status", ret = vehicle_status_flags.copy(&status), 100); printf("\n");