diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index 6dddfb286c..f0c35e35bb 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -58,6 +58,7 @@ typedef struct gyroDev_s { int16_t gyroADCRaw[XYZ_AXIS_COUNT]; int32_t gyroZero[XYZ_AXIS_COUNT]; int32_t gyroADC[XYZ_AXIS_COUNT]; // gyro data after calibration and alignment + int16_t temperature; uint8_t lpf; gyroRateKHz_e gyroRateKHz; uint8_t mpuDividerDrops; diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index cc23aed86c..1b5dbc9562 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -190,7 +190,7 @@ void mwArm(void) static bool firstArmingCalibrationWasCompleted; if (armingConfig()->gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) { - gyroSetCalibrationCycles(); + gyroStartCalibration(); armingCalibrationWasInitialised = true; firstArmingCalibrationWasCompleted = true; } diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index ccde4501b1..f64f911288 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -556,7 +556,7 @@ void init(void) if (mixerConfig()->mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } - gyroSetCalibrationCycles(); + gyroStartCalibration(); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 74fc3200d1..06208befd2 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -197,7 +197,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { // GYRO calibration - gyroSetCalibrationCycles(); + gyroStartCalibration(); #ifdef GPS if (feature(FEATURE_GPS)) { diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5c0b6e00cf..ba4ac5228c 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -71,8 +71,6 @@ gyro_t gyro; -STATIC_UNIT_TESTED gyroDev_t gyroDev0; -static int16_t gyroTemperature0; typedef struct gyroCalibration_s { int32_t g[XYZ_AXIS_COUNT]; @@ -80,14 +78,25 @@ typedef struct gyroCalibration_s { uint16_t calibratingG; } gyroCalibration_t; -STATIC_UNIT_TESTED gyroCalibration_t gyroCalibration; +typedef struct gyroSensor_s { + gyroDev_t gyroDev; + gyroCalibration_t gyroCalibration; + // gyro soft filter + filterApplyFnPtr softLpfFilterApplyFn; + biquadFilter_t gyroFilterLPFState[XYZ_AXIS_COUNT]; + pt1Filter_t gyroFilterPt1State[XYZ_AXIS_COUNT]; + firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT]; + void *softLpfFilterPtr[XYZ_AXIS_COUNT]; + // notch filters + filterApplyFnPtr notchFilter1ApplyFn; + biquadFilter_t notchFilter1[XYZ_AXIS_COUNT]; + filterApplyFnPtr notchFilter2ApplyFn; + biquadFilter_t notchFilter2[XYZ_AXIS_COUNT]; +} gyroSensor_t; -static filterApplyFnPtr softLpfFilterApplyFn; -static void *softLpfFilter[3]; -static filterApplyFnPtr notchFilter1ApplyFn; -static void *notchFilter1[3]; -static filterApplyFnPtr notchFilter2ApplyFn; -static void *notchFilter2[3]; +static gyroSensor_t gyroSensor0; + +static void gyroInitSensorFilters(gyroSensor_t *gyroSensor); #define DEBUG_GYRO_CALIBRATION 3 @@ -120,17 +129,17 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, const busDevice_t *gyroSensorBus(void) { - return &gyroDev0.bus; + return &gyroSensor0.gyroDev.bus; } const mpuConfiguration_t *gyroMpuConfiguration(void) { - return &gyroDev0.mpuConfiguration; + return &gyroSensor0.gyroDev.mpuConfiguration; } const mpuDetectionResult_t *gyroMpuDetectionResult(void) { - return &gyroDev0.mpuDetectionResult; + return &gyroSensor0.gyroDev.mpuDetectionResult; } STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) @@ -284,29 +293,29 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) return gyroHardware; } -bool gyroInit(void) +static bool gyroInitSensor(gyroSensor_t *gyroSensor) { - 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_ICM20601) || defined(USE_GYRO_SPI_ICM20689) #if defined(MPU_INT_EXTI) - gyroDev0.mpuIntExtiTag = IO_TAG(MPU_INT_EXTI); + gyroSensor->gyroDev.mpuIntExtiTag = IO_TAG(MPU_INT_EXTI); #elif defined(USE_HARDWARE_REVISION_DETECTION) - gyroDev0.mpuIntExtiTag = selectMPUIntExtiConfigByHardwareRevision(); + gyroSensor->gyroDev.mpuIntExtiTag = selectMPUIntExtiConfigByHardwareRevision(); #else - gyroDev0.mpuIntExtiTag = IO_TAG_NONE; -#endif + gyroSensor->gyroDev.mpuIntExtiTag = IO_TAG_NONE; +#endif // MPU_INT_EXTI #ifdef USE_DUAL_GYRO // set cnsPin using GYRO_n_CS_PIN defined in target.h - gyroDev0.bus.spi.csnPin = gyroConfig()->gyro_to_use == 0 ? IOGetByTag(IO_TAG(GYRO_0_CS_PIN)) : IOGetByTag(IO_TAG(GYRO_1_CS_PIN)); + gyroSensor->gyroDev.bus.spi.csnPin = gyroConfig()->gyro_to_use == 0 ? IOGetByTag(IO_TAG(GYRO_0_CS_PIN)) : IOGetByTag(IO_TAG(GYRO_1_CS_PIN)); #else - gyroDev0.bus.spi.csnPin = IO_NONE; // set cnsPin to IO_NONE so mpuDetect will set it according to value defined in target.h + gyroSensor->gyroDev.bus.spi.csnPin = IO_NONE; // set cnsPin to IO_NONE so mpuDetect will set it according to value defined in target.h #endif // USE_DUAL_GYRO - mpuDetect(&gyroDev0); - mpuResetFn = gyroDev0.mpuConfiguration.resetFn; // must be set after mpuDetect + mpuDetect(&gyroSensor->gyroDev); + mpuResetFn = gyroSensor->gyroDev.mpuConfiguration.resetFn; // must be set after mpuDetect #endif - const gyroSensor_e gyroHardware = gyroDetect(&gyroDev0); + + const gyroSensor_e gyroHardware = gyroDetect(&gyroSensor->gyroDev); if (gyroHardware == GYRO_NONE) { return false; } @@ -327,98 +336,105 @@ bool gyroInit(void) } // Must set gyro targetLooptime before gyroDev.init and initialisation of filters - gyro.targetLooptime = gyroSetSampleRate(&gyroDev0, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz); - gyroDev0.lpf = gyroConfig()->gyro_lpf; - gyroDev0.initFn(&gyroDev0); + gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz); + gyroSensor->gyroDev.lpf = gyroConfig()->gyro_lpf; + gyroSensor->gyroDev.initFn(&gyroSensor->gyroDev); if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { - gyroDev0.gyroAlign = gyroConfig()->gyro_align; + gyroSensor->gyroDev.gyroAlign = gyroConfig()->gyro_align; } - gyroInitFilters(); + gyroInitSensorFilters(gyroSensor); #ifdef USE_GYRO_DATA_ANALYSE gyroDataAnalyseInit(gyro.targetLooptime); #endif return true; } -void gyroInitFilterLpf(uint8_t lpfHz) +bool gyroInit(void) { - static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT]; - static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT]; - static firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT]; + memset(&gyro, 0, sizeof(gyro)); + return gyroInitSensor(&gyroSensor0); +} - softLpfFilterApplyFn = nullFilterApply; +void gyroInitFilterLpf(gyroSensor_t *gyroSensor, uint8_t lpfHz) +{ + gyroSensor->softLpfFilterApplyFn = nullFilterApply; const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed if (lpfHz && lpfHz <= gyroFrequencyNyquist) { // Initialisation needs to happen once samplingrate is known switch (gyroConfig()->gyro_soft_lpf_type) { case FILTER_BIQUAD: - softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; + gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { - softLpfFilter[axis] = &gyroFilterLPF[axis]; - biquadFilterInitLPF(softLpfFilter[axis], lpfHz, gyro.targetLooptime); + gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->gyroFilterLPFState[axis]; + biquadFilterInitLPF(gyroSensor->softLpfFilterPtr[axis], lpfHz, gyro.targetLooptime); } break; case FILTER_PT1: - softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; + gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; const float gyroDt = (float) gyro.targetLooptime * 0.000001f; for (int axis = 0; axis < 3; axis++) { - softLpfFilter[axis] = &gyroFilterPt1[axis]; - pt1FilterInit(softLpfFilter[axis], lpfHz, gyroDt); + gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->gyroFilterPt1State[axis]; + pt1FilterInit(gyroSensor->softLpfFilterPtr[axis], lpfHz, gyroDt); } break; default: - softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; + gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; for (int axis = 0; axis < 3; axis++) { - softLpfFilter[axis] = &gyroDenoiseState[axis]; - firFilterDenoiseInit(softLpfFilter[axis], lpfHz, gyro.targetLooptime); + gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->gyroDenoiseState[axis]; + firFilterDenoiseInit(gyroSensor->softLpfFilterPtr[axis], lpfHz, gyro.targetLooptime); } break; } } } -void gyroInitFilterNotch1(uint16_t notchHz, uint16_t notchCutoffHz) +void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz) { - static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT]; - - notchFilter1ApplyFn = nullFilterApply; + gyroSensor->notchFilter1ApplyFn = nullFilterApply; const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed if (notchHz && notchHz <= gyroFrequencyNyquist) { - notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; + gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz); for (int axis = 0; axis < 3; axis++) { - notchFilter1[axis] = &gyroFilterNotch[axis]; - biquadFilterInit(notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH); + biquadFilterInit(&gyroSensor->notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH); } } } -void gyroInitFilterNotch2(uint16_t notchHz, uint16_t notchCutoffHz) +void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz) { - static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT]; - notchFilter2ApplyFn = nullFilterApply; + gyroSensor->notchFilter2ApplyFn = nullFilterApply; const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed if (notchHz && notchHz <= gyroFrequencyNyquist) { - notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; + gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz); for (int axis = 0; axis < 3; axis++) { - notchFilter2[axis] = &gyroFilterNotch[axis]; - biquadFilterInit(notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH); + biquadFilterInit(&gyroSensor->notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH); } } } +static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) +{ + gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz); + gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); + gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); +} + void gyroInitFilters(void) { - gyroInitFilterLpf(gyroConfig()->gyro_soft_lpf_hz); - gyroInitFilterNotch1(gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); - gyroInitFilterNotch2(gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); + gyroInitSensorFilters(&gyroSensor0); +} + +bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor) +{ + return gyroSensor->gyroCalibration.calibratingG == 0; } bool isGyroCalibrationComplete(void) { - return gyroCalibration.calibratingG == 0; + return isGyroSensorCalibrationComplete(&gyroSensor0); } static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration) @@ -436,111 +452,69 @@ static bool isOnFirstGyroCalibrationCycle(const gyroCalibration_t *gyroCalibrati return gyroCalibration->calibratingG == gyroCalculateCalibratingCycles(); } -void gyroSetCalibrationCycles(void) +static void gyroSetCalibrationCycles(gyroSensor_t *gyroSensor) { - gyroCalibration.calibratingG = gyroCalculateCalibratingCycles(); + gyroSensor->gyroCalibration.calibratingG = gyroCalculateCalibratingCycles(); } -STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *gyroDev, gyroCalibration_t *gyroCalibration, uint8_t gyroMovementCalibrationThreshold) +void gyroStartCalibration(void) { - for (int axis = 0; axis < 3; axis++) { + gyroSetCalibrationCycles(&gyroSensor0); +} + +STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t gyroMovementCalibrationThreshold) +{ + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // Reset g[axis] at start of calibration - if (isOnFirstGyroCalibrationCycle(gyroCalibration)) { - gyroCalibration->g[axis] = 0; - devClear(&gyroCalibration->var[axis]); + if (isOnFirstGyroCalibrationCycle(&gyroSensor->gyroCalibration)) { + gyroSensor->gyroCalibration.g[axis] = 0; + devClear(&gyroSensor->gyroCalibration.var[axis]); // gyroZero is set to zero until calibration complete - gyroDev->gyroZero[axis] = 0; + gyroSensor->gyroDev.gyroZero[axis] = 0; } // Sum up CALIBRATING_GYRO_CYCLES readings - gyroCalibration->g[axis] += gyroDev->gyroADCRaw[axis]; - devPush(&gyroCalibration->var[axis], gyroDev->gyroADCRaw[axis]); + gyroSensor->gyroCalibration.g[axis] += gyroSensor->gyroDev.gyroADCRaw[axis]; + devPush(&gyroSensor->gyroCalibration.var[axis], gyroSensor->gyroDev.gyroADCRaw[axis]); - if (isOnFinalGyroCalibrationCycle(gyroCalibration)) { - const float stddev = devStandardDeviation(&gyroCalibration->var[axis]); + if (isOnFinalGyroCalibrationCycle(&gyroSensor->gyroCalibration)) { + const float stddev = devStandardDeviation(&gyroSensor->gyroCalibration.var[axis]); DEBUG_SET(DEBUG_GYRO, DEBUG_GYRO_CALIBRATION, lrintf(stddev)); // check deviation and startover in case the model was moved if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) { - gyroSetCalibrationCycles(); + gyroSetCalibrationCycles(gyroSensor); return; } - gyroDev->gyroZero[axis] = (gyroCalibration->g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles(); + gyroSensor->gyroDev.gyroZero[axis] = (gyroSensor->gyroCalibration.g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles(); } } - if (isOnFinalGyroCalibrationCycle(gyroCalibration)) { + if (isOnFinalGyroCalibrationCycle(&gyroSensor->gyroCalibration)) { schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics beeper(BEEPER_GYRO_CALIBRATED); } - gyroCalibration->calibratingG--; + --gyroSensor->gyroCalibration.calibratingG; } -#if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL) -static bool gyroUpdateISR(gyroDev_t* gyroDev) +void gyroUpdateSensor(gyroSensor_t *gyroSensor) { - if (!gyroDev->dataReady || !gyroDev->readFn(gyroDev)) { - return false; - } -#ifdef DEBUG_MPU_DATA_READY_INTERRUPT - debug[2] = (uint16_t)(micros() & 0xffff); -#endif - gyroDev->dataReady = false; - // move gyro data into 32-bit variables to avoid overflows in calculations - gyroDev->gyroADC[X] = (int32_t)gyroDev->gyroADCRaw[X] - (int32_t)gyroDev->gyroZero[X]; - gyroDev->gyroADC[Y] = (int32_t)gyroDev->gyroADCRaw[Y] - (int32_t)gyroDev->gyroZero[Y]; - gyroDev->gyroADC[Z] = (int32_t)gyroDev->gyroADCRaw[Z] - (int32_t)gyroDev->gyroZero[Z]; - - alignSensors(gyroDev->gyroADC, gyroDev->gyroAlign); - - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - // scale gyro output to degrees per second - float gyroADCf = (float)gyroDev->gyroADC[axis] * gyroDev->scale; - gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf); - gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf); - gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf); - gyro.gyroADCf[axis] = gyroADCf; - } -#ifdef USE_GYRO_DATA_ANALYSE - gyroDataAnalyse(gyroDev, &gyro); -#endif - return true; -} -#endif - -void gyroUpdate(void) -{ - // range: +/- 8192; +/- 2000 deg/sec - if (gyroDev0.updateFn) { - // if the gyro update function is set then return, since the gyro is read in gyroUpdateISR + if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) { return; } - if (!gyroDev0.readFn(&gyroDev0)) { - return; - } - gyroDev0.dataReady = false; + gyroSensor->gyroDev.dataReady = false; - if (isGyroCalibrationComplete()) { -#if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL) - // SPI-based gyro so can read and update in ISR - if (gyroConfig()->gyro_isr_update) { - mpuGyroSetIsrUpdate(&gyroDev0, gyroUpdateISR); - return; - } -#endif -#ifdef DEBUG_MPU_DATA_READY_INTERRUPT - debug[3] = (uint16_t)(micros() & 0xffff); -#endif + if (isGyroSensorCalibrationComplete(gyroSensor)) { // move gyro data into 32-bit variables to avoid overflows in calculations - gyroDev0.gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X]; - gyroDev0.gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y]; - gyroDev0.gyroADC[Z] = (int32_t)gyroDev0.gyroADCRaw[Z] - (int32_t)gyroDev0.gyroZero[Z]; + gyroSensor->gyroDev.gyroADC[X] = (int32_t)gyroSensor->gyroDev.gyroADCRaw[X] - (int32_t)gyroSensor->gyroDev.gyroZero[X]; + gyroSensor->gyroDev.gyroADC[Y] = (int32_t)gyroSensor->gyroDev.gyroADCRaw[Y] - (int32_t)gyroSensor->gyroDev.gyroZero[Y]; + gyroSensor->gyroDev.gyroADC[Z] = (int32_t)gyroSensor->gyroDev.gyroADCRaw[Z] - (int32_t)gyroSensor->gyroDev.gyroZero[Z]; - alignSensors(gyroDev0.gyroADC, gyroDev0.gyroAlign); + alignSensors(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign); } else { - performGyroCalibration(&gyroDev0, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold); + performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold); // Reset gyro values to zero to prevent other code from using uncalibrated data gyro.gyroADCf[X] = 0.0f; gyro.gyroADCf[Y] = 0.0f; @@ -551,37 +525,42 @@ void gyroUpdate(void) for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // scale gyro output to degrees per second - float gyroADCf = (float)gyroDev0.gyroADC[axis] * gyroDev0.scale; + float gyroADCf = (float)gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; // Apply LPF DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); - gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf); + gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf); // Apply Notch filtering DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf)); - gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf); - gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf); + gyroADCf = gyroSensor->notchFilter1ApplyFn(&gyroSensor->notchFilter1[axis], gyroADCf); + gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf); gyro.gyroADCf[axis] = gyroADCf; } #ifdef USE_GYRO_DATA_ANALYSE - gyroDataAnalyse(&gyroDev0, &gyro); + gyroDataAnalyse(&gyroSensor->gyroDev, &gyro); #endif } +void gyroUpdate(void) +{ + gyroUpdateSensor(&gyroSensor0); +} + void gyroReadTemperature(void) { - if (gyroDev0.temperatureFn) { - gyroDev0.temperatureFn(&gyroDev0, &gyroTemperature0); + if (gyroSensor0.gyroDev.temperatureFn) { + gyroSensor0.gyroDev.temperatureFn(&gyroSensor0.gyroDev, &gyroSensor0.gyroDev.temperature); } } int16_t gyroGetTemperature(void) { - return gyroTemperature0; + return gyroSensor0.gyroDev.temperature; } int16_t gyroRateDps(int axis) { - return lrintf(gyro.gyroADCf[axis] / gyroDev0.scale); + return lrintf(gyro.gyroADCf[axis] / gyroSensor0.gyroDev.scale); } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index ac46fe2324..0b2c0c36a4 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -73,7 +73,7 @@ struct mpuConfiguration_s; const struct mpuConfiguration_s *gyroMpuConfiguration(void); struct mpuDetectionResult_s; const struct mpuDetectionResult_s *gyroMpuDetectionResult(void); -void gyroSetCalibrationCycles(void); +void gyroStartCalibration(void); bool isGyroCalibrationComplete(void); void gyroReadTemperature(void); int16_t gyroGetTemperature(void);