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)
# FailureDetector status
uint8 FAILURE_NONE = 0
uint8 FAILURE_ROLL = 1 # (1 << 0)
uint8 FAILURE_PITCH = 2 # (1 << 1)
uint8 FAILURE_ALT = 4 # (1 << 2)
uint8 FAILURE_EXT = 8 # (1 << 3)
uint8 FAILURE_ARM_ESC = 16 # (1 << 4)
uint8 FAILURE_HIGH_WIND = 32 # (1 << 5)
uint8 FAILURE_BATTERY = 64 # (1 << 6)
uint8 FAILURE_IMBALANCED_PROP = 128 # (1 << 7)
uint8 failure_status # Bitmask containing FailureDetector status
bool fd_roll
bool fd_pitch
bool fd_alt
bool fd_ext
bool fd_arm_escs
bool fd_high_wind
bool fd_battery
bool fd_imbalanced_prop
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 */
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;
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
if (hrt_elapsed_time(&_status.armed_time) < 500_ms) {
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 |
vehicle_status_s::FAILURE_ALT | vehicle_status_s::FAILURE_EXT)) {
if (fd_status_flags.roll || fd_status_flags.pitch || fd_status_flags.alt || fd_status_flags.ext) {
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) {
@ -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) {
_status_changed = true;
_imbalanced_propeller_check_triggered = true;
@ -2843,7 +2843,14 @@ Commander::run()
/* publish failure_detector data */
failure_detector_status_s fd_status{};
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();
_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)
{
uint8_t previous_status = _status;
failure_detector_status_u status_prev = _status;
if (vehicle_control_mode.flag_control_attitude_enabled) {
updateAttitudeStatus();
@ -59,7 +59,10 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
}
} 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()) {
@ -70,7 +73,7 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
updateImbalancedPropStatus();
}
return _status != previous_status;
return _status.value != status_prev.value;
}
void FailureDetector::updateAttitudeStatus()
@ -99,16 +102,9 @@ void FailureDetector::updateAttitudeStatus()
_roll_failure_hysteresis.set_state_and_update(roll_status, time_now);
_pitch_failure_hysteresis.set_state_and_update(pitch_status, time_now);
// Update bitmask
_status &= ~(FAILURE_ROLL | FAILURE_PITCH);
if (_roll_failure_hysteresis.get_state()) {
_status |= FAILURE_ROLL;
}
if (_pitch_failure_hysteresis.get_state()) {
_status |= FAILURE_PITCH;
}
// Update status
_status.flags.roll = _roll_failure_hysteresis.get_state();
_status.flags.pitch = _pitch_failure_hysteresis.get_state();
}
}
@ -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_state_and_update(ats_trigger_status, time_now);
_status &= ~FAILURE_EXT;
if (_ext_ats_failure_hysteresis.get_state()) {
_status |= FAILURE_EXT;
}
_status.flags.ext = _ext_ats_failure_hysteresis.get_state();
}
}
@ -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);
if (_esc_failure_hysteresis.get_state()) {
_status |= FAILURE_ARM_ESCS;
_status.flags.arm_escs = true;
}
}
} else {
// reset ESC bitfield
_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 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/pwm_input.h>
typedef enum {
FAILURE_NONE = vehicle_status_s::FAILURE_NONE,
FAILURE_ROLL = vehicle_status_s::FAILURE_ROLL,
FAILURE_PITCH = vehicle_status_s::FAILURE_PITCH,
FAILURE_ALT = vehicle_status_s::FAILURE_ALT,
FAILURE_EXT = vehicle_status_s::FAILURE_EXT,
FAILURE_ARM_ESCS = vehicle_status_s::FAILURE_ARM_ESC,
FAILURE_HIGH_WIND = vehicle_status_s::FAILURE_HIGH_WIND,
FAILURE_BATTERY = vehicle_status_s::FAILURE_BATTERY,
FAILURE_IMBALANCED_PROP = vehicle_status_s::FAILURE_IMBALANCED_PROP
} failure_detector_bitmak;
union failure_detector_status_u {
struct {
uint16_t roll : 1;
uint16_t pitch : 1;
uint16_t alt : 1;
uint16_t ext : 1;
uint16_t arm_escs : 1;
uint16_t high_wind : 1;
uint16_t battery : 1;
uint16_t imbalanced_prop : 1;
} flags;
uint16_t value;
};
using uORB::SubscriptionData;
@ -80,7 +82,8 @@ public:
FailureDetector(ModuleParams *parent);
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(); }
private:
@ -89,7 +92,7 @@ private:
void updateEscsStatus(const vehicle_status_s &vehicle_status);
void updateImbalancedPropStatus();
uint8_t _status{FAILURE_NONE};
failure_detector_status_u _status{};
systemlib::Hysteresis _roll_failure_hysteresis{false};
systemlib::Hysteresis _pitch_failure_hysteresis{false};