fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 15:47: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
committed by Silvan Fuhrer
parent 70eecf6070
commit 2adc36b2af
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
@@ -51,7 +51,7 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
{
_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) {
updateAttitudeStatus(vehicle_status);
@@ -61,10 +61,10 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
}
} else {
_status.flags.roll = false;
_status.flags.pitch = false;
_status.flags.alt = false;
_status.flags.ext = false;
_failure_detector_status.flags.roll = false;
_failure_detector_status.flags.pitch = false;
_failure_detector_status.flags.alt = false;
_failure_detector_status.flags.ext = false;
}
// esc_status subscriber is shared between subroutines
@@ -86,7 +86,7 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
updateImbalancedPropStatus();
}
return _status.value != status_prev.value;
return _failure_detector_status.value != status_prev.value;
}
void FailureDetector::updateAttitudeStatus(const vehicle_status_s &vehicle_status)
@@ -132,8 +132,8 @@ void FailureDetector::updateAttitudeStatus(const vehicle_status_s &vehicle_statu
_pitch_failure_hysteresis.set_state_and_update(pitch_status, time_now);
// Update status
_status.flags.roll = _roll_failure_hysteresis.get_state();
_status.flags.pitch = _pitch_failure_hysteresis.get_state();
_failure_detector_status.flags.roll = _roll_failure_hysteresis.get_state();
_failure_detector_status.flags.pitch = _pitch_failure_hysteresis.get_state();
}
}
@@ -152,7 +152,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_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();
}
}
@@ -175,13 +175,13 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, c
_esc_failure_hysteresis.set_state_and_update(is_esc_failure, time_now);
if (_esc_failure_hysteresis.get_state()) {
_status.flags.arm_escs = true;
_failure_detector_status.flags.arm_escs = true;
}
} else {
// reset ESC bitfield
_esc_failure_hysteresis.set_state_and_update(false, time_now);
_status.flags.arm_escs = false;
_failure_detector_status.flags.arm_escs = false;
}
}
@@ -237,7 +237,7 @@ void FailureDetector::updateImbalancedPropStatus()
const float metric_lpf = _imbalanced_prop_lpf.update(metric);
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;
}
}
}
@@ -333,13 +333,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);
if (critical_esc_failure && !(_status.flags.motor)) {
if (critical_esc_failure && !(_failure_detector_status.flags.motor)) {
// 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
_status.flags.motor = false;
_failure_detector_status.flags.motor = false;
}
} else { // Disarmed
@@ -349,6 +349,6 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
}
_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
@@ -85,8 +85,8 @@ public:
~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 _status; }
const decltype(failure_detector_status_u::flags) &getStatusFlags() const { return _status.flags; }
const failure_detector_status_u &getStatus() const { return _failure_detector_status; }
const decltype(failure_detector_status_u::flags) &getStatusFlags() const { return _failure_detector_status.flags; }
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 getMotorStopMask() { return _failure_injector.getMotorStopMask(); }
@@ -98,7 +98,7 @@ private:
void updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status);
void updateImbalancedPropStatus();
failure_detector_status_u _status{};
failure_detector_status_u _failure_detector_status{};
systemlib::Hysteresis _roll_failure_hysteresis{false};
systemlib::Hysteresis _pitch_failure_hysteresis{false};