dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FD: use flags union instead of bitmask

This commit is contained in:
bresch 2021-11-03 11:13:37 +01:00 committed by Daniel Agar
parent 3f1025fb1e
commit 1e94512719
4 changed files with 50 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
msg/failure_detector_status.msg
View File

@ -1,16 +1,13 @@
uint64 timestamp # time since system start (microseconds) uint64 timestamp # time since system start (microseconds)
# FailureDetector status # FailureDetector status
uint8 FAILURE_NONE = 0 bool fd_roll
uint8 FAILURE_ROLL = 1 # (1 << 0) bool fd_pitch
uint8 FAILURE_PITCH = 2 # (1 << 1) bool fd_alt
uint8 FAILURE_ALT = 4 # (1 << 2) bool fd_ext
uint8 FAILURE_EXT = 8 # (1 << 3) bool fd_arm_escs
uint8 FAILURE_ARM_ESC = 16 # (1 << 4) bool fd_high_wind
uint8 FAILURE_HIGH_WIND = 32 # (1 << 5) bool fd_battery
uint8 FAILURE_BATTERY = 64 # (1 << 6) bool fd_imbalanced_prop
uint8 FAILURE_IMBALANCED_PROP = 128 # (1 << 7)
uint8 failure_status # Bitmask containing FailureDetector status
float32 imbalanced_prop_metric # Metric of the imbalanced propeller check (low-passed) float32 imbalanced_prop_metric # Metric of the imbalanced propeller check (low-passed)

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

@ -2607,11 +2607,12 @@ Commander::run()
/* Check for failure detector status */ /* Check for failure detector status */
if (_failure_detector.update(_status, _vehicle_control_mode)) { if (_failure_detector.update(_status, _vehicle_control_mode)) {
_status.failure_detector_status = _failure_detector.getStatus(); _status.failure_detector_status = _failure_detector.getStatus().value;
auto fd_status_flags = _failure_detector.getStatusFlags();
_status_changed = true; _status_changed = true;
if (_armed.armed) { if (_armed.armed) {
if (_status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) { if (fd_status_flags.arm_escs) {
// 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs // 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs
if (hrt_elapsed_time(&_status.armed_time) < 500_ms) { if (hrt_elapsed_time(&_status.armed_time) < 500_ms) {
disarm(arm_disarm_reason_t::failure_detector); disarm(arm_disarm_reason_t::failure_detector);
@ -2620,8 +2621,7 @@ Commander::run()
} }
} }
if (_status.failure_detector_status & (vehicle_status_s::FAILURE_ROLL | vehicle_status_s::FAILURE_PITCH | if (fd_status_flags.roll || fd_status_flags.pitch || fd_status_flags.alt || fd_status_flags.ext) {
vehicle_status_s::FAILURE_ALT | vehicle_status_s::FAILURE_EXT)) {
const bool is_right_after_takeoff = hrt_elapsed_time(&_status.takeoff_time) < (1_s * _param_com_lkdown_tko.get()); const bool is_right_after_takeoff = hrt_elapsed_time(&_status.takeoff_time) < (1_s * _param_com_lkdown_tko.get());
if (is_right_after_takeoff && !_lockdown_triggered) { if (is_right_after_takeoff && !_lockdown_triggered) {
@ -2661,7 +2661,7 @@ Commander::run()
} }
} }
if ((_status.failure_detector_status & vehicle_status_s::FAILURE_IMBALANCED_PROP) if (fd_status_flags.imbalanced_prop
&& !_imbalanced_propeller_check_triggered) { && !_imbalanced_propeller_check_triggered) {
_status_changed = true; _status_changed = true;
_imbalanced_propeller_check_triggered = true; _imbalanced_propeller_check_triggered = true;
@ -2843,7 +2843,14 @@ Commander::run()
/* publish failure_detector data */ /* publish failure_detector data */
failure_detector_status_s fd_status{}; failure_detector_status_s fd_status{};
fd_status.timestamp = hrt_absolute_time(); fd_status.timestamp = hrt_absolute_time();
fd_status.failure_status = _failure_detector.getStatus(); fd_status.fd_roll = _failure_detector.getStatusFlags().roll;
fd_status.fd_pitch = _failure_detector.getStatusFlags().pitch;
fd_status.fd_alt = _failure_detector.getStatusFlags().alt;
fd_status.fd_ext = _failure_detector.getStatusFlags().ext;
fd_status.fd_arm_escs = _failure_detector.getStatusFlags().arm_escs;
fd_status.fd_high_wind = _failure_detector.getStatusFlags().high_wind;
fd_status.fd_battery = _failure_detector.getStatusFlags().battery;
fd_status.fd_imbalanced_prop = _failure_detector.getStatusFlags().imbalanced_prop;
fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric(); fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric();
_failure_detector_status_pub.publish(fd_status); _failure_detector_status_pub.publish(fd_status);
} }

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

@ -49,7 +49,7 @@ FailureDetector::FailureDetector(ModuleParams *parent) :
bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode) bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode)
{ {
uint8_t previous_status = _status; failure_detector_status_u status_prev = _status;
if (vehicle_control_mode.flag_control_attitude_enabled) { if (vehicle_control_mode.flag_control_attitude_enabled) {
updateAttitudeStatus(); updateAttitudeStatus();
@ -59,7 +59,10 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
} }
} else { } else {
_status &= ~(FAILURE_ROLL | FAILURE_PITCH | FAILURE_ALT | FAILURE_EXT); _status.flags.roll = false;
_status.flags.pitch = false;
_status.flags.alt = false;
_status.flags.ext = false;
} }
if (_param_escs_en.get()) { if (_param_escs_en.get()) {
@ -70,7 +73,7 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
updateImbalancedPropStatus(); updateImbalancedPropStatus();
} }
return _status != previous_status; return _status.value != status_prev.value;
} }
void FailureDetector::updateAttitudeStatus() void FailureDetector::updateAttitudeStatus()
@ -99,16 +102,9 @@ void FailureDetector::updateAttitudeStatus()
_roll_failure_hysteresis.set_state_and_update(roll_status, time_now); _roll_failure_hysteresis.set_state_and_update(roll_status, time_now);
_pitch_failure_hysteresis.set_state_and_update(pitch_status, time_now); _pitch_failure_hysteresis.set_state_and_update(pitch_status, time_now);
// Update bitmask // Update status
_status &= ~(FAILURE_ROLL | FAILURE_PITCH); _status.flags.roll = _roll_failure_hysteresis.get_state();
_status.flags.pitch = _pitch_failure_hysteresis.get_state();
if (_roll_failure_hysteresis.get_state()) {
_status |= FAILURE_ROLL;
}
if (_pitch_failure_hysteresis.get_state()) {
_status |= FAILURE_PITCH;
}
} }
} }
@ -127,11 +123,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 &= ~FAILURE_EXT; _status.flags.ext = _ext_ats_failure_hysteresis.get_state();
if (_ext_ats_failure_hysteresis.get_state()) {
_status |= FAILURE_EXT;
}
} }
} }
@ -149,14 +141,14 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status)
_esc_failure_hysteresis.set_state_and_update(all_escs_armed != esc_status.esc_armed_flags, time_now); _esc_failure_hysteresis.set_state_and_update(all_escs_armed != esc_status.esc_armed_flags, time_now);
if (_esc_failure_hysteresis.get_state()) { if (_esc_failure_hysteresis.get_state()) {
_status |= FAILURE_ARM_ESCS; _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 &= ~FAILURE_ARM_ESCS; _status.flags.arm_escs = false;
} }
} }
@ -209,7 +201,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 = (_status & ~FAILURE_IMBALANCED_PROP) | (is_imbalanced * FAILURE_IMBALANCED_PROP); _status.flags.imbalanced_prop = is_imbalanced;
} }
} }
} }

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

@ -60,17 +60,19 @@
#include <uORB/topics/esc_status.h> #include <uORB/topics/esc_status.h>
#include <uORB/topics/pwm_input.h> #include <uORB/topics/pwm_input.h>
typedef enum { union failure_detector_status_u {
FAILURE_NONE = vehicle_status_s::FAILURE_NONE, struct {
FAILURE_ROLL = vehicle_status_s::FAILURE_ROLL, uint16_t roll : 1;
FAILURE_PITCH = vehicle_status_s::FAILURE_PITCH, uint16_t pitch : 1;
FAILURE_ALT = vehicle_status_s::FAILURE_ALT, uint16_t alt : 1;
FAILURE_EXT = vehicle_status_s::FAILURE_EXT, uint16_t ext : 1;
FAILURE_ARM_ESCS = vehicle_status_s::FAILURE_ARM_ESC, uint16_t arm_escs : 1;
FAILURE_HIGH_WIND = vehicle_status_s::FAILURE_HIGH_WIND, uint16_t high_wind : 1;
FAILURE_BATTERY = vehicle_status_s::FAILURE_BATTERY, uint16_t battery : 1;
FAILURE_IMBALANCED_PROP = vehicle_status_s::FAILURE_IMBALANCED_PROP uint16_t imbalanced_prop : 1;
} failure_detector_bitmak; } flags;
uint16_t value;
};
using uORB::SubscriptionData; using uORB::SubscriptionData;
@ -80,7 +82,8 @@ public:
FailureDetector(ModuleParams *parent); FailureDetector(ModuleParams *parent);
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);
uint8_t getStatus() const { return _status; } const failure_detector_status_u &getStatus() const { return _status; }
const decltype(failure_detector_status_u::flags) &getStatusFlags() const { return _status.flags; }
float getImbalancedPropMetric() const { return _imbalanced_prop_lpf.getState(); } float getImbalancedPropMetric() const { return _imbalanced_prop_lpf.getState(); }
private: private:
@ -89,7 +92,7 @@ private:
void updateEscsStatus(const vehicle_status_s &vehicle_status); void updateEscsStatus(const vehicle_status_s &vehicle_status);
void updateImbalancedPropStatus(); void updateImbalancedPropStatus();
uint8_t _status{FAILURE_NONE}; failure_detector_status_u _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};