Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

FailureDetector: refactor naming, member order

Browse Source
This commit is contained in:
Matthias Grob
2020-07-30 15:41:25 +02:00
parent 25cdb383c6
commit 2bb5917188
2 changed files with 20 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/failure_detector/FailureDetector.cpp
+4 -5
View File
@@ -78,7 +78,7 @@ FailureDetector::update(const vehicle_status_s &vehicle_status)
updated |= resetAttitudeStatus();
}
if (_sub_esc_status.updated()) {
if (_esc_status_sub.updated()) {
if (_param_escs_en.get()) {
updated |= updateEscsStatus(vehicle_status);
@@ -115,7 +115,7 @@ FailureDetector::updateAttitudeStatus()
bool updated(false);
vehicle_attitude_s attitude;
if (_sub_vehicule_attitude.update(&attitude)) {
if (_vehicule_attitude_sub.update(&attitude)) {
const matrix::Eulerf euler(matrix::Quatf(attitude.q));
const float roll(euler.phi());
@@ -160,7 +160,7 @@ FailureDetector::updateExternalAtsStatus()
pwm_input_s pwm_input;
bool updated(false);
if (_sub_pwm_input.update(&pwm_input)) {
if (_pwm_input_sub.update(&pwm_input)) {
uint32_t pulse_width = pwm_input.pulse_width;
bool ats_trigger_status = (pulse_width >= (uint32_t)_param_fd_ext_ats_trig.get()) && (pulse_width < 3_ms);
@@ -192,11 +192,10 @@ FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status)
if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
esc_status_s esc_status{};
_sub_esc_status.copy(&esc_status);
_esc_status_sub.copy(&esc_status);
int all_escs_armed = (1 << esc_status.esc_count) - 1;
_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
_esc_failure_hysteresis.set_state_and_update(all_escs_armed != esc_status.esc_armed_flags, time_now);
src/modules/commander/failure_detector/FailureDetector.hpp
+16 -20
View File
@@ -79,6 +79,22 @@ public:
bool isFailure() const { return _status != FAILURE_NONE; }
private:
bool resetAttitudeStatus();
bool isAttitudeStabilized(const vehicle_status_s &vehicle_status);
bool updateAttitudeStatus();
bool updateExternalAtsStatus();
bool updateEscsStatus(const vehicle_status_s &vehicle_status);
uint8_t _status{FAILURE_NONE};
systemlib::Hysteresis _roll_failure_hysteresis{false};
systemlib::Hysteresis _pitch_failure_hysteresis{false};
systemlib::Hysteresis _ext_ats_failure_hysteresis{false};
systemlib::Hysteresis _esc_failure_hysteresis{false};
uORB::Subscription _vehicule_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)};
DEFINE_PARAMETERS(
(ParamInt<px4::params::FD_FAIL_P>) _param_fd_fail_p,
@@ -88,25 +104,5 @@ private:
(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_ESCS_EN>) _param_escs_en
)
// Subscriptions
uORB::Subscription _sub_vehicule_attitude{ORB_ID(vehicle_attitude)};
uORB::Subscription _sub_esc_status{ORB_ID(esc_status)};
uORB::Subscription _sub_pwm_input{ORB_ID(pwm_input)};
uint8_t _status{FAILURE_NONE};
systemlib::Hysteresis _roll_failure_hysteresis{false};
systemlib::Hysteresis _pitch_failure_hysteresis{false};
systemlib::Hysteresis _ext_ats_failure_hysteresis{false};
systemlib::Hysteresis _esc_failure_hysteresis{false};
bool resetAttitudeStatus();
bool isAttitudeStabilized(const vehicle_status_s &vehicle_status);
bool updateAttitudeStatus();
bool updateExternalAtsStatus();
bool updateEscsStatus(const vehicle_status_s &vehicle_status);
};