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

Merge pull request #5706 from ctzsnooze/yaw_spin_fix2

Browse source 
Yaw Spin Recovery
This commit is contained in:
Michael Keller 2018-04-25 00:23:08 +12:00 • committed by GitHub
commit 5c8f330c33
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 129 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -58,6 +58,7 @@
#include "rx/rx.h"
#include "sensors/battery.h"
#include "sensors/gyro.h"
PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0);
@ -749,8 +750,19 @@ NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
constrainf(pidData[FD_ROLL].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
const float scaledAxisPidPitch =
constrainf(pidData[FD_PITCH].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
uint16_t yawPidSumLimit = currentPidProfile->pidSumLimitYaw;
#ifdef USE_YAW_SPIN_RECOVERY
const bool yawSpinDetected = gyroYawSpinDetected();
if (yawSpinDetected) {
yawPidSumLimit = PIDSUM_LIMIT_MAX; // Set to the maximum limit during yaw spin recovery to prevent limiting motor authority
}
#endif // USE_YAW_SPIN_RECOVERY
float scaledAxisPidYaw =
constrainf(pidData[FD_YAW].Sum, -currentPidProfile->pidSumLimitYaw, currentPidProfile->pidSumLimitYaw) / PID_MIXER_SCALING;
constrainf(pidData[FD_YAW].Sum, -yawPidSumLimit, yawPidSumLimit) / PID_MIXER_SCALING;
if (!mixerConfig()->yaw_motors_reversed) {
scaledAxisPidYaw = -scaledAxisPidYaw;
}
@ -763,6 +775,14 @@ NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
throttle = applyThrottleLimit(throttle);
}
#ifdef USE_YAW_SPIN_RECOVERY
// 50% throttle provides the maximum authority for yaw recovery when airmode is not active.
// When airmode is active the throttle setting doesn't impact recovery authority.
if (yawSpinDetected && !isAirmodeActive()) {
throttle = 0.5f; //
}
#endif // USE_YAW_SPIN_RECOVERY
// Find roll/pitch/yaw desired output
float motorMix[MAX_SUPPORTED_MOTORS];
float motorMixMax = 0, motorMixMin = 0;

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

@ -541,6 +541,10 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
const float tpaFactor = getThrottlePIDAttenuation();
const float motorMixRange = getMotorMixRange();
#ifdef USE_YAW_SPIN_RECOVERY
const bool yawSpinActive = gyroYawSpinDetected();
#endif
// Dynamic i component,
// gradually scale back integration when above windup point
const float dynCi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f) * dT * itermAccelerator;
@ -571,6 +575,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) && yawSpinActive) {
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 +630,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 (yawSpinActive) {
// 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 +646,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 (yawSpinActive) {
// 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;

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

@ -30,6 +30,8 @@
#define PID_SERVO_MIXER_SCALING 0.7f
#define PIDSUM_LIMIT 500
#define PIDSUM_LIMIT_YAW 400
#define PIDSUM_LIMIT_MIN 100
#define PIDSUM_LIMIT_MAX 1000
// Scaling factors for Pids for better tunable range in configurator for betaflight pid controller. The scaling is based on legacy pid controller or previous float
#define PTERM_SCALE 0.032029f

23
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) },
@ -726,8 +731,8 @@ const clivalue_t valueTable[] = {
{ "iterm_rotation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_rotation) },
{ "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) },
{ "iterm_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermLimit) },
{ "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 = { PIDSUM_LIMIT_MIN, PIDSUM_LIMIT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) },
{ "pidsum_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { PIDSUM_LIMIT_MIN, PIDSUM_LIMIT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) },
{ "yaw_lowpass_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lowpass_hz) },
{ "throttle_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, throttle_boost) },

59
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,42 @@ 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
if (abs(gyro.gyroADCf[Z]) > gyroConfig()->yaw_spin_threshold) {
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 +1066,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 +1247,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