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

Merge pull request #1847 from martinbudden/bf_issue1844_fix

Browse source 
Fixes issue 1844
This commit is contained in:
J Blackman 2016-12-18 09:05:55 +11:00 • committed by GitHub
commit 1746d8e532
3 changed files with 13 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/fc/fc_msp.c
View file

@ -1468,7 +1468,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
} }
// reinitialize the gyro filters with the new values // reinitialize the gyro filters with the new values
validateAndFixGyroConfig(); validateAndFixGyroConfig();
gyroInit(&masterConfig.gyroConfig); gyroInitFilters();
// reinitialize the PID filters with the new values // reinitialize the PID filters with the new values
pidInitFilters(&currentProfile->pidProfile); pidInitFilters(&currentProfile->pidProfile);
break; break;

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

@ -228,12 +228,6 @@ case GYRO_MPU9250:
bool gyroInit(const gyroConfig_t *gyroConfigToUse) bool gyroInit(const gyroConfig_t *gyroConfigToUse)
{ {
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
static firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT];
gyroConfig = gyroConfigToUse; gyroConfig = gyroConfigToUse;
memset(&gyro, 0, sizeof(gyro)); memset(&gyro, 0, sizeof(gyro));
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
@ -247,6 +241,17 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse)
gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation
gyro.dev.lpf = gyroConfig->gyro_lpf; gyro.dev.lpf = gyroConfig->gyro_lpf;
gyro.dev.init(&gyro.dev); gyro.dev.init(&gyro.dev);
gyroInitFilters();
return true;
}
void gyroInitFilters(void)
{
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
static firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT];
softLpfFilterApplyFn = nullFilterApply; softLpfFilterApplyFn = nullFilterApply;
notchFilter1ApplyFn = nullFilterApply; notchFilter1ApplyFn = nullFilterApply;
@ -291,7 +296,6 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse)
biquadFilterInit(notchFilter2[axis], gyroConfig->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH); biquadFilterInit(notchFilter2[axis], gyroConfig->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
} }
} }
return true;
} }
bool isGyroCalibrationComplete(void) bool isGyroCalibrationComplete(void)

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

@ -57,5 +57,6 @@ typedef struct gyroConfig_s {
void gyroSetCalibrationCycles(void); void gyroSetCalibrationCycles(void);
bool gyroInit(const gyroConfig_t *gyroConfigToUse); bool gyroInit(const gyroConfig_t *gyroConfigToUse);
void gyroInitFilters(void);
void gyroUpdate(void); void gyroUpdate(void);
bool isGyroCalibrationComplete(void); bool isGyroCalibrationComplete(void);