Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-22 07:45:29 +03:00
Code Issues Wiki Activity

Merge pull request #2783 from martinbudden/mb_crash_recovery

Browse source 
Added experimental crash detection and recovery
This commit is contained in:
Martin Budden 2017-04-12 07:57:52 +01:00 • committed by GitHub
commit a9bb3fb8f6
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

@ -196,6 +196,10 @@ static const char * const lookupTableOffOn[] = {
"OFF", "ON"
};
static const char * const lookupTableCrashRecovery[] = {
"OFF", "ON" ,"BEEP"
};
static const char * const lookupTableUnit[] = {
"IMPERIAL", "METRIC"
};
@ -382,6 +386,7 @@ typedef enum {
TABLE_RC_INTERPOLATION_CHANNELS,
TABLE_LOWPASS_TYPE,
TABLE_FAILSAFE,
TABLE_CRASH_RECOVERY,
#ifdef OSD
TABLE_OSD,
#endif
@ -425,6 +430,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
@ -727,16 +733,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

@ -174,7 +174,7 @@ void mwDisarm(void)
finishBlackbox();
}
#endif
BEEP_OFF;
beeper(BEEPER_DISARMING); // emit disarm tone
}
}
@ -474,12 +474,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);
}
@ -629,7 +629,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
pidUpdateCountdown--;
} else {
pidUpdateCountdown = setPidUpdateCountDown();
subTaskPidController();
subTaskPidController(currentTimeUs);
subTaskMotorUpdate();
runTaskMainSubprocesses = true;
}

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

@ -33,6 +33,8 @@
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/sound_beeper.h"
#include "fc/fc_core.h"
#include "fc/fc_rc.h"
@ -118,7 +120,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
);
}
@ -229,6 +237,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++) {
@ -245,6 +258,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)
@ -297,11 +315,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);
@ -318,6 +338,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. ----------
@ -353,6 +388,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

@ -19,6 +19,7 @@
#ifndef USE_OSD_SLAVE
#include <stdbool.h>
#include "common/time.h"
#include "config/parameter_group.h"
#define MAX_PID_PROCESS_DENOM 16
@ -58,6 +59,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];
@ -84,6 +91,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;
PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);
@ -95,7 +108,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;