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

Merge pull request #9455 from etracer65/yaw_spin_recovery_auto 

Add "auto" mode to yaw spin recovery
This commit is contained in:
Michael Keller 2020-02-08 14:55:47 +13:00 committed by GitHub
parent d7c7919540 653fe308cb
commit 55bdf61b2d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 55 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/cli/settings.c
View File

@ -639,8 +639,8 @@ const clivalue_t valueTable[] = {
{ "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.minmaxUnsigned = { 500, 1950 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, yaw_spin_threshold) },
{ "yaw_spin_recovery", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON_AUTO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, yaw_spin_recovery) },
{ "yaw_spin_threshold", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { YAW_SPIN_RECOVERY_THRESHOLD_MIN, YAW_SPIN_RECOVERY_THRESHOLD_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, yaw_spin_threshold) },
#endif
#ifdef USE_MULTI_GYRO

6
src/main/fc/rc.c
View File

@ -53,6 +53,7 @@
#include "rx/rx.h"
#include "sensors/battery.h"
#include "sensors/gyro.h"
#include "rc.h"
@ -841,6 +842,11 @@ void initRcProcessing(void)
break;
}
#ifdef USE_YAW_SPIN_RECOVERY
const int maxYawRate = (int)applyRates(FD_YAW, 1.0f, 1.0f);
initYawSpinRecovery(maxYawRate);
#endif
}
bool rcSmoothingIsEnabled(void)

37
src/main/sensors/gyro.c
View File

@ -100,6 +100,8 @@ static FAST_RAM_ZERO_INIT uint8_t overflowAxisMask;
#endif
#ifdef USE_YAW_SPIN_RECOVERY
static FAST_RAM_ZERO_INIT bool yawSpinRecoveryEnabled;
static FAST_RAM_ZERO_INIT int yawSpinRecoveryThreshold;
static FAST_RAM_ZERO_INIT bool yawSpinDetected;
static FAST_RAM_ZERO_INIT timeUs_t yawSpinTimeUs;
#endif
@ -180,7 +182,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
gyroConfig->gyro_soft_notch_cutoff_2 = 0;
gyroConfig->checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES;
gyroConfig->gyro_offset_yaw = 0;
gyroConfig->yaw_spin_recovery = true;
gyroConfig->yaw_spin_recovery = YAW_SPIN_RECOVERY_AUTO;
gyroConfig->yaw_spin_threshold = 1950;
gyroConfig->dyn_lpf_gyro_min_hz = 200;
gyroConfig->dyn_lpf_gyro_max_hz = 500;
@ -998,7 +1000,7 @@ static FAST_CODE void checkForOverflow(timeUs_t currentTimeUs)
#ifdef USE_YAW_SPIN_RECOVERY
static FAST_CODE_NOINLINE void handleYawSpin(timeUs_t currentTimeUs)
{
const float yawSpinResetRate = gyroConfig()->yaw_spin_threshold - 100.0f;
const float yawSpinResetRate = yawSpinRecoveryThreshold - 100.0f;
if (fabsf(gyro.gyroADCf[Z]) < yawSpinResetRate) {
// testing whether 20ms of consecutive OK gyro yaw values is enough
if (cmpTimeUs(currentTimeUs, yawSpinTimeUs) > 20000) {
@ -1025,7 +1027,7 @@ static FAST_CODE void checkForYawSpin(timeUs_t currentTimeUs)
} else {
#ifndef SIMULATOR_BUILD
// check for spin on yaw axis only
if (abs((int)gyro.gyroADCf[Z]) > gyroConfig()->yaw_spin_threshold) {
if (abs((int)gyro.gyroADCf[Z]) > yawSpinRecoveryThreshold) {
yawSpinDetected = true;
yawSpinTimeUs = currentTimeUs;
}
@ -1181,7 +1183,7 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
#endif
#ifdef USE_YAW_SPIN_RECOVERY
if (gyroConfig()->yaw_spin_recovery) {
if (yawSpinRecoveryEnabled) {
checkForYawSpin(currentTimeUs);
}
#endif
@ -1310,3 +1312,30 @@ void dynLpfGyroUpdate(float throttle)
}
}
#endif
#ifdef USE_YAW_SPIN_RECOVERY
void initYawSpinRecovery(int maxYawRate)
{
bool enabledFlag;
int threshold;
switch (gyroConfig()->yaw_spin_recovery) {
case YAW_SPIN_RECOVERY_OFF:
enabledFlag = false;
threshold = YAW_SPIN_RECOVERY_THRESHOLD_MAX;
break;
case YAW_SPIN_RECOVERY_ON:
enabledFlag = true;
threshold = gyroConfig()->yaw_spin_threshold;
break;
case YAW_SPIN_RECOVERY_AUTO:
enabledFlag = true;
const int overshootAllowance = MAX(maxYawRate / 4, 200); // Allow a 25% or minimum 200dps overshoot tolerance
threshold = constrain(maxYawRate + overshootAllowance, YAW_SPIN_RECOVERY_THRESHOLD_MIN, YAW_SPIN_RECOVERY_THRESHOLD_MAX);
break;
}
yawSpinRecoveryEnabled = enabledFlag;
yawSpinRecoveryThreshold = threshold;
}
#endif

14
src/main/sensors/gyro.h
View File

@ -38,6 +38,11 @@
#define FILTER_FREQUENCY_MAX 4000 // maximum frequency for filter cutoffs (nyquist limit of 8K max sampling)
#ifdef USE_YAW_SPIN_RECOVERY
#define YAW_SPIN_RECOVERY_THRESHOLD_MIN 500
#define YAW_SPIN_RECOVERY_THRESHOLD_MAX 1950
#endif
typedef union gyroLowpassFilter_u {
pt1Filter_t pt1FilterState;
biquadFilter_t biquadFilterState;
@ -98,6 +103,12 @@ enum {
DYN_LPF_BIQUAD
};
typedef enum {
YAW_SPIN_RECOVERY_OFF,
YAW_SPIN_RECOVERY_ON,
YAW_SPIN_RECOVERY_AUTO
} yawSpinRecoveryMode_e;
#define GYRO_CONFIG_USE_GYRO_1 0
#define GYRO_CONFIG_USE_GYRO_2 1
#define GYRO_CONFIG_USE_GYRO_BOTH 2
@ -181,3 +192,6 @@ gyroDetectionFlags_t getGyroDetectionFlags(void);
float dynThrottle(float throttle);
void dynLpfGyroUpdate(float throttle);
#endif
#ifdef USE_YAW_SPIN_RECOVERY
void initYawSpinRecovery(int maxYawRate);
#endif