diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 1ace6e22d4..0d30a8895c 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2016,7 +2016,7 @@ void Commander::run() _vehicle_status.timestamp = hrt_absolute_time(); _vehicle_status_pub.publish(_vehicle_status); - _failure_detector.publishStatus(); + _failure_detector.publishStatus(_health_and_arming_checks.getEscArmStatus(), _health_and_arming_checks.getMotorFailureMask()); } checkWorkerThread(); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 6a989d9f96..ff6ed018be 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -224,7 +224,7 @@ private: Failsafe _failsafe_instance{this}; FailsafeBase &_failsafe{_failsafe_instance}; - FailureDetector _failure_detector{this, _health_and_arming_checks}; + FailureDetector _failure_detector{this}; HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status}; MulticopterThrowLaunch _multicopter_throw_launch{this}; Safety _safety{}; diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index 8aa6df16bf..c4d6bd9692 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -43,9 +43,8 @@ using namespace time_literals; -FailureDetector::FailureDetector(ModuleParams *parent, HealthAndArmingChecks &health_and_arming_checks) : - ModuleParams(parent), - _health_and_arming_checks(health_and_arming_checks) +FailureDetector::FailureDetector(ModuleParams *parent) : + ModuleParams(parent) { } @@ -76,19 +75,19 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic return _failure_detector_status.value != status_prev.value; } -void FailureDetector::publishStatus() +void FailureDetector::publishStatus(bool esc_arm_status, uint16_t motor_failure_mask) { failure_detector_status_s failure_detector_status{}; failure_detector_status.fd_roll = _failure_detector_status.flags.roll; failure_detector_status.fd_pitch = _failure_detector_status.flags.pitch; failure_detector_status.fd_alt = _failure_detector_status.flags.alt; failure_detector_status.fd_ext = _failure_detector_status.flags.ext; - failure_detector_status.fd_arm_escs = _health_and_arming_checks.getEscArmStatus() || (_health_and_arming_checks.getMotorFailureMask() != 0); + failure_detector_status.fd_arm_escs = esc_arm_status || (motor_failure_mask != 0); failure_detector_status.fd_battery = _failure_detector_status.flags.battery; failure_detector_status.fd_imbalanced_prop = _failure_detector_status.flags.imbalanced_prop; - failure_detector_status.fd_motor = (_health_and_arming_checks.getMotorFailureMask() != 0) || (_failure_injector.getMotorStopMask() != 0); + failure_detector_status.fd_motor = (motor_failure_mask != 0) || (_failure_injector.getMotorStopMask() != 0); failure_detector_status.imbalanced_prop_metric = _imbalanced_prop_lpf.getState(); - failure_detector_status.motor_failure_mask = _health_and_arming_checks.getMotorFailureMask(); + failure_detector_status.motor_failure_mask = motor_failure_mask; failure_detector_status.motor_stop_mask = _failure_injector.getMotorStopMask(); failure_detector_status.timestamp = hrt_absolute_time(); _failure_detector_status_pub.publish(failure_detector_status); diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index 6ecb782931..75aed5bc40 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -48,7 +48,6 @@ #include #include #include -#include #include // subscriptions @@ -79,18 +78,16 @@ union failure_detector_status_u { using uORB::SubscriptionData; -class HealthAndArmingChecks; - class FailureDetector : public ModuleParams { public: - FailureDetector(ModuleParams *parent, HealthAndArmingChecks &health_and_arming_checks); + FailureDetector(ModuleParams *parent); ~FailureDetector() = default; bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode); const failure_detector_status_u &getStatus() const { return _failure_detector_status; } - void publishStatus(); + void publishStatus(bool esc_arm_status, uint16_t motor_failure_mask); private: void updateAttitudeStatus(const vehicle_status_s &vehicle_status); @@ -99,8 +96,6 @@ private: failure_detector_status_u _failure_detector_status{}; - HealthAndArmingChecks &_health_and_arming_checks; - systemlib::Hysteresis _roll_failure_hysteresis{false}; systemlib::Hysteresis _pitch_failure_hysteresis{false}; systemlib::Hysteresis _ext_ats_failure_hysteresis{false}; @@ -127,5 +122,6 @@ private: (ParamFloat) _param_fd_fail_p_ttri, (ParamBool) _param_fd_ext_ats_en, (ParamInt) _param_fd_ext_ats_trig, - (ParamInt) _param_fd_imb_prop_thr) + (ParamInt) _param_fd_imb_prop_thr + ) };