Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00
Code Issues Wiki Activity

Added crash detection and recovery

Browse source
This commit is contained in:
Martin Budden 2017-03-23 18:06:03 +00:00
parent d0475a6987
commit 1dc5f88820
4 changed files with 83 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

22
src/main/fc/cli.c
View file

@ -197,6 +197,10 @@ static const char * const lookupTableOffOn[] = {
"OFF", "ON"
};
static const char * const lookupTableCrashRecovery[] = {
"OFF", "ON" ,"BEEP"
};
static const char * const lookupTableUnit[] = {
"IMPERIAL", "METRIC"
};
@ -383,6 +387,7 @@ typedef enum {
TABLE_RC_INTERPOLATION_CHANNELS,
TABLE_LOWPASS_TYPE,
TABLE_FAILSAFE,
TABLE_CRASH_RECOVERY,
#ifdef OSD
TABLE_OSD,
#endif
@ -426,6 +431,7 @@ static const lookupTableEntry_t lookupTables[] = {
{ lookupTableRcInterpolationChannels, sizeof(lookupTableRcInterpolationChannels) / sizeof(char *) },
{ lookupTableLowpassType, sizeof(lookupTableLowpassType) / sizeof(char *) },
{ lookupTableFailsafe, sizeof(lookupTableFailsafe) / sizeof(char *) },
{ lookupTableCrashRecovery, sizeof(lookupTableCrashRecovery) / sizeof(char *) },
#ifdef OSD
{ lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) },
#endif
@ -726,16 +732,22 @@ static const clivalue_t valueTable[] = {
{ "vbat_pid_gain", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, vbatPidCompensation) },
{ "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, pidAtMinThrottle) },
{ "anti_gravity_thresh", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) },
{ "anti_gravity_gain", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 30000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermAcceleratorGain) },
{ "anti_gravity_gain", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 30000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermAcceleratorGain) },
{ "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, setpointRelaxRatio) },
{ "d_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 254 }, PG_PID_PROFILE, offsetof(pidProfile_t, dtermSetpointWeight) },
{ "yaw_accel_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) },
{ "accel_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) },
{ "yaw_accel_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) },
{ "accel_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_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) },
{ "crash_recovery", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRASH_RECOVERY }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery) },
{ "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) },
{ "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lpf_hz) },
{ "pidsum_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) },
{ "pidsum_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) },
{ "pidsum_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) },
{ "pidsum_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) },
{ "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, P8[PITCH]) },
{ "i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, I8[PITCH]) },

8
src/main/fc/fc_core.c
View file

@ -175,7 +175,7 @@ void mwDisarm(void)
finishBlackbox();
}
#endif
BEEP_OFF;
beeper(BEEPER_DISARMING); // emit disarm tone
}
}
@ -475,12 +475,12 @@ void processRx(timeUs_t currentTimeUs)
#endif
}
static void subTaskPidController(void)
static void subTaskPidController(timeUs_t currentTimeUs)
{
uint32_t startTime = 0;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
// PID - note this is function pointer set by setPIDController()
pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims);
pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims, currentTimeUs);
DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
}
@ -630,7 +630,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
pidUpdateCountdown--;
} else {
pidUpdateCountdown = setPidUpdateCountDown();
subTaskPidController();
subTaskPidController(currentTimeUs);
subTaskMotorUpdate();
runTaskMainSubprocesses = true;
}

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

@ -34,6 +34,8 @@
#include "config/parameter_group_ids.h"
#include "config/config_profile.h"
#include "drivers/sound_beeper.h"
#include "fc/fc_core.h"
#include "fc/fc_rc.h"
@ -119,7 +121,13 @@ void resetPidProfile(pidProfile_t *pidProfile)
.yawRateAccelLimit = 100,
.rateAccelLimit = 0,
.itermThrottleThreshold = 350,
.itermAcceleratorGain = 1000
.itermAcceleratorGain = 1000,
.crash_time = 500, // 500ms
.crash_recovery_angle = 10, // 10 degrees
.crash_recovery_rate = 100, // 100 degrees/second
.crash_dthreshold = 50, // 50 degrees/second/second
.crash_gthreshold = 200, // 200 degrees/second
.crash_recovery = PID_CRASH_RECOVERY_OFF // off by default
);
}
@ -230,6 +238,11 @@ static float Kp[3], Ki[3], Kd[3], maxVelocity[3];
static float relaxFactor;
static float dtermSetpointWeight;
static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermWindupPointInv;
static timeDelta_t crashTimeLimitUs;
static int32_t crashRecoveryAngleDeciDegrees;
static float crashRecoveryRate;
static float crashDtermThreshold;
static float crashGyroThreshold;
void pidInitConfig(const pidProfile_t *pidProfile) {
for(int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@ -246,6 +259,11 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT;
ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint);
crashTimeLimitUs = pidProfile->crash_time * 1000;
crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10;
crashRecoveryRate = pidProfile->crash_recovery_rate;
crashGyroThreshold = pidProfile->crash_gthreshold;
crashDtermThreshold = pidProfile->crash_dthreshold;
}
void pidInit(const pidProfile_t *pidProfile)
@ -298,11 +316,13 @@ static float accelerationLimit(int axis, float currentPidSetpoint) {
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
// Based on 2DOF reference design (matlab)
void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim)
void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
{
static float previousRateError[2];
const float tpaFactor = getThrottlePIDAttenuation();
const float motorMixRange = getMotorMixRange();
static bool inCrashRecoveryMode = false;
static timeUs_t crashDetectedAtUs;
// Dynamic ki component to gradually scale back integration when above windup point
const float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f);
@ -319,6 +339,21 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
}
if (inCrashRecoveryMode && axis != FD_YAW) {
// self-level - errorAngle is deviation from horizontal
const float errorAngle = -(attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
currentPidSetpoint = errorAngle * levelGain;
if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs
|| (motorMixRange < 1.0f
&& ABS(attitude.raw[FD_ROLL] - angleTrim->raw[FD_ROLL]) < crashRecoveryAngleDeciDegrees
&& ABS(attitude.raw[FD_PITCH] - angleTrim->raw[FD_PITCH]) < crashRecoveryAngleDeciDegrees
&& ABS(gyro.gyroADCf[FD_ROLL]) < crashRecoveryRate
&& ABS(gyro.gyroADCf[FD_PITCH]) < crashRecoveryRate)
) {
inCrashRecoveryMode = false;
BEEP_OFF;
}
}
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
@ -351,6 +386,17 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
float delta = (rD - previousRateError[axis]) / dT;
previousRateError[axis] = rD;
// if crash recovery is on check for a crash
if (pidProfile->crash_recovery) {
if (motorMixRange >= 1.0f && ABS(delta) > crashDtermThreshold && ABS(errorRate) > crashGyroThreshold) {
inCrashRecoveryMode = true;
crashDetectedAtUs = currentTimeUs;
if (pidProfile->crash_recovery == PID_CRASH_RECOVERY_BEEP) {
BEEP_ON;
}
}
}
// apply filters
delta = dtermNotchFilterApplyFn(dtermFilterNotch[axis], delta);
delta = dtermLpfApplyFn(dtermFilterLpf[axis], delta);

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

@ -18,6 +18,7 @@
#pragma once
#include <stdbool.h>
#include "common/time.h"
#include "config/parameter_group.h"
#define MAX_PID_PROCESS_DENOM 16
@ -57,6 +58,12 @@ typedef enum {
PID_STABILISATION_ON
} pidStabilisationState_e;
typedef enum {
PID_CRASH_RECOVERY_OFF = 0,
PID_CRASH_RECOVERY_ON,
PID_CRASH_RECOVERY_BEEP
} pidCrashRecovery_e;
typedef struct pidProfile_s {
uint8_t P8[PID_ITEM_COUNT];
uint8_t I8[PID_ITEM_COUNT];
@ -83,6 +90,12 @@ typedef struct pidProfile_s {
uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive 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_time; // ms
uint8_t crash_recovery_angle; // degrees
uint8_t crash_recovery_rate; // degree/second
pidCrashRecovery_e crash_recovery; // off, on, on and beeps when it is in crash recovery mode
} pidProfile_t;
#if FLASH_SIZE <= 128
@ -99,7 +112,7 @@ typedef struct pidConfig_s {
PG_DECLARE(pidConfig_t, pidConfig);
union rollAndPitchTrims_u;
void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim);
void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim, timeUs_t currentTimeUs);
extern float axisPID_P[3], axisPID_I[3], axisPID_D[3];
bool airmodeWasActivated;