fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

FailureDetector - Add roll and pitch failures hysteresis

Browse Source
This commit is contained in:
bresch 2019-01-07 11:10:20 +01:00 committed by Beat Küng
parent a72de95c94
commit 38345be41a
3 changed files with 45 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/commander/failure_detector/FailureDetector.cpp
View File

@ -77,14 +77,20 @@ FailureDetector::update_attitude_status()
const bool roll_status = (max_roll > 0.0f) && (fabsf(roll) > max_roll);
const bool pitch_status = (max_pitch > 0.0f) && (fabsf(pitch) > max_pitch);
// Update hysteresis
_roll_failure_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _param_fd_fail_r_ttri.get()));
_pitch_failure_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _param_fd_fail_p_ttri.get()));
_roll_failure_hysteresis.set_state_and_update(roll_status);
_pitch_failure_hysteresis.set_state_and_update(pitch_status);
// Update bitmask
_status &= ~(FAILURE_ROLL | FAILURE_PITCH);
if (roll_status) {
if (_roll_failure_hysteresis.get_state()) {
_status |= FAILURE_ROLL;
}
if (pitch_status) {
if (_pitch_failure_hysteresis.get_state()) {
_status |= FAILURE_PITCH;
}

8
src/modules/commander/failure_detector/FailureDetector.hpp
View File

@ -46,6 +46,7 @@
#include <matrix/matrix/math.hpp>
#include <mathlib/mathlib.h>
#include <px4_module_params.h>
#include <systemlib/hysteresis/hysteresis.h>
// subscriptions
#include <uORB/Subscription.hpp>
@ -75,7 +76,9 @@ private:
DEFINE_PARAMETERS(
(ParamInt<px4::params::FD_FAIL_P>) _param_fd_fail_p,
(ParamInt<px4::params::FD_FAIL_R>) _param_fd_fail_r
(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
)
// Subscriptions
@ -84,5 +87,8 @@ private:
uint8_t _status{FAILURE_NONE};
systemlib::Hysteresis _roll_failure_hysteresis{false};
systemlib::Hysteresis _pitch_failure_hysteresis{false};
bool update_attitude_status();
};

32
src/modules/commander/failure_detector/failure_detector_params.c
View File

@ -46,7 +46,7 @@
* FailureDetector Max Roll
*
* Maximum roll angle before FailureDetector triggers the attitude_failure flag
* If flight termination is enabled (@CBRK_FLIGHTTERM set to zero), the autopilot
* If flight termination is enabled (@CBRK_FLIGHTTERM set to 0), the autopilot
* will terminate the flight and set all the outputs to their failsafe value
* as soon as the attitude_failure flag is set.
*
@ -63,7 +63,7 @@ PARAM_DEFINE_INT32(FD_FAIL_R, 60);
* FailureDetector Max Pitch
*
* Maximum pitch angle before FailureDetector triggers the attitude_failure flag
* If flight termination is enabled (@CBRK_FLIGHTTERM set to zero), the autopilot
* If flight termination is enabled (@CBRK_FLIGHTTERM set to 0), the autopilot
* will terminate the flight and set all the outputs to their failsafe value
* as soon as the attitude_failure flag is set.
*
@ -75,3 +75,31 @@ PARAM_DEFINE_INT32(FD_FAIL_R, 60);
* @group Failure Detector
*/
PARAM_DEFINE_INT32(FD_FAIL_P, 60);
/**
* Roll failure trigger time
*
* Seconds (decimal) that roll has to exceed FD_FAIL_R before being considered as a failure.
*
* @unit s
* @min 0.02
* @max 5
* @decimal 2
*
* @group Failure Detector
*/
PARAM_DEFINE_FLOAT(FD_FAIL_R_TTRI, 0.3);
/**
* Pitch failure trigger time
*
* Seconds (decimal) that pitch has to exceed FD_FAIL_P before being considered as a failure.
*
* @unit s
* @min 0.02
* @max 5
* @decimal 2
*
* @group Failure Detector
*/
PARAM_DEFINE_FLOAT(FD_FAIL_P_TTRI, 0.3);