fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 01:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FailureDetector: write out status veriable name for clarity

Browse Source
This commit is contained in:
Matthias Grob
2025-05-15 16:52:07 +02:00
parent 70784e83d7
commit 8eb57d4f5b
2 changed files with 20 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/failure_detector/FailureDetector.cpp
+17 -17
View File
@@ -168,7 +168,7 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
{ {
_failure_injector.update(); _failure_injector.update();
failure_detector_status_u status_prev = _status; failure_detector_status_u status_prev = _failure_detector_status;
if (vehicle_control_mode.flag_control_attitude_enabled) { if (vehicle_control_mode.flag_control_attitude_enabled) {
updateAttitudeStatus(vehicle_status); updateAttitudeStatus(vehicle_status);
@@ -178,10 +178,10 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
} }
} else { } else {
_status.flags.roll = false; _failure_detector_status.flags.roll = false;
_status.flags.pitch = false; _failure_detector_status.flags.pitch = false;
_status.flags.alt = false; _failure_detector_status.flags.alt = false;
_status.flags.ext = false; _failure_detector_status.flags.ext = false;
} }
// esc_status subscriber is shared between subroutines // esc_status subscriber is shared between subroutines
@@ -203,7 +203,7 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
updateImbalancedPropStatus(); updateImbalancedPropStatus();
} }
return _status.value != status_prev.value; return _failure_detector_status.value != status_prev.value;
} }
void FailureDetector::updateAttitudeStatus(const vehicle_status_s &vehicle_status) void FailureDetector::updateAttitudeStatus(const vehicle_status_s &vehicle_status)
@@ -249,8 +249,8 @@ void FailureDetector::updateAttitudeStatus(const vehicle_status_s &vehicle_statu
_pitch_failure_hysteresis.set_state_and_update(pitch_status, time_now); _pitch_failure_hysteresis.set_state_and_update(pitch_status, time_now);
// Update status // Update status
_status.flags.roll = _roll_failure_hysteresis.get_state(); _failure_detector_status.flags.roll = _roll_failure_hysteresis.get_state();
_status.flags.pitch = _pitch_failure_hysteresis.get_state(); _failure_detector_status.flags.pitch = _pitch_failure_hysteresis.get_state();
} }
} }
@@ -269,7 +269,7 @@ void FailureDetector::updateExternalAtsStatus()
_ext_ats_failure_hysteresis.set_hysteresis_time_from(false, 100_ms); // 5 consecutive pulses at 50hz _ext_ats_failure_hysteresis.set_hysteresis_time_from(false, 100_ms); // 5 consecutive pulses at 50hz
_ext_ats_failure_hysteresis.set_state_and_update(ats_trigger_status, time_now); _ext_ats_failure_hysteresis.set_state_and_update(ats_trigger_status, time_now);
_status.flags.ext = _ext_ats_failure_hysteresis.get_state(); _failure_detector_status.flags.ext = _ext_ats_failure_hysteresis.get_state();
} }
} }
@@ -292,13 +292,13 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, c
_esc_failure_hysteresis.set_state_and_update(is_esc_failure, time_now); _esc_failure_hysteresis.set_state_and_update(is_esc_failure, time_now);
if (_esc_failure_hysteresis.get_state()) { if (_esc_failure_hysteresis.get_state()) {
_status.flags.arm_escs = true; _failure_detector_status.flags.arm_escs = true;
} }
} else { } else {
// reset ESC bitfield // reset ESC bitfield
_esc_failure_hysteresis.set_state_and_update(false, time_now); _esc_failure_hysteresis.set_state_and_update(false, time_now);
_status.flags.arm_escs = false; _failure_detector_status.flags.arm_escs = false;
} }
} }
@@ -354,7 +354,7 @@ void FailureDetector::updateImbalancedPropStatus()
const float metric_lpf = _imbalanced_prop_lpf.update(metric); const float metric_lpf = _imbalanced_prop_lpf.update(metric);
const bool is_imbalanced = metric_lpf > _param_fd_imb_prop_thr.get(); const bool is_imbalanced = metric_lpf > _param_fd_imb_prop_thr.get();
_status.flags.imbalanced_prop = is_imbalanced; _failure_detector_status.flags.imbalanced_prop = is_imbalanced;
} }
} }
} }
@@ -453,13 +453,13 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
bool critical_esc_failure = (_motor_failure_esc_timed_out_mask != 0 || _motor_failure_esc_under_current_mask != 0); bool critical_esc_failure = (_motor_failure_esc_timed_out_mask != 0 || _motor_failure_esc_under_current_mask != 0);
if (critical_esc_failure && !(_status.flags.motor)) { if (critical_esc_failure && !(_failure_detector_status.flags.motor)) {
// Add motor failure flag to bitfield // Add motor failure flag to bitfield
_status.flags.motor = true; _failure_detector_status.flags.motor = true;
} else if (!critical_esc_failure && _status.flags.motor) { } else if (!critical_esc_failure && _failure_detector_status.flags.motor) {
// Reset motor failure flag // Reset motor failure flag
_status.flags.motor = false; _failure_detector_status.flags.motor = false;
} }
} else { // Disarmed } else { // Disarmed
@@ -469,6 +469,6 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
} }
_motor_failure_esc_under_current_mask = 0; _motor_failure_esc_under_current_mask = 0;
_status.flags.motor = false; _failure_detector_status.flags.motor = false;
} }
} }
src/modules/commander/failure_detector/FailureDetector.hpp
+3 -3
View File
@@ -101,8 +101,8 @@ public:
~FailureDetector() = default; ~FailureDetector() = default;
bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode); bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode);
const failure_detector_status_u &getStatus() const { return _status; } const failure_detector_status_u &getStatus() const { return _failure_detector_status; }
const decltype(failure_detector_status_u::flags) &getStatusFlags() const { return _status.flags; } const decltype(failure_detector_status_u::flags) &getStatusFlags() const { return _failure_detector_status.flags; }
float getImbalancedPropMetric() const { return _imbalanced_prop_lpf.getState(); } float getImbalancedPropMetric() const { return _imbalanced_prop_lpf.getState(); }
uint16_t getMotorFailures() const { return _motor_failure_esc_timed_out_mask | _motor_failure_esc_under_current_mask; } uint16_t getMotorFailures() const { return _motor_failure_esc_timed_out_mask | _motor_failure_esc_under_current_mask; }
@@ -113,7 +113,7 @@ private:
void updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status); void updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status);
void updateImbalancedPropStatus(); void updateImbalancedPropStatus();
failure_detector_status_u _status{}; failure_detector_status_u _failure_detector_status{};
systemlib::Hysteresis _roll_failure_hysteresis{false}; systemlib::Hysteresis _roll_failure_hysteresis{false};
systemlib::Hysteresis _pitch_failure_hysteresis{false}; systemlib::Hysteresis _pitch_failure_hysteresis{false};