Do runtime check for gyro slew filter
This commit is contained in:
parent
d4fb9d5bdc
commit
cbaad53c46
|
@ -76,6 +76,7 @@
|
||||||
|
|
||||||
gyro_t gyro;
|
gyro_t gyro;
|
||||||
static uint8_t gyroDebugMode;
|
static uint8_t gyroDebugMode;
|
||||||
|
static bool gyroUseSlewFilter;
|
||||||
|
|
||||||
|
|
||||||
typedef struct gyroCalibration_s {
|
typedef struct gyroCalibration_s {
|
||||||
|
@ -363,6 +364,18 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
switch (gyroHardware) {
|
||||||
|
case GYRO_ICM20601:
|
||||||
|
case GYRO_ICM20602:
|
||||||
|
case GYRO_ICM20608G:
|
||||||
|
case GYRO_ICM20689:
|
||||||
|
gyroUseSlewFilter = true;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
gyroUseSlewFilter = false;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
// Must set gyro targetLooptime before gyroDev.init and initialisation of filters
|
// Must set gyro targetLooptime before gyroDev.init and initialisation of filters
|
||||||
gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz);
|
gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz);
|
||||||
gyroSensor->gyroDev.lpf = gyroConfig()->gyro_lpf;
|
gyroSensor->gyroDev.lpf = gyroConfig()->gyro_lpf;
|
||||||
|
@ -604,6 +617,9 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t
|
||||||
int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
|
int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
|
||||||
{
|
{
|
||||||
int32_t newRawGyro = (int32_t)gyroSensor->gyroDev.gyroADCRaw[axis];
|
int32_t newRawGyro = (int32_t)gyroSensor->gyroDev.gyroADCRaw[axis];
|
||||||
|
if (gyroUseSlewFilter == false) {
|
||||||
|
return newRawGyro;
|
||||||
|
}
|
||||||
|
|
||||||
if (abs(newRawGyro - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) {
|
if (abs(newRawGyro - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) {
|
||||||
newRawGyro = gyroSensor->gyroDev.gyroADCRawPrevious[axis];
|
newRawGyro = gyroSensor->gyroDev.gyroADCRawPrevious[axis];
|
||||||
|
|
Loading…
Reference in New Issue