Allow gyro read and filter in ISR for SPI gyros
This commit is contained in:
parent
fb4559bfa3
commit
a1c14320bf
|
@ -40,6 +40,7 @@ typedef struct gyroDev_s {
|
|||
sensorGyroReadFuncPtr read; // read 3 axis data function
|
||||
sensorGyroReadDataFuncPtr temperature; // read temperature if available
|
||||
sensorGyroInterruptStatusFuncPtr intStatus;
|
||||
sensorGyroUpdateFuncPtr update;
|
||||
extiCallbackRec_t exti;
|
||||
float scale; // scalefactor
|
||||
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
|
||||
|
|
|
@ -22,6 +22,7 @@
|
|||
|
||||
#include "platform.h"
|
||||
|
||||
#include "build/atomic.h"
|
||||
#include "build/build_config.h"
|
||||
#include "build/debug.h"
|
||||
|
||||
|
@ -46,7 +47,6 @@
|
|||
#include "accgyro_spi_mpu9250.h"
|
||||
#include "accgyro_mpu.h"
|
||||
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
|
||||
mpuResetFuncPtr mpuReset;
|
||||
|
||||
|
@ -220,15 +220,20 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
|
|||
#if defined(MPU_INT_EXTI)
|
||||
static void mpuIntExtiHandler(extiCallbackRec_t *cb)
|
||||
{
|
||||
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
static uint32_t lastCalledAtUs = 0;
|
||||
const uint32_t nowUs = micros();
|
||||
debug[0] = (uint16_t)(nowUs - lastCalledAtUs);
|
||||
lastCalledAtUs = nowUs;
|
||||
#endif
|
||||
gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
|
||||
gyro->dataReady = true;
|
||||
|
||||
if (gyro->update) {
|
||||
gyro->update(gyro);
|
||||
}
|
||||
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
static uint32_t lastCalledAt = 0;
|
||||
uint32_t now = micros();
|
||||
uint32_t callDelta = now - lastCalledAt;
|
||||
debug[0] = callDelta;
|
||||
lastCalledAt = now;
|
||||
const uint32_t now2Us = micros();
|
||||
debug[1] = (uint16_t)(now2Us - nowUs);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
@ -296,6 +301,13 @@ bool mpuAccRead(accDev_t *acc)
|
|||
return true;
|
||||
}
|
||||
|
||||
void mpuGyroSetIsrUpdate(gyroDev_t *gyro, sensorGyroUpdateFuncPtr updateFn)
|
||||
{
|
||||
ATOMIC_BLOCK(NVIC_PRIO_MPU_INT_EXTI) {
|
||||
gyro->update = updateFn;
|
||||
}
|
||||
}
|
||||
|
||||
bool mpuGyroRead(gyroDev_t *gyro)
|
||||
{
|
||||
uint8_t data[6];
|
||||
|
|
|
@ -18,6 +18,13 @@
|
|||
#pragma once
|
||||
|
||||
#include "exti.h"
|
||||
#include "sensor.h"
|
||||
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
|
||||
#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
|
||||
#define GYRO_USES_SPI
|
||||
#endif
|
||||
|
||||
// MPU6050
|
||||
#define MPU_RA_WHO_AM_I 0x75
|
||||
|
@ -190,3 +197,5 @@ bool mpuAccRead(struct accDev_s *acc);
|
|||
bool mpuGyroRead(struct gyroDev_s *gyro);
|
||||
mpuDetectionResult_t *mpuDetect(struct gyroDev_s *gyro);
|
||||
bool mpuCheckDataReady(struct gyroDev_s *gyro);
|
||||
void mpuGyroSetIsrUpdate(struct gyroDev_s *gyro, sensorGyroUpdateFuncPtr updateFn);
|
||||
|
||||
|
|
|
@ -38,5 +38,6 @@ typedef bool (*sensorAccReadFuncPtr)(struct accDev_s *acc);
|
|||
struct gyroDev_s;
|
||||
typedef void (*sensorGyroInitFuncPtr)(struct gyroDev_s *gyro);
|
||||
typedef bool (*sensorGyroReadFuncPtr)(struct gyroDev_s *gyro);
|
||||
typedef bool (*sensorGyroUpdateFuncPtr)(struct gyroDev_s *gyro);
|
||||
typedef bool (*sensorGyroReadDataFuncPtr)(struct gyroDev_s *gyro, int16_t *data);
|
||||
typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyroDev_s *gyro);
|
||||
|
|
|
@ -601,6 +601,9 @@ const clivalue_t valueTable[] = {
|
|||
|
||||
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
|
||||
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyro_sync_denom, .config.minmax = { 1, 8 } },
|
||||
#if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL)
|
||||
{ "gyro_isr_update", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_isr_update, .config.lookup = { TABLE_OFF_ON } },
|
||||
#endif
|
||||
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
|
||||
{ "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyro_soft_lpf_hz, .config.minmax = { 0, 255 } },
|
||||
{ "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } },
|
||||
|
|
|
@ -364,20 +364,62 @@ static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
|
|||
|
||||
}
|
||||
|
||||
#if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL)
|
||||
static bool gyroUpdateISR(gyroDev_t* gyroDev)
|
||||
{
|
||||
if (!gyroDev->dataReady || !gyroDev->read(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
|
||||
gyroADC[X] = gyroDev->gyroADCRaw[X];
|
||||
gyroADC[Y] = gyroDev->gyroADCRaw[Y];
|
||||
gyroADC[Z] = gyroDev->gyroADCRaw[Z];
|
||||
|
||||
alignSensors(gyroADC, gyroDev->gyroAlign);
|
||||
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
gyroADC[axis] -= gyroZero[axis];
|
||||
// scale gyro output to degrees per second
|
||||
float gyroADCf = (float)gyroADC[axis] * gyroDev->scale;
|
||||
gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf);
|
||||
gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
|
||||
gyro.gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
||||
void gyroUpdate(void)
|
||||
{
|
||||
// range: +/- 8192; +/- 2000 deg/sec
|
||||
if (!gyro.dev.read(&gyro.dev)) {
|
||||
if (!gyro.dev.dataReady || !gyro.dev.read(&gyro.dev)) {
|
||||
return;
|
||||
}
|
||||
const bool calibrationComplete = isGyroCalibrationComplete();
|
||||
if (calibrationComplete) {
|
||||
#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(&gyro.dev, gyroUpdateISR);
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
debug[3] = (uint16_t)(micros() & 0xffff);
|
||||
#endif
|
||||
}
|
||||
gyro.dev.dataReady = false;
|
||||
// move gyro data into 32-bit variables to avoid overflows in calculations
|
||||
gyroADC[X] = gyro.dev.gyroADCRaw[X];
|
||||
gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
|
||||
gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
|
||||
|
||||
alignSensors(gyroADC, gyro.dev.gyroAlign);
|
||||
|
||||
const bool calibrationComplete = isGyroCalibrationComplete();
|
||||
if (!calibrationComplete) {
|
||||
performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold);
|
||||
}
|
||||
|
@ -385,17 +427,17 @@ void gyroUpdate(void)
|
|||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
gyroADC[axis] -= gyroZero[axis];
|
||||
// scale gyro output to degrees per second
|
||||
gyro.gyroADCf[axis] = (float)gyroADC[axis] * gyro.dev.scale;
|
||||
float gyroADCf = (float)gyroADC[axis] * gyro.dev.scale;
|
||||
|
||||
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyro.gyroADCf[axis]));
|
||||
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
|
||||
|
||||
gyro.gyroADCf[axis] = softLpfFilterApplyFn(softLpfFilter[axis], gyro.gyroADCf[axis]);
|
||||
gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf);
|
||||
|
||||
DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyro.gyroADCf[axis]));
|
||||
DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf));
|
||||
|
||||
gyro.gyroADCf[axis] = notchFilter1ApplyFn(notchFilter1[axis], gyro.gyroADCf[axis]);
|
||||
gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
|
||||
|
||||
gyro.gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyro.gyroADCf[axis]);
|
||||
gyro.gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
|
||||
|
||||
if (!calibrationComplete) {
|
||||
gyroADC[axis] = lrintf(gyro.gyroADCf[axis] / gyro.dev.scale);
|
||||
|
|
|
@ -51,6 +51,7 @@ typedef struct gyroConfig_s {
|
|||
uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
|
||||
uint8_t gyro_soft_lpf_type;
|
||||
uint8_t gyro_soft_lpf_hz;
|
||||
bool gyro_isr_update;
|
||||
uint16_t gyro_soft_notch_hz_1;
|
||||
uint16_t gyro_soft_notch_cutoff_1;
|
||||
uint16_t gyro_soft_notch_hz_2;
|
||||
|
|
Loading…
Reference in New Issue