Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

Added setpoint check to crash detection

Browse source
This commit is contained in:
Martin Budden 2017-07-23 07:10:14 +01:00
parent 3fc3de9ebd
commit 1ee034c38a
3 changed files with 11 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/fc/settings.c
View file

@ -548,6 +548,7 @@ const clivalue_t valueTable[] = {
{ "acc_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) },
{ "crash_dthreshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_dthreshold) },
{ "crash_gthreshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_gthreshold) },
{ "crash_setpoint_threshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_setpoint_threshold) },
{ "crash_time", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 5000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_time) },
{ "crash_recovery_angle", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery_angle) },
{ "crash_recovery_rate", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery_rate) },

10
src/main/flight/pid.c
View file

@ -109,7 +109,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
.crash_recovery_angle = 10, // 10 degrees
.crash_recovery_rate = 100, // 100 degrees/second
.crash_dthreshold = 50, // 50 degrees/second/second
.crash_gthreshold = 500, // 500 degrees/second
.crash_gthreshold = 200, // 200 degrees/second
.crash_setpoint_threshold = 400, // 400 degrees/second
.crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default
.horizon_tilt_effect = 75,
.horizon_tilt_expert_mode = false
@ -241,6 +242,7 @@ static int32_t crashRecoveryAngleDeciDegrees;
static float crashRecoveryRate;
static float crashDtermThreshold;
static float crashGyroThreshold;
static float crashSetpointThreshold;
void pidInitConfig(const pidProfile_t *pidProfile) {
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@ -265,6 +267,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
crashRecoveryRate = pidProfile->crash_recovery_rate;
crashGyroThreshold = pidProfile->crash_gthreshold;
crashDtermThreshold = pidProfile->crash_dthreshold;
crashSetpointThreshold = pidProfile->crash_setpoint_threshold;
}
void pidInit(const pidProfile_t *pidProfile)
@ -448,7 +451,10 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// if crash recovery is on and accelerometer enabled then check for a crash
if (pidProfile->crash_recovery && sensors(SENSOR_ACC)) {
if (motorMixRange >= 1.0f && ABS(delta) > crashDtermThreshold && ABS(errorRate) > crashGyroThreshold) {
if (motorMixRange >= 1.0f
&& ABS(delta) > crashDtermThreshold
&& ABS(errorRate) > crashGyroThreshold
&& ABS(getSetpointRate(axis)) < crashSetpointThreshold) {
inCrashRecoveryMode = true;
crashDetectedAtUs = currentTimeUs;
if (pidProfile->crash_recovery == PID_CRASH_RECOVERY_BEEP) {

3
src/main/flight/pid.h
View file

@ -95,11 +95,12 @@ typedef struct pidProfile_s {
uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms
uint16_t itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit
uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect
uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative)
uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > aggressive derivative)
uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms
uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms
uint16_t crash_dthreshold; // dterm crash value
uint16_t crash_gthreshold; // gyro crash value
uint16_t crash_setpoint_threshold; // setpoint must be below this value to detect crash, so flips and rolls are not interpreted as crashes
uint16_t crash_time; // ms
uint8_t crash_recovery_angle; // degrees
uint8_t crash_recovery_rate; // degree/second