mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
feat(failure_detector)!: rename the tilt parameters to FD_TILT_ERROR
The tilt check limits the error from the attitude setpoint, which the FD_FAIL_R name no longer describes. FD_FAIL_R becomes FD_TILT_ERROR, now a float, and FD_FAIL_R_TTRI becomes FD_TILT_ERROR_T. The old roll and pitch limits and trigger times meant something else, so they start at the new defaults instead of being taken over. Only a check disabled for both roll and pitch stays disabled, as -1.
This commit is contained in:
1 parent
8225bb350f
commit
fa877566a4
20 files changed
+56
-40
No files matched your search
@@ -92,4 +92,4 @@ param set-default PWM_MAIN_FUNC8 108
|
||||
param set-default CA_METHOD 0
|
||||
|
||||
# disable attitude failure detection
|
||||
param set-default FD_FAIL_R -1
|
||||
param set-default FD_TILT_ERROR -1
|
||||
@@ -33,8 +33,6 @@ param set-default PWM_MAIN_FUNC3 103
|
||||
param set-default PWM_MAIN_FUNC4 104
|
||||
param set-default PWM_MAIN_FUNC5 0
|
||||
|
||||
param set-default FD_FAIL_R 70
|
||||
|
||||
param set-default FW_P_TC 0.6
|
||||
|
||||
param set-default FW_PR_I 0.3
|
||||
|
||||
@@ -48,8 +48,6 @@ param set-default SIM_GZ_EC_MAX2 1500
|
||||
param set-default SIM_GZ_EC_MAX3 1500
|
||||
param set-default SIM_GZ_EC_MAX4 1500
|
||||
|
||||
param set-default FD_FAIL_R 70
|
||||
|
||||
param set-default FW_P_TC 0.6
|
||||
|
||||
param set-default FW_PR_I 0.3
|
||||
|
||||
@@ -298,8 +298,8 @@ param set-default RTL_RETURN_ALT 180
|
||||
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_R_TTRI 1
|
||||
param set-default FD_TILT_ERROR 85
|
||||
param set-default FD_TILT_ERROR_T 1
|
||||
param set-default WV_EN 0
|
||||
|
||||
################################################################################
|
||||
|
||||
@@ -35,7 +35,7 @@ param set-default GF_ACTION 1
|
||||
param set-default NAV_RCL_ACT 1
|
||||
|
||||
# disable attitude failure detection
|
||||
param set-default FD_FAIL_R -1
|
||||
param set-default FD_TILT_ERROR -1
|
||||
|
||||
param set-default CA_ROTOR_COUNT 8
|
||||
param set-default CA_R_REV 255
|
||||
|
||||
@@ -30,7 +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_R -1
|
||||
param set-default FD_TILT_ERROR -1
|
||||
|
||||
# Disable magnetometer fusion in EKF2
|
||||
param set-default EKF2_MAG_TYPE 5
|
||||
|
||||
@@ -30,7 +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_R -1
|
||||
param set-default FD_TILT_ERROR -1
|
||||
|
||||
# Disable magnetometer fusion in EKF2
|
||||
param set-default EKF2_MAG_TYPE 5
|
||||
|
||||
@@ -114,4 +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_R -1
|
||||
param set-default FD_TILT_ERROR -1
|
||||
@@ -50,7 +50,7 @@ param set-default GF_ACTION 1
|
||||
param set-default NAV_RCL_ACT 1
|
||||
|
||||
# disable attitude failure detection
|
||||
param set-default FD_FAIL_R -1
|
||||
param set-default FD_TILT_ERROR -1
|
||||
|
||||
param set-default CA_ROTOR_COUNT 8
|
||||
param set-default CA_R_REV 255
|
||||
|
||||
@@ -25,7 +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_R -1
|
||||
param set-default FD_TILT_ERROR -1
|
||||
|
||||
# Disable magnetometer fusion in EKF2
|
||||
param set-default EKF2_MAG_TYPE 5
|
||||
|
||||
@@ -29,7 +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_R -1
|
||||
param set-default FD_TILT_ERROR -1
|
||||
|
||||
# Disable magnetometer fusion in EKF2
|
||||
param set-default EKF2_MAG_TYPE 5
|
||||
|
||||
@@ -458,8 +458,8 @@ 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_R"></a>[FD_FAIL_R](../advanced_config/parameter_reference.md#FD_FAIL_R) | Maximum allowed tilt error (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 1s). |
|
||||
| <a id="FD_TILT_ERROR"></a>[FD_TILT_ERROR](../advanced_config/parameter_reference.md#FD_TILT_ERROR) | Maximum allowed tilt error (in degrees). |
|
||||
| <a id="FD_TILT_ERROR_T"></a>[FD_TILT_ERROR_T](../advanced_config/parameter_reference.md#FD_TILT_ERROR_T) | Time to exceed [FD_TILT_ERROR](#FD_TILT_ERROR) for failure detection (default 1s). |
|
||||
|
||||
### 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_R](../advanced_config/parameter_reference.md#FD_FAIL_R) to -1.
|
||||
- Disable failure detection: set [FD_TILT_ERROR](../advanced_config/parameter_reference.md#FD_TILT_ERROR) 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
|
||||
|
||||
@@ -89,12 +89,12 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
|
||||
- **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 the tilt error instead of roll and pitch separately.**
|
||||
[FD_FAIL_R](../advanced_config/parameter_reference.md#FD_FAIL_R) is now the maximum angle between the vehicle tilt and the attitude setpoint tilt, in any direction, and [FD_FAIL_R_TTRI](../advanced_config/parameter_reference.md#FD_FAIL_R_TTRI) the time it has to be exceeded (default now 1 s instead of 0.3 s).
|
||||
[FD_TILT_ERROR](../advanced_config/parameter_reference.md#FD_TILT_ERROR) replaces `FD_FAIL_R` and `FD_FAIL_P` and is the maximum angle between the vehicle tilt and the attitude setpoint tilt, in any direction. [FD_TILT_ERROR_T](../advanced_config/parameter_reference.md#FD_TILT_ERROR_T) replaces `FD_FAIL_R_TTRI` and `FD_FAIL_P_TTRI` and is the time it has to be exceeded (default now 1 s instead of 0.3 s).
|
||||
The old values are not migrated because their meaning changed: the new parameters start at their defaults, except that a check disabled for both roll and pitch stays disabled.
|
||||
Without a recent attitude setpoint, for example on rovers, the tilt from level is checked instead.
|
||||
Vehicles may now fly at any tilt their attitude setpoint asks for, and a large tracking error triggers the check at any tilt.
|
||||
`FD_FAIL_P` and `FD_FAIL_P_TTRI` have been removed and are not migrated.
|
||||
If you raised `FD_FAIL_R` or `FD_FAIL_R_TTRI` to allow larger tilts, consider resetting them to their defaults.
|
||||
The check is now disabled with `FD_FAIL_R` set to -1; 0 still disables it.
|
||||
Limits raised or disabled to allow large tilts, for example roll on fixed-wing vehicles, are no longer needed.
|
||||
The check is disabled with `FD_TILT_ERROR` set to -1.
|
||||
The `fd_roll` and `fd_pitch` fields of [FailureDetectorStatus](../msg_docs/FailureDetectorStatus.md) are replaced by `fd_tilt`.
|
||||
|
||||
## Other changes
|
||||
|
||||
@@ -474,5 +474,27 @@ param_modify_on_import_ret param_modify_on_import(bson_node_t node)
|
||||
}
|
||||
}
|
||||
|
||||
// 2026-11-17: the failure detector checks the tilt error from the attitude setpoint instead of absolute roll and pitch.
|
||||
// The old limits and trigger times mean something else now and are not taken over, only a check disabled for both
|
||||
// roll and pitch stays disabled. The other old parameter may still follow in the import, so both are tracked.
|
||||
{
|
||||
static bool roll_disabled = false, pitch_disabled = false; // FD_FAIL_R and FD_FAIL_P default to 60°
|
||||
|
||||
const bool is_roll = (node->type == bson_type_t::BSON_INT32) && (strcmp("FD_FAIL_R", node->name) == 0);
|
||||
const bool is_pitch = (node->type == bson_type_t::BSON_INT32) && (strcmp("FD_FAIL_P", node->name) == 0);
|
||||
|
||||
if (is_roll || is_pitch) {
|
||||
(is_roll ? roll_disabled : pitch_disabled) = (node->i32 == 0);
|
||||
|
||||
if (roll_disabled && pitch_disabled) {
|
||||
float max_tilt_error_deg = -1.f;
|
||||
param_set(param_find("FD_TILT_ERROR"), &max_tilt_error_deg);
|
||||
PX4_INFO("migrating FD_FAIL_R and FD_FAIL_P -> FD_TILT_ERROR (disabled)");
|
||||
}
|
||||
|
||||
return param_modify_on_import_ret::PARAM_SKIP_IMPORT;
|
||||
}
|
||||
}
|
||||
|
||||
return param_modify_on_import_ret::PARAM_NOT_MODIFIED;
|
||||
}
|
||||
@@ -50,7 +50,7 @@ void FailureDetectorChecks::checkAndReport(const Context &context, Report &repor
|
||||
* The vehicle tilt deviated from the attitude setpoint, by more than the configured angle.
|
||||
*
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>FD_FAIL_R</param> parameter.
|
||||
* This check can be configured via <param>FD_TILT_ERROR</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_tilt"),
|
||||
|
||||
@@ -123,14 +123,11 @@ void FailureDetector::updateAttitudeStatus(const vehicle_status_s &vehicle_statu
|
||||
z_setpoint = matrix::Quatf(attitude_setpoint.q_d).dcm_z();
|
||||
}
|
||||
|
||||
// angle between body z-axis and setpoint z-axis
|
||||
const float max_tilt_error_deg = _param_fd_tilt_error.get();
|
||||
const float tilt_error = acosf(math::constrain(q.dcm_z().dot(z_setpoint), -1.f, 1.f));
|
||||
// -1 disables, 0 as well for backwards compatibility
|
||||
const int max_tilt_error_deg = _param_fd_fail_r.get();
|
||||
const bool tilt_status = (max_tilt_error_deg > 0)
|
||||
&& (tilt_error > math::radians(static_cast<float>(max_tilt_error_deg)));
|
||||
const bool tilt_status = (max_tilt_error_deg > 0.f) && (tilt_error > math::radians(max_tilt_error_deg));
|
||||
|
||||
_tilt_failure_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1_s * _param_fd_fail_r_ttri.get()));
|
||||
_tilt_failure_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1_s * _param_fd_tilt_error_t.get()));
|
||||
_tilt_failure_hysteresis.set_state_and_update(tilt_status, attitude.timestamp);
|
||||
_failure_detector_status.flags.tilt = _tilt_failure_hysteresis.get_state();
|
||||
}
|
||||
|
||||
@@ -130,8 +130,8 @@ private:
|
||||
failure_injection::MotorFailureMasks _injected_motor_masks{};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(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_TILT_ERROR>) _param_fd_tilt_error,
|
||||
(ParamFloat<px4::params::FD_TILT_ERROR_T>) _param_fd_tilt_error_t,
|
||||
(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,
|
||||
|
||||
@@ -48,7 +48,7 @@ using matrix::Eulerf;
|
||||
using matrix::Quatf;
|
||||
using matrix::Vector3f;
|
||||
|
||||
static constexpr int kMaxTiltErrorDeg = 60;
|
||||
static constexpr float kMaxTiltErrorDeg = 60.f;
|
||||
|
||||
// Tilt by angle around a horizontal axis, direction 0° tilts around the x-axis (roll), 90° around the y-axis (pitch)
|
||||
static Quatf tilt(float angle_deg, float direction_deg = 0.f)
|
||||
@@ -76,10 +76,10 @@ public:
|
||||
_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
|
||||
}
|
||||
|
||||
void setParams(int32_t max_tilt_error_deg, float trigger_time_s)
|
||||
void setParams(float max_tilt_error_deg, float trigger_time_s)
|
||||
{
|
||||
param_set(param_handle(px4::params::FD_FAIL_R), &max_tilt_error_deg);
|
||||
param_set(param_handle(px4::params::FD_FAIL_R_TTRI), &trigger_time_s);
|
||||
param_set(param_handle(px4::params::FD_TILT_ERROR), &max_tilt_error_deg);
|
||||
param_set(param_handle(px4::params::FD_TILT_ERROR_T), &trigger_time_s);
|
||||
updateParams();
|
||||
}
|
||||
|
||||
@@ -195,11 +195,11 @@ TEST_F(FailureDetectorTiltTest, StaleSetpointFallsBackToLevel)
|
||||
|
||||
TEST_F(FailureDetectorTiltTest, NoTriggerWhenDisabled)
|
||||
{
|
||||
setParams(-1, 0.1f);
|
||||
setParams(-1.f, 0.1f);
|
||||
EXPECT_FALSE(updateAfterTriggerTime(tilt(180.f)));
|
||||
|
||||
// 0 also disables for backwards compatibility
|
||||
setParams(0, 0.1f);
|
||||
setParams(0.f, 0.1f);
|
||||
EXPECT_FALSE(updateAfterTriggerTime(tilt(180.f)));
|
||||
}
|
||||
|
||||
|
||||
@@ -2,7 +2,7 @@ module_name: failure_detector
|
||||
parameters:
|
||||
- group: Failure Detector
|
||||
definitions:
|
||||
FD_FAIL_R:
|
||||
FD_TILT_ERROR:
|
||||
description:
|
||||
short: FailureDetector Max Tilt Error
|
||||
long: |-
|
||||
@@ -14,15 +14,16 @@ parameters:
|
||||
which disarms motors but does not set outputs to failsafe values.
|
||||
|
||||
Set to -1 to disable.
|
||||
type: int32
|
||||
type: float
|
||||
default: 60
|
||||
min: -1
|
||||
max: 180
|
||||
unit: deg
|
||||
FD_FAIL_R_TTRI:
|
||||
decimal: 0
|
||||
FD_TILT_ERROR_T:
|
||||
description:
|
||||
short: Tilt failure trigger time
|
||||
long: Seconds (decimal) that the tilt error has to exceed FD_FAIL_R before being considered as a failure.
|
||||
long: Seconds (decimal) that the tilt error has to exceed FD_TILT_ERROR before being considered as a failure.
|
||||
type: float
|
||||
default: 1.0
|
||||
unit: s
|
||||
|
||||
Reference in new issue
Block a user