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];
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;

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

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

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

@ -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

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) {
// GYRO calibration
gyroSetCalibrationCycles();
gyroStartCalibration();
#ifdef GPS
if (feature(FEATURE_GPS)) {

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

@ -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;
if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) {
return;
}
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
debug[2] = (uint16_t)(micros() & 0xffff);
#endif
gyroDev->dataReady = false;
gyroSensor->gyroDev.dataReady = false;
if (isGyroSensorCalibrationComplete(gyroSensor)) {
// 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];
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(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;
}
if (!gyroDev0.readFn(&gyroDev0)) {
return;
}
gyroDev0.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
// 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];
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);
}

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

@ -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);