rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #3606 from martinbudden/bf_crash_setpoint 

Added setpoint check to crash detection
This commit is contained in:
Martin Budden 2017-07-23 11:49:08 +01:00 committed by GitHub
parent 3cd73f1a29 20f0a40f96
commit 8ccb5097df
3 changed files with 15 additions and 7 deletions
Show Stats 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) },

18
src/main/flight/pid.c
View File

@ -105,11 +105,12 @@ void resetPidProfile(pidProfile_t *pidProfile)
.rateAccelLimit = 0,
.itermThrottleThreshold = 350,
.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 = 500, // 500 degrees/second
.crash_time = 500, // ms
.crash_recovery_angle = 10, // degrees
.crash_recovery_rate = 100, // degrees/second
.crash_dthreshold = 50, // degrees/second/second
.crash_gthreshold = 400, // degrees/second
.crash_setpoint_threshold = 350, // 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