mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
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:
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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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,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
|
||||
|
||||
Reference in new issue
Block a user