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

Allow gyro read and filter in ISR for SPI gyros

This commit is contained in:
Martin Budden 2017-01-05 12:22:49 +00:00
parent fb4559bfa3
commit a1c14320bf
7 changed files with 84 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

26
src/main/drivers/accgyro_mpu.c
View File

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

9
src/main/drivers/accgyro_mpu.h
View File

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

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

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

3
src/main/fc/serial_cli.c
View File

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

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

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

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

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