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

Clean code for yaw spin recovery

Browse source
This commit is contained in:
ctzsnooze 2018-04-22 22:45:29 +10:00 • committed by Michael Keller
parent a2c6264d55
commit 41fb37a264
6 changed files with 111 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -59,6 +59,8 @@
#include "sensors/battery.h"
#include "sensors/gyro.h"
PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0);
#ifndef TARGET_DEFAULT_MIXER
@ -763,6 +765,14 @@ NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
throttle = applyThrottleLimit(throttle);
}
// Handle yaw spin recovery - throttle is set to zero to prevent flyaway
// and to give the mixer full authority to stop the spin
#ifdef USE_YAW_SPIN_RECOVERY
if (gyroYawSpinDetected()) {
throttle = 0.0f;
}
#endif // USE_YAW_SPIN_RECOVERY
// Find roll/pitch/yaw desired output
float motorMix[MAX_SUPPORTED_MOTORS];
float motorMixMax = 0, motorMixMin = 0;

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

@ -571,6 +571,14 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
}
// Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
// It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
#ifdef USE_YAW_SPIN_RECOVERY
if ((axis == FD_YAW) && gyroYawSpinDetected()) {
currentPidSetpoint = 0.0f;
}
#endif // USE_YAW_SPIN_RECOVERY
// -----calculate error rate
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
float errorRate = currentPidSetpoint - gyroRate; // r - y
@ -618,6 +626,15 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate);
pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor;
#ifdef USE_YAW_SPIN_RECOVERY
if (gyroYawSpinDetected()) {
// zero PIDs on pitch and roll leaving yaw P to correct spin
pidData[axis].P = 0;
pidData[axis].I = 0;
pidData[axis].D = 0;
}
#endif // USE_YAW_SPIN_RECOVERY
}
}
@ -625,6 +642,13 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
pidData[FD_ROLL].Sum = pidData[FD_ROLL].P + pidData[FD_ROLL].I + pidData[FD_ROLL].D;
pidData[FD_PITCH].Sum = pidData[FD_PITCH].P + pidData[FD_PITCH].I + pidData[FD_PITCH].D;
#ifdef USE_YAW_SPIN_RECOVERY
if (gyroYawSpinDetected()) {
// yaw P alone to correct spin
pidData[FD_YAW].I = 0;
}
#endif // USE_YAW_SPIN_RECOVERY
// YAW has no D
pidData[FD_YAW].Sum = pidData[FD_YAW].P + pidData[FD_YAW].I;

19
src/main/interface/settings.c
View file

@ -412,12 +412,12 @@ const clivalue_t valueTable[] = {
#endif
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 32 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_sync_denom) },
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_type) },
{ "gyro_lowpass_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_hz) },
{ "gyro_lowpass_order", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, GYRO_LPF_ORDER_MAX}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_order) },
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_type) },
{ "gyro_lowpass_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_hz) },
{ "gyro_lowpass_order", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, GYRO_LPF_ORDER_MAX}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_order) },
{ "gyro_lowpass2_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_type) },
{ "gyro_lowpass2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_hz) },
{ "gyro_lowpass2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_hz) },
{ "gyro_lowpass2_order", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, GYRO_LPF_ORDER_MAX}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_order) },
{ "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_1) },
@ -425,14 +425,19 @@ const clivalue_t valueTable[] = {
{ "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_2) },
{ "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) },
{ "gyro_lma_depth", VAR_UINT8 | MASTER_VALUE, .config.minmax = {0, 11}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lma_depth)},
{ "gyro_lma_weight", VAR_UINT8 | MASTER_VALUE, .config.minmax = {0, 100}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lma_weight)},
{ "gyro_lma_depth", VAR_UINT8 | MASTER_VALUE, .config.minmax = {0, 11}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lma_depth)},
{ "gyro_lma_weight", VAR_UINT8 | MASTER_VALUE, .config.minmax = {0, 100}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lma_weight)},
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) },
{ "gyro_offset_yaw", VAR_INT16 | MASTER_VALUE, .config.minmax = { -1000, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_offset_yaw) },
{ "gyro_offset_yaw", VAR_INT16 | MASTER_VALUE, .config.minmax = { -1000, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_offset_yaw) },
#ifdef USE_GYRO_OVERFLOW_CHECK
{ "gyro_overflow_detect", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_OVERFLOW_CHECK }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, checkOverflow) },
#endif
#ifdef USE_YAW_SPIN_RECOVERY
{ "yaw_spin_recovery", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, yaw_spin_recovery) },
{ "yaw_spin_threshold", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 500, 1950 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, yaw_spin_threshold) },
#endif
#if defined(GYRO_USES_SPI)
#ifdef USE_32K_CAPABLE_GYRO
{ "gyro_use_32khz", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_use_32khz) },

60
src/main/sensors/gyro.c
View file

@ -138,8 +138,14 @@ typedef struct gyroSensor_s {
filterApplyFnPtr notchFilterDynApplyFn;
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
// overflow and recovery
timeUs_t overflowTimeUs;
bool overflowDetected;
#ifdef USE_YAW_SPIN_RECOVERY
timeUs_t yawSpinTimeUs;
bool yawSpinDetected;
#endif // USE_YAW_SPIN_RECOVERY
} gyroSensor_t;
STATIC_UNIT_TESTED FAST_RAM gyroSensor_t gyroSensor1;
@ -202,6 +208,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_offset_yaw = 0,
.gyro_lma_depth = 0,
.gyro_lma_weight = 100,
.yaw_spin_recovery = true,
.yaw_spin_threshold = 1950,
);
@ -970,6 +978,9 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
if (overflowCheck & overflowAxisMask) {
gyroSensor->overflowDetected = true;
gyroSensor->overflowTimeUs = currentTimeUs;
#ifdef USE_YAW_SPIN_RECOVERY
gyroSensor->yawSpinDetected = false;
#endif // USE_YAW_SPIN_RECOVERY
}
#endif // SIMULATOR_BUILD
}
@ -979,6 +990,43 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
#endif // USE_GYRO_OVERFLOW_CHECK
}
static void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{
#ifdef USE_YAW_SPIN_RECOVERY
// if not in overflow mode, handle yaw spins above threshold
#ifdef USE_GYRO_OVERFLOW_CHECK
if (gyroSensor->overflowDetected) {
gyroSensor->yawSpinDetected = false;
return;
}
#endif // USE_GYRO_OVERFLOW_CHECK
if (gyroSensor->yawSpinDetected) {
const float yawSpinResetRate = gyroConfig()->yaw_spin_threshold - 100.0f;
if (abs(gyro.gyroADCf[Z]) < yawSpinResetRate) {
// testing whether 20ms of consecutive OK gyro yaw values is enough
if (cmpTimeUs(currentTimeUs, gyroSensor->yawSpinTimeUs) > 20000) {
gyroSensor->yawSpinDetected = false;
}
} else {
// reset the yaw spin time
gyroSensor->yawSpinTimeUs = currentTimeUs;
}
} else {
#ifndef SIMULATOR_BUILD
// check for spin on yaw axis only
const float yawSpinTriggerRate = gyroConfig()->yaw_spin_threshold;
if (abs(gyro.gyroADCf[Z]) > yawSpinTriggerRate) {
gyroSensor->yawSpinDetected = true;
gyroSensor->yawSpinTimeUs = currentTimeUs;
}
#endif // SIMULATOR_BUILD
}
#else
UNUSED(gyroSensor);
UNUSED(currentTimeUs);
#endif // USE_YAW_SPIN_RECOVERY
}
static FAST_CODE NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{
if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) {
@ -1019,6 +1067,11 @@ static FAST_CODE NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs
if (gyroConfig()->checkOverflow && !gyroHasOverflowProtection) {
checkForOverflow(gyroSensor, currentTimeUs);
}
if (gyroConfig()->yaw_spin_recovery) {
checkForYawSpin(gyroSensor, currentTimeUs);
}
if (gyroDebugMode == DEBUG_NONE) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch
@ -1195,6 +1248,13 @@ bool gyroOverflowDetected(void)
return gyroSensor1.overflowDetected;
}
#ifdef USE_YAW_SPIN_RECOVERY
bool gyroYawSpinDetected(void)
{
return gyroSensor1.yawSpinDetected;
}
#endif // USE_YAW_SPIN_RECOVERY
uint16_t gyroAbsRateDps(int axis)
{
return fabsf(gyro.gyroADCf[axis]);

4
src/main/sensors/gyro.h
View file

@ -101,6 +101,9 @@ typedef struct gyroConfig_s {
gyroOverflowCheck_e checkOverflow;
int16_t gyro_offset_yaw;
bool yaw_spin_recovery;
int16_t yaw_spin_threshold;
} gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig);
@ -122,5 +125,6 @@ void gyroReadTemperature(void);
int16_t gyroGetTemperature(void);
int16_t gyroRateDps(int axis);
bool gyroOverflowDetected(void);
bool gyroYawSpinDetected(void);
uint16_t gyroAbsRateDps(int axis);
uint8_t gyroReadRegister(uint8_t reg);

1
src/main/target/common_fc_pre.h
View file

@ -160,6 +160,7 @@
#define USE_EXTENDED_CMS_MENUS
#define USE_DSHOT_DMAR
#define USE_GYRO_OVERFLOW_CHECK
#define USE_YAW_SPIN_RECOVERY
#define USE_HUFFMAN
#define USE_MSP_DISPLAYPORT
#define USE_MSP_OVER_TELEMETRY