fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 22:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FD: use flags union instead of bitmask

Browse Source
This commit is contained in:
bresch
2021-11-05 09:45:52 -04:00
committed by Daniel Agar
parent 3f1025fb1e
commit 1e94512719
4 changed files with 50 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/failure_detector/FailureDetector.cpp
+13 -21
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;
}
}
}
src/modules/commander/failure_detector/FailureDetector.hpp
+16 -13
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};