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

Further work on multiple gyro support

This commit is contained in:
Martin Budden 2017-05-07 19:36:29 +01:00
parent a518531c3f
commit 4be1e31d01
6 changed files with 124 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/drivers/accgyro/accgyro.h
View File

@ -58,6 +58,7 @@ typedef struct gyroDev_s {
int16_t gyroADCRaw[XYZ_AXIS_COUNT]; int16_t gyroADCRaw[XYZ_AXIS_COUNT];
int32_t gyroZero[XYZ_AXIS_COUNT]; int32_t gyroZero[XYZ_AXIS_COUNT];
int32_t gyroADC[XYZ_AXIS_COUNT]; // gyro data after calibration and alignment int32_t gyroADC[XYZ_AXIS_COUNT]; // gyro data after calibration and alignment
int16_t temperature;
uint8_t lpf; uint8_t lpf;
gyroRateKHz_e gyroRateKHz; gyroRateKHz_e gyroRateKHz;
uint8_t mpuDividerDrops; uint8_t mpuDividerDrops;

2
src/main/fc/fc_core.c
View File

@ -190,7 +190,7 @@ void mwArm(void)
static bool firstArmingCalibrationWasCompleted; static bool firstArmingCalibrationWasCompleted;
if (armingConfig()->gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) { if (armingConfig()->gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
gyroSetCalibrationCycles(); gyroStartCalibration();
armingCalibrationWasInitialised = true; armingCalibrationWasInitialised = true;
firstArmingCalibrationWasCompleted = true; firstArmingCalibrationWasCompleted = true;
} }

2
src/main/fc/fc_init.c
View File

@ -556,7 +556,7 @@ void init(void)
if (mixerConfig()->mixerMode == MIXER_GIMBAL) { if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
} }
gyroSetCalibrationCycles(); gyroStartCalibration();
#ifdef BARO #ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif #endif

2
src/main/fc/rc_controls.c
View File

@ -197,7 +197,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
// GYRO calibration // GYRO calibration
gyroSetCalibrationCycles(); gyroStartCalibration();
#ifdef GPS #ifdef GPS
if (feature(FEATURE_GPS)) { if (feature(FEATURE_GPS)) {

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

@ -71,8 +71,6 @@
gyro_t gyro; gyro_t gyro;
STATIC_UNIT_TESTED gyroDev_t gyroDev0;
static int16_t gyroTemperature0;
typedef struct gyroCalibration_s { typedef struct gyroCalibration_s {
int32_t g[XYZ_AXIS_COUNT]; int32_t g[XYZ_AXIS_COUNT];
@ -80,14 +78,25 @@ typedef struct gyroCalibration_s {
uint16_t calibratingG; uint16_t calibratingG;
} gyroCalibration_t; } 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 gyroSensor_t gyroSensor0;
static void *softLpfFilter[3];
static filterApplyFnPtr notchFilter1ApplyFn; static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
static void *notchFilter1[3];
static filterApplyFnPtr notchFilter2ApplyFn;
static void *notchFilter2[3];
#define DEBUG_GYRO_CALIBRATION 3 #define DEBUG_GYRO_CALIBRATION 3
@ -120,17 +129,17 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
const busDevice_t *gyroSensorBus(void) const busDevice_t *gyroSensorBus(void)
{ {
return &gyroDev0.bus; return &gyroSensor0.gyroDev.bus;
} }
const mpuConfiguration_t *gyroMpuConfiguration(void) const mpuConfiguration_t *gyroMpuConfiguration(void)
{ {
return &gyroDev0.mpuConfiguration; return &gyroSensor0.gyroDev.mpuConfiguration;
} }
const mpuDetectionResult_t *gyroMpuDetectionResult(void) const mpuDetectionResult_t *gyroMpuDetectionResult(void)
{ {
return &gyroDev0.mpuDetectionResult; return &gyroSensor0.gyroDev.mpuDetectionResult;
} }
STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev)
@ -284,29 +293,29 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev)
return gyroHardware; 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(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) #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) #elif defined(USE_HARDWARE_REVISION_DETECTION)
gyroDev0.mpuIntExtiTag = selectMPUIntExtiConfigByHardwareRevision(); gyroSensor->gyroDev.mpuIntExtiTag = selectMPUIntExtiConfigByHardwareRevision();
#else #else
gyroDev0.mpuIntExtiTag = IO_TAG_NONE; gyroSensor->gyroDev.mpuIntExtiTag = IO_TAG_NONE;
#endif #endif // MPU_INT_EXTI
#ifdef USE_DUAL_GYRO #ifdef USE_DUAL_GYRO
// set cnsPin using GYRO_n_CS_PIN defined in target.h // 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 #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 #endif // USE_DUAL_GYRO
mpuDetect(&gyroDev0); mpuDetect(&gyroSensor->gyroDev);
mpuResetFn = gyroDev0.mpuConfiguration.resetFn; // must be set after mpuDetect mpuResetFn = gyroSensor->gyroDev.mpuConfiguration.resetFn; // must be set after mpuDetect
#endif #endif
const gyroSensor_e gyroHardware = gyroDetect(&gyroDev0);
const gyroSensor_e gyroHardware = gyroDetect(&gyroSensor->gyroDev);
if (gyroHardware == GYRO_NONE) { if (gyroHardware == GYRO_NONE) {
return false; return false;
} }
@ -327,98 +336,105 @@ bool gyroInit(void)
} }
// 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(&gyroDev0, 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);
gyroDev0.lpf = gyroConfig()->gyro_lpf; gyroSensor->gyroDev.lpf = gyroConfig()->gyro_lpf;
gyroDev0.initFn(&gyroDev0); gyroSensor->gyroDev.initFn(&gyroSensor->gyroDev);
if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { 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 #ifdef USE_GYRO_DATA_ANALYSE
gyroDataAnalyseInit(gyro.targetLooptime); gyroDataAnalyseInit(gyro.targetLooptime);
#endif #endif
return true; return true;
} }
void gyroInitFilterLpf(uint8_t lpfHz) bool gyroInit(void)
{ {
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT]; memset(&gyro, 0, sizeof(gyro));
static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT]; return gyroInitSensor(&gyroSensor0);
static firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT]; }
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 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 if (lpfHz && lpfHz <= gyroFrequencyNyquist) { // Initialisation needs to happen once samplingrate is known
switch (gyroConfig()->gyro_soft_lpf_type) { switch (gyroConfig()->gyro_soft_lpf_type) {
case FILTER_BIQUAD: case FILTER_BIQUAD:
softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
softLpfFilter[axis] = &gyroFilterLPF[axis]; gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->gyroFilterLPFState[axis];
biquadFilterInitLPF(softLpfFilter[axis], lpfHz, gyro.targetLooptime); biquadFilterInitLPF(gyroSensor->softLpfFilterPtr[axis], lpfHz, gyro.targetLooptime);
} }
break; break;
case FILTER_PT1: case FILTER_PT1:
softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
const float gyroDt = (float) gyro.targetLooptime * 0.000001f; const float gyroDt = (float) gyro.targetLooptime * 0.000001f;
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
softLpfFilter[axis] = &gyroFilterPt1[axis]; gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->gyroFilterPt1State[axis];
pt1FilterInit(softLpfFilter[axis], lpfHz, gyroDt); pt1FilterInit(gyroSensor->softLpfFilterPtr[axis], lpfHz, gyroDt);
} }
break; break;
default: default:
softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
softLpfFilter[axis] = &gyroDenoiseState[axis]; gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->gyroDenoiseState[axis];
firFilterDenoiseInit(softLpfFilter[axis], lpfHz, gyro.targetLooptime); firFilterDenoiseInit(gyroSensor->softLpfFilterPtr[axis], lpfHz, gyro.targetLooptime);
} }
break; 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]; gyroSensor->notchFilter1ApplyFn = nullFilterApply;
notchFilter1ApplyFn = nullFilterApply;
const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
if (notchHz && notchHz <= gyroFrequencyNyquist) { if (notchHz && notchHz <= gyroFrequencyNyquist) {
notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz); const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
notchFilter1[axis] = &gyroFilterNotch[axis]; biquadFilterInit(&gyroSensor->notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
biquadFilterInit(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 const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
if (notchHz && notchHz <= gyroFrequencyNyquist) { if (notchHz && notchHz <= gyroFrequencyNyquist) {
notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz); const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
notchFilter2[axis] = &gyroFilterNotch[axis]; biquadFilterInit(&gyroSensor->notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
biquadFilterInit(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) void gyroInitFilters(void)
{ {
gyroInitFilterLpf(gyroConfig()->gyro_soft_lpf_hz); gyroInitSensorFilters(&gyroSensor0);
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);
bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
{
return gyroSensor->gyroCalibration.calibratingG == 0;
} }
bool isGyroCalibrationComplete(void) bool isGyroCalibrationComplete(void)
{ {
return gyroCalibration.calibratingG == 0; return isGyroSensorCalibrationComplete(&gyroSensor0);
} }
static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration) static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration)
@ -436,111 +452,69 @@ static bool isOnFirstGyroCalibrationCycle(const gyroCalibration_t *gyroCalibrati
return gyroCalibration->calibratingG == gyroCalculateCalibratingCycles(); 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 // Reset g[axis] at start of calibration
if (isOnFirstGyroCalibrationCycle(gyroCalibration)) { if (isOnFirstGyroCalibrationCycle(&gyroSensor->gyroCalibration)) {
gyroCalibration->g[axis] = 0; gyroSensor->gyroCalibration.g[axis] = 0;
devClear(&gyroCalibration->var[axis]); devClear(&gyroSensor->gyroCalibration.var[axis]);
// gyroZero is set to zero until calibration complete // gyroZero is set to zero until calibration complete
gyroDev->gyroZero[axis] = 0; gyroSensor->gyroDev.gyroZero[axis] = 0;
} }
// Sum up CALIBRATING_GYRO_CYCLES readings // Sum up CALIBRATING_GYRO_CYCLES readings
gyroCalibration->g[axis] += gyroDev->gyroADCRaw[axis]; gyroSensor->gyroCalibration.g[axis] += gyroSensor->gyroDev.gyroADCRaw[axis];
devPush(&gyroCalibration->var[axis], gyroDev->gyroADCRaw[axis]); devPush(&gyroSensor->gyroCalibration.var[axis], gyroSensor->gyroDev.gyroADCRaw[axis]);
if (isOnFinalGyroCalibrationCycle(gyroCalibration)) { if (isOnFinalGyroCalibrationCycle(&gyroSensor->gyroCalibration)) {
const float stddev = devStandardDeviation(&gyroCalibration->var[axis]); const float stddev = devStandardDeviation(&gyroSensor->gyroCalibration.var[axis]);
DEBUG_SET(DEBUG_GYRO, DEBUG_GYRO_CALIBRATION, lrintf(stddev)); DEBUG_SET(DEBUG_GYRO, DEBUG_GYRO_CALIBRATION, lrintf(stddev));
// check deviation and startover in case the model was moved // check deviation and startover in case the model was moved
if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) { if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
gyroSetCalibrationCycles(); gyroSetCalibrationCycles(gyroSensor);
return; 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 schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
beeper(BEEPER_GYRO_CALIBRATED); beeper(BEEPER_GYRO_CALIBRATED);
} }
gyroCalibration->calibratingG--; --gyroSensor->gyroCalibration.calibratingG;
} }
#if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL) void gyroUpdateSensor(gyroSensor_t *gyroSensor)
static bool gyroUpdateISR(gyroDev_t* gyroDev)
{ {
if (!gyroDev->dataReady || !gyroDev->readFn(gyroDev)) { if (!gyroSensor->gyroDev.readFn(&gyroSensor->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
return; return;
} }
if (!gyroDev0.readFn(&gyroDev0)) { gyroSensor->gyroDev.dataReady = false;
return;
}
gyroDev0.dataReady = false;
if (isGyroCalibrationComplete()) { if (isGyroSensorCalibrationComplete(gyroSensor)) {
#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
// move gyro data into 32-bit variables to avoid overflows in calculations // 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]; gyroSensor->gyroDev.gyroADC[X] = (int32_t)gyroSensor->gyroDev.gyroADCRaw[X] - (int32_t)gyroSensor->gyroDev.gyroZero[X];
gyroDev0.gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y]; gyroSensor->gyroDev.gyroADC[Y] = (int32_t)gyroSensor->gyroDev.gyroADCRaw[Y] - (int32_t)gyroSensor->gyroDev.gyroZero[Y];
gyroDev0.gyroADC[Z] = (int32_t)gyroDev0.gyroADCRaw[Z] - (int32_t)gyroDev0.gyroZero[Z]; 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 { } else {
performGyroCalibration(&gyroDev0, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold); performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold);
// Reset gyro values to zero to prevent other code from using uncalibrated data // Reset gyro values to zero to prevent other code from using uncalibrated data
gyro.gyroADCf[X] = 0.0f; gyro.gyroADCf[X] = 0.0f;
gyro.gyroADCf[Y] = 0.0f; gyro.gyroADCf[Y] = 0.0f;
@ -551,37 +525,42 @@ void gyroUpdate(void)
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// scale gyro output to degrees per second // 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 // Apply LPF
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf); gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf);
// Apply Notch filtering // Apply Notch filtering
DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf)); DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf));
gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf); gyroADCf = gyroSensor->notchFilter1ApplyFn(&gyroSensor->notchFilter1[axis], gyroADCf);
gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf); gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf);
gyro.gyroADCf[axis] = gyroADCf; gyro.gyroADCf[axis] = gyroADCf;
} }
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
gyroDataAnalyse(&gyroDev0, &gyro); gyroDataAnalyse(&gyroSensor->gyroDev, &gyro);
#endif #endif
} }
void gyroUpdate(void)
{
gyroUpdateSensor(&gyroSensor0);
}
void gyroReadTemperature(void) void gyroReadTemperature(void)
{ {
if (gyroDev0.temperatureFn) { if (gyroSensor0.gyroDev.temperatureFn) {
gyroDev0.temperatureFn(&gyroDev0, &gyroTemperature0); gyroSensor0.gyroDev.temperatureFn(&gyroSensor0.gyroDev, &gyroSensor0.gyroDev.temperature);
} }
} }
int16_t gyroGetTemperature(void) int16_t gyroGetTemperature(void)
{ {
return gyroTemperature0; return gyroSensor0.gyroDev.temperature;
} }
int16_t gyroRateDps(int axis) int16_t gyroRateDps(int axis)
{ {
return lrintf(gyro.gyroADCf[axis] / gyroDev0.scale); return lrintf(gyro.gyroADCf[axis] / gyroSensor0.gyroDev.scale);
} }

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

@ -73,7 +73,7 @@ struct mpuConfiguration_s;
const struct mpuConfiguration_s *gyroMpuConfiguration(void); const struct mpuConfiguration_s *gyroMpuConfiguration(void);
struct mpuDetectionResult_s; struct mpuDetectionResult_s;
const struct mpuDetectionResult_s *gyroMpuDetectionResult(void); const struct mpuDetectionResult_s *gyroMpuDetectionResult(void);
void gyroSetCalibrationCycles(void); void gyroStartCalibration(void);
bool isGyroCalibrationComplete(void); bool isGyroCalibrationComplete(void);
void gyroReadTemperature(void); void gyroReadTemperature(void);
int16_t gyroGetTemperature(void); int16_t gyroGetTemperature(void);