fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: avoid leaking health checks into failure detector

Browse Source
This commit is contained in:
Matthias Grob 2026-02-20 17:32:21 +01:00
parent 270ad06e5f
commit 7207c34c5b
4 changed files with 12 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/commander/Commander.cpp
View File

@ -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();

2
src/modules/commander/Commander.hpp
View File

@ -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{};

13
src/modules/commander/failure_detector/FailureDetector.cpp
View File

@ -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);

12
src/modules/commander/failure_detector/FailureDetector.hpp
View File

@ -48,7 +48,6 @@
#include <lib/mathlib/mathlib.h>
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
#include <matrix/matrix/math.hpp>
#include <mixer_module/output_functions.hpp>
#include <px4_platform_common/module_params.h>
// 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<px4::params::FD_FAIL_P_TTRI>) _param_fd_fail_p_ttri,
(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr)
(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr
)
};