fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-08 18:30:36 +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-03 11:13:37 +01: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/Commander.cpp
+13 -6
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);
}