Merge pull request #9455 from etracer65/yaw_spin_recovery_auto
Add "auto" mode to yaw spin recovery
This commit is contained in:
commit
55bdf61b2d
|
@ -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
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue