feat(failure_detector)!: check tilt instead of roll and pitch separately

Checking roll and pitch against separate limits misses a combined
tilt: 45° roll with 45° pitch is a 60° tilt but stays below both
default 60° limits. Euler angles are also unreliable near 90° pitch
and when inverted.

FD_FAIL_R and FD_FAIL_R_TTRI now set the maximum tilt and its trigger
time. FD_FAIL_P and FD_FAIL_P_TTRI are removed without migration
(see the Upgrade Guide). The check is disabled with -1; 0 still
disables it for existing setups. In FailureDetectorStatus, fd_roll
and fd_pitch are replaced by fd_tilt.
This commit is contained in:
Matthias Grob committed 2026-11-18 11:02:27 +01:00
1 parent f1dae4865c
commit 7f83c28400
17 files changed
+48 -118

No files matched your search

@@ -92,5 +92,4 @@ param set-default PWM_MAIN_FUNC8 108
param set-default CA_METHOD 0
# disable attitude failure detection
param set-default FD_FAIL_P 0
param set-default FD_FAIL_R 0
param set-default FD_FAIL_R -1
@@ -299,9 +299,7 @@ param set-default RTL_DESCEND_ALT 150
param set-default RTL_MIN_DIST 50
param set-default FD_FAIL_R 85
param set-default FD_FAIL_P 85
param set-default FD_FAIL_R_TTRI 1.0
param set-default FD_FAIL_P_TTRI 1.0
param set-default FD_FAIL_R_TTRI 1
param set-default WV_EN 0
################################################################################
@@ -35,8 +35,7 @@ param set-default GF_ACTION 1
param set-default NAV_RCL_ACT 1
# disable attitude failure detection
param set-default FD_FAIL_P 0
param set-default FD_FAIL_R 0
param set-default FD_FAIL_R -1
param set-default CA_ROTOR_COUNT 8
param set-default CA_R_REV 255
@@ -30,8 +30,7 @@ param set-default SENS_EN_MAGSIM 0
param set-default COM_RC_IN_MODE 1
# disable attitude failure detection
param set-default FD_FAIL_P 0
param set-default FD_FAIL_R 0
param set-default FD_FAIL_R -1
# Disable magnetometer fusion in EKF2
param set-default EKF2_MAG_TYPE 5
@@ -30,8 +30,7 @@ param set-default SENS_EN_MAGSIM 0
param set-default COM_RC_IN_MODE 1
# disable attitude failure detection
param set-default FD_FAIL_P 0
param set-default FD_FAIL_R 0
param set-default FD_FAIL_R -1
# Disable magnetometer fusion in EKF2
param set-default EKF2_MAG_TYPE 5
@@ -114,5 +114,4 @@ param set-default SIM_GZ_EC_MAX8 1100
param set-default CA_METHOD 0
# disable attitude failure detection
param set-default FD_FAIL_P 0
param set-default FD_FAIL_R 0
param set-default FD_FAIL_R -1
@@ -50,8 +50,7 @@ param set-default GF_ACTION 1
param set-default NAV_RCL_ACT 1
# disable attitude failure detection
param set-default FD_FAIL_P 0
param set-default FD_FAIL_R 0
param set-default FD_FAIL_R -1
param set-default CA_ROTOR_COUNT 8
param set-default CA_R_REV 255
@@ -25,8 +25,7 @@ param set-default UXRCE_DDS_AG_IP -1062731775
param set-default UXRCE_DDS_CFG 1000
# disable attitude failure detection
param set-default FD_FAIL_P 0
param set-default FD_FAIL_R 0
param set-default FD_FAIL_R -1
# Disable magnetometer fusion in EKF2
param set-default EKF2_MAG_TYPE 5
@@ -29,8 +29,7 @@ param set-default UXRCE_DDS_AG_IP -1062731775
param set-default UXRCE_DDS_CFG 1000
# disable attitude failure detection
param set-default FD_FAIL_P 0
param set-default FD_FAIL_R 0
param set-default FD_FAIL_R -1
# Disable magnetometer fusion in EKF2
param set-default EKF2_MAG_TYPE 5
+2 -4
View File
@@ -448,16 +448,14 @@ The reference altitude depends on what the vehicle is commanded to do:
### Attitude Trigger
The failure detector can be configured to trigger if the vehicle attitude exceeds predefined pitch and roll values for longer than a specified time.
The failure detector can be configured to trigger if the vehicle tilt (the angle between the vehicle's vertical axis and the vertical, in any direction) exceeds a predefined value for longer than a specified time.
The relevant parameters are shown below:
| Parameter | Description |
| -------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------- |
| <a id="CBRK_FLIGHTTERM"></a>[CBRK_FLIGHTTERM](../advanced_config/parameter_reference.md#CBRK_FLIGHTTERM) | Flight termination circuit breaker. Unset from 121212 (default) to enable flight termination due to FailureDetector or FMU loss. |
| <a id="FD_FAIL_P"></a>[FD_FAIL_P](../advanced_config/parameter_reference.md#FD_FAIL_P) | Maximum allowed pitch (in degrees). |
| <a id="FD_FAIL_R"></a>[FD_FAIL_R](../advanced_config/parameter_reference.md#FD_FAIL_R) | Maximum allowed roll (in degrees). |
| <a id="FD_FAIL_P_TTRI"></a>[FD_FAIL_P_TTRI](../advanced_config/parameter_reference.md#FD_FAIL_P_TTRI) | Time to exceed [FD_FAIL_P](#FD_FAIL_P) for failure detection (default 0.3s). |
| <a id="FD_FAIL_R"></a>[FD_FAIL_R](../advanced_config/parameter_reference.md#FD_FAIL_R) | Maximum allowed tilt (in degrees). |
| <a id="FD_FAIL_R_TTRI"></a>[FD_FAIL_R_TTRI](../advanced_config/parameter_reference.md#FD_FAIL_R_TTRI) | Time to exceed [FD_FAIL_R](#FD_FAIL_R) for failure detection (default 0.3s). |
### Motor Failure Trigger
+1 -1
View File
@@ -114,7 +114,7 @@ Make sure the motors do not overheat with the changed settings.
Make sure to use the right props, as there is a CCW and a CW version.
- Parameters:
- Change the desaturation logic for better attitude tracking: set [CA_METHOD](../advanced_config/parameter_reference.md#CA_METHOD) to 0.
- Disable failure detection: set [FD_FAIL_P](../advanced_config/parameter_reference.md#FD_FAIL_P) and [FD_FAIL_R](../advanced_config/parameter_reference.md#FD_FAIL_R) to 0.
- Disable failure detection: set [FD_FAIL_R](../advanced_config/parameter_reference.md#FD_FAIL_R) to -1.
- [This file](https://github.com/PX4/PX4-Autopilot/raw/main/docs/assets/airframes/multicopter/omnicopter/omnicopter.params) contains all the relevant parameters.
## Video
+6
View File
@@ -88,6 +88,12 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
In flight the primary receiver is now also left when it has failed the strict checks for 10 s while the other receiver passed them. ([PX4-Autopilot#28798](https://github.com/PX4/PX4-Autopilot/pull/28798))
- **Re-check the motor failure trigger thresholds.**
`MOTFAIL_OFF` and `MOTFAIL_TIME` are replaced by a separate band and trigger time per direction ([MOTFAIL_OVER](../advanced_config/parameter_reference.md#MOTFAIL_OVER)/[MOTFAIL_OVR_TIME](../advanced_config/parameter_reference.md#MOTFAIL_OVR_TIME) and [MOTFAIL_UNDER](../advanced_config/parameter_reference.md#MOTFAIL_UNDER)/[MOTFAIL_UND_TIME](../advanced_config/parameter_reference.md#MOTFAIL_UND_TIME)), and are not migrated: the new parameters start at their defaults. The check now compares a low-pass filtered current residual instead of the instantaneous value, so the thresholds are worth re-deriving: see [Motor Failure Detection > Upgrading from PX4 v1.18](../config/motor_failure_detection.md#upgrading-from-px4-v1-18). ([PX4-Autopilot#27917](https://github.com/PX4/PX4-Autopilot/pull/27917))
- **The failure detector attitude trigger checks tilt instead of roll and pitch separately.**
[FD_FAIL_R](../advanced_config/parameter_reference.md#FD_FAIL_R) is now the maximum tilt angle (the angle between the vehicle's vertical axis and the vertical, in any direction) and [FD_FAIL_R_TTRI](../advanced_config/parameter_reference.md#FD_FAIL_R_TTRI) the time it has to be exceeded.
`FD_FAIL_P` and `FD_FAIL_P_TTRI` have been removed and are not migrated: a combined roll and pitch now triggers earlier than either did alone (e.g. 45° roll with 45° pitch is 60° tilt).
If you had set `FD_FAIL_P` below `FD_FAIL_R`, or set only `FD_FAIL_P` to 0 to disable the check, set `FD_FAIL_R` accordingly.
The check is now disabled with `FD_FAIL_R` set to -1; 0 still disables it.
The `fd_roll` and `fd_pitch` fields of [FailureDetectorStatus](../msg_docs/FailureDetectorStatus.md) are replaced by `fd_tilt`.
## Other changes
+1 -2
View File
@@ -1,8 +1,7 @@
uint64 timestamp # time since system start (microseconds)
# FailureDetector status
bool fd_roll
bool fd_pitch
bool fd_tilt
bool fd_alt
bool fd_ext
bool fd_arm_escs
@@ -44,36 +44,20 @@ void FailureDetectorChecks::checkAndReport(const Context &context, Report &repor
// don't warn while the vehicle is tilted during calibration
const bool report_attitude_failure = !context.status().calibration_enabled;
if (report_attitude_failure && fd_status.fd_roll) {
if (report_attitude_failure && fd_status.fd_tilt) {
/* EVENT
* @description
* The vehicle exceeded the maximum configured roll angle.
* The vehicle exceeded the maximum configured tilt angle.
*
* <profile name="dev">
* This check can be configured via <param>FD_FAIL_R</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_roll"),
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_tilt"),
events::Log::Critical, "Attitude failure detected");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (roll)");
}
} else if (report_attitude_failure && fd_status.fd_pitch) {
/* EVENT
* @description
* The vehicle exceeded the maximum configured pitch angle.
*
* <profile name="dev">
* This check can be configured via <param>FD_FAIL_P</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_pitch"),
events::Log::Critical, "Attitude failure detected");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (pitch)");
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (tilt)");
}
}
@@ -103,7 +87,7 @@ void FailureDetectorChecks::checkAndReport(const Context &context, Report &repor
}
}
reporter.failsafeFlags().fd_critical_failure = fd_status.fd_roll || fd_status.fd_pitch || fd_status.fd_ext;
reporter.failsafeFlags().fd_critical_failure = fd_status.fd_tilt || fd_status.fd_ext;
reporter.failsafeFlags().fd_alt_loss = fd_status.fd_alt;
reporter.failsafeFlags().fd_imbalanced_prop = fd_status.fd_imbalanced_prop;
@@ -65,8 +65,7 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
}
} else {
_failure_detector_status.flags.roll = false;
_failure_detector_status.flags.pitch = false;
_failure_detector_status.flags.tilt = false;
_failure_detector_status.flags.alt = false;
_failure_detector_status.flags.ext = false;
// Reset altitude loss state so it reinitialises cleanly when altitude control re-engages.
@@ -86,8 +85,7 @@ void FailureDetector::publishStatus(bool esc_arm_status, uint16_t motor_failure_
const uint16_t injected_motor_failure_mask = _injected_motor_masks.failure_mask;
failure_detector_status_s failure_detector_status{};
failure_detector_status.fd_roll = _failure_detector_status.flags.roll;
failure_detector_status.fd_pitch = _failure_detector_status.flags.pitch;
failure_detector_status.fd_tilt = _failure_detector_status.flags.tilt;
failure_detector_status.fd_alt = _failure_detector_status.flags.alt;
failure_detector_status.fd_ext = _failure_detector_status.flags.ext;
failure_detector_status.fd_arm_escs = esc_arm_status || (motor_failure_mask != 0);
@@ -107,45 +105,31 @@ void FailureDetector::updateAttitudeStatus(const vehicle_status_s &vehicle_statu
if (_vehicle_attitude_sub.update(&attitude)) {
const matrix::Eulerf euler(matrix::Quatf(attitude.q));
float roll(euler.phi());
float pitch(euler.theta());
matrix::Quatf q(attitude.q);
bool check_tilt = true;
// special handling for tailsitter
if (vehicle_status.is_vtol_tailsitter) {
if (vehicle_status.in_transition_mode) {
// disable attitude check during tailsitter transition
roll = 0.f;
pitch = 0.f;
check_tilt = false;
} else if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
// in FW flight rotate the attitude by 90° around pitch (level FW flight = 0° pitch)
const matrix::Eulerf euler_rotated = matrix::Eulerf(matrix::Quatf(attitude.q) * matrix::Quatf(matrix::Eulerf(0.f,
M_PI_2_F, 0.f)));
roll = euler_rotated.phi();
pitch = euler_rotated.theta();
// in FW flight rotate the attitude by 90° around pitch (level FW flight = 0° tilt)
q = q * matrix::Quatf(matrix::Eulerf(0.f, M_PI_2_F, 0.f));
}
}
const float max_roll_deg = _param_fd_fail_r.get();
const float max_pitch_deg = _param_fd_fail_p.get();
const float max_roll(fabsf(math::radians(max_roll_deg)));
const float max_pitch(fabsf(math::radians(max_pitch_deg)));
// angle between body z-axis and world z-axis
const float tilt = acosf(math::constrain(q.dcm_z()(2), -1.f, 1.f));
// -1 disables, 0 as well for backwards compatibility
const int max_tilt_deg = _param_fd_fail_r.get();
const bool tilt_status = check_tilt && (max_tilt_deg > 0)
&& (tilt > math::radians(static_cast<float>(max_tilt_deg)));
const bool roll_status = (max_roll > FLT_EPSILON) && (fabsf(roll) > max_roll);
const bool pitch_status = (max_pitch > FLT_EPSILON) && (fabsf(pitch) > max_pitch);
hrt_abstime now = hrt_absolute_time();
// Update hysteresis
_roll_failure_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1_s * _param_fd_fail_r_ttri.get()));
_pitch_failure_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1_s * _param_fd_fail_p_ttri.get()));
_roll_failure_hysteresis.set_state_and_update(roll_status, now);
_pitch_failure_hysteresis.set_state_and_update(pitch_status, now);
// Update status
_failure_detector_status.flags.roll = _roll_failure_hysteresis.get_state();
_failure_detector_status.flags.pitch = _pitch_failure_hysteresis.get_state();
_tilt_failure_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1_s * _param_fd_fail_r_ttri.get()));
_tilt_failure_hysteresis.set_state_and_update(tilt_status, hrt_absolute_time());
_failure_detector_status.flags.tilt = _tilt_failure_hysteresis.get_state();
}
}
@@ -69,8 +69,7 @@ using namespace time_literals;
union failure_detector_status_u {
struct {
uint16_t roll : 1;
uint16_t pitch : 1;
uint16_t tilt : 1;
uint16_t alt : 1;
uint16_t ext : 1;
uint16_t battery : 1;
@@ -101,8 +100,7 @@ private:
failure_detector_status_u _failure_detector_status{};
systemlib::Hysteresis _roll_failure_hysteresis{false};
systemlib::Hysteresis _pitch_failure_hysteresis{false};
systemlib::Hysteresis _tilt_failure_hysteresis{false};
systemlib::Hysteresis _alt_loss_hysteresis{false};
systemlib::Hysteresis _ext_ats_failure_hysteresis{false};
@@ -131,10 +129,8 @@ private:
failure_injection::MotorFailureMasks _injected_motor_masks{};
DEFINE_PARAMETERS(
(ParamInt<px4::params::FD_FAIL_P>) _param_fd_fail_p,
(ParamInt<px4::params::FD_FAIL_R>) _param_fd_fail_r,
(ParamFloat<px4::params::FD_FAIL_R_TTRI>) _param_fd_fail_r_ttri,
(ParamFloat<px4::params::FD_FAIL_P_TTRI>) _param_fd_fail_p_ttri,
(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_IMB_PROP_THR>) _param_fd_imb_prop_thr,
@@ -4,51 +4,24 @@ parameters:
definitions:
FD_FAIL_R:
description:
short: FailureDetector Max Roll
short: FailureDetector Max Tilt
long: |-
Maximum roll angle before FailureDetector triggers the attitude_failure flag.
Maximum tilt angle before FailureDetector triggers the attitude_failure flag.
The flag triggers flight termination (if @CBRK_FLIGHTTERM = 0),
which sets outputs to their failsafe values.
On takeoff the flag triggers lockdown (irrespective of @CBRK_FLIGHTTERM),
which disarms motors but does not set outputs to failsafe values.
Setting this parameter to 0 disables the check
Set to -1 to disable.
type: int32
default: 60
min: 0
max: 180
unit: deg
FD_FAIL_P:
description:
short: FailureDetector Max Pitch
long: |-
Maximum pitch angle before FailureDetector triggers the attitude_failure flag.
The flag triggers flight termination (if @CBRK_FLIGHTTERM = 0),
which sets outputs to their failsafe values.
On takeoff the flag triggers lockdown (irrespective of @CBRK_FLIGHTTERM),
which disarms motors but does not set outputs to failsafe values.
Setting this parameter to 0 disables the check
type: int32
default: 60
min: 0
min: -1
max: 180
unit: deg
FD_FAIL_R_TTRI:
description:
short: Roll failure trigger time
long: Seconds (decimal) that roll has to exceed FD_FAIL_R before being considered
as a failure.
type: float
default: 0.3
unit: s
min: 0.02
max: 5
decimal: 2
FD_FAIL_P_TTRI:
description:
short: Pitch failure trigger time
long: Seconds (decimal) that pitch has to exceed FD_FAIL_P before being considered
short: Tilt failure trigger time
long: Seconds (decimal) that tilt has to exceed FD_FAIL_R before being considered
as a failure.
type: float
default: 0.3