Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 22:35:23 +03:00
Code Issues Wiki Activity

Merge pull request #2014 from martinbudden/bf_gyro_isr

Browse source 
Allow gyro read and filter in ISR for SPI gyros
This commit is contained in:
borisbstyle 2017-01-06 00:05:57 +01:00 • committed by GitHub
commit f0fc42b7f5
19 changed files with 193 additions and 51 deletions
Download patch file Download diff file Expand all files Collapse all files

15
src/main/drivers/accgyro.h
View file

@ -26,6 +26,10 @@
#define MPU_I2C_INSTANCE I2C_DEVICE
#endif
#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
#define GYRO_SUPPORTS_32KHZ
#endif
#define GYRO_LPF_256HZ 0
#define GYRO_LPF_188HZ 1
#define GYRO_LPF_98HZ 2
@ -35,15 +39,24 @@
#define GYRO_LPF_5HZ 6
#define GYRO_LPF_NONE 7
typedef enum {
GYRO_RATE_1_kHz,
GYRO_RATE_8_kHz,
GYRO_RATE_32_kHz,
} gyroRateKHz_e;
typedef struct gyroDev_s {
sensorGyroInitFuncPtr init; // initialize function
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];
uint16_t lpf;
uint8_t lpf;
gyroRateKHz_e gyroRateKHz;
uint8_t mpuDividerDrops;
volatile bool dataReady;
sensor_align_e gyroAlign;
mpuDetectionResult_t mpuDetectionResult;

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

2
src/main/drivers/accgyro_mpu6050.c
View file

@ -82,7 +82,7 @@ static void mpu6050GyroInit(gyroDev_t *gyro)
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec

5
src/main/drivers/accgyro_mpu6500.c
View file

@ -63,13 +63,14 @@ void mpu6500GyroInit(gyroDev_t *gyro)
delay(100);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, raGyroConfigData);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
delay(100);
// Data ready interrupt configuration

5
src/main/drivers/accgyro_spi_icm20689.c
View file

@ -138,13 +138,14 @@ void icm20689GyroInit(gyroDev_t *gyro)
// delay(100);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, raGyroConfigData);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
delay(100);
// Data ready interrupt configuration

8
src/main/drivers/accgyro_spi_mpu6000.c
View file

@ -45,7 +45,7 @@
#include "accgyro_spi_mpu6000.h"
static void mpu6000AccAndGyroInit(void);
static void mpu6000AccAndGyroInit(gyroDev_t *gyro);
static bool mpuSpi6000InitDone = false;
@ -128,7 +128,7 @@ void mpu6000SpiGyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
mpu6000AccAndGyroInit();
mpu6000AccAndGyroInit(gyro);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
@ -201,7 +201,7 @@ bool mpu6000SpiDetect(void)
return false;
}
static void mpu6000AccAndGyroInit(void)
static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
{
if (mpuSpi6000InitDone) {
return;
@ -229,7 +229,7 @@ static void mpu6000AccAndGyroInit(void)
// Accel Sample Rate 1kHz
// Gyroscope Output Rate = 1kHz when the DLPF is enabled
mpu6000WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops());
mpu6000WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
delayMicroseconds(15);
// Gyro +/- 1000 DPS Full Scale

18
src/main/drivers/accgyro_spi_mpu9250.c
View file

@ -46,7 +46,7 @@
#include "accgyro_mpu.h"
#include "accgyro_spi_mpu9250.h"
static void mpu9250AccAndGyroInit(uint8_t lpf);
static void mpu9250AccAndGyroInit(gyroDev_t *gyro);
static bool mpuSpi9250InitDone = false;
@ -100,7 +100,7 @@ void mpu9250SpiGyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
mpu9250AccAndGyroInit(gyro->lpf);
mpu9250AccAndGyroInit(gyro);
spiResetErrorCounter(MPU9250_SPI_INSTANCE);
@ -140,7 +140,7 @@ bool verifympu9250WriteRegister(uint8_t reg, uint8_t data)
return false;
}
static void mpu9250AccAndGyroInit(uint8_t lpf) {
static void mpu9250AccAndGyroInit(gyroDev_t *gyro) {
if (mpuSpi9250InitDone) {
return;
@ -153,17 +153,19 @@ static void mpu9250AccAndGyroInit(uint8_t lpf) {
verifympu9250WriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED); //Fchoice_b defaults to 00 which makes fchoice 11
//Fchoice_b defaults to 00 which makes fchoice 11
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, raGyroConfigData);
if (lpf == 4) {
if (gyro->lpf == 4) {
verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF
} else if (lpf < 4) {
} else if (gyro->lpf < 4) {
verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
} else if (lpf > 4) {
} else if (gyro->lpf > 4) {
verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF
}
verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
verifympu9250WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN

30
src/main/drivers/gyro_sync.c
View file

@ -14,7 +14,6 @@
#include "accgyro.h"
#include "gyro_sync.h"
static uint8_t mpuDividerDrops;
bool gyroSyncCheckUpdate(gyroDev_t *gyro)
{
@ -23,24 +22,37 @@ bool gyroSyncCheckUpdate(gyroDev_t *gyro)
return gyro->intStatus(gyro);
}
uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator)
uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenominator, bool gyro_use_32khz)
{
int gyroSamplePeriod;
#ifndef GYRO_SUPPORTS_32KHZ
if (gyro_use_32khz) {
gyro_use_32khz = false;
}
#endif
float gyroSamplePeriod;
if (lpf == GYRO_LPF_256HZ || lpf == GYRO_LPF_NONE) {
gyroSamplePeriod = 125;
if (gyro_use_32khz) {
gyro->gyroRateKHz = GYRO_RATE_32_kHz;
gyroSamplePeriod = 31.5f;
} else {
gyro->gyroRateKHz = GYRO_RATE_8_kHz;
gyroSamplePeriod = 125.0f;
}
} else {
gyroSamplePeriod = 1000;
gyro->gyroRateKHz = GYRO_RATE_1_kHz;
gyroSamplePeriod = 1000.0f;
gyroSyncDenominator = 1; // Always full Sampling 1khz
}
// calculate gyro divider and targetLooptime (expected cycleTime)
mpuDividerDrops = gyroSyncDenominator - 1;
const uint32_t targetLooptime = gyroSyncDenominator * gyroSamplePeriod;
gyro->mpuDividerDrops = gyroSyncDenominator - 1;
const uint32_t targetLooptime = (uint32_t)(gyroSyncDenominator * gyroSamplePeriod);
return targetLooptime;
}
uint8_t gyroMPU6xxxGetDividerDrops(void)
uint8_t gyroMPU6xxxGetDividerDrops(const gyroDev_t *gyro)
{
return mpuDividerDrops;
return gyro->mpuDividerDrops;
}

8
src/main/drivers/gyro_sync.h
View file

@ -5,8 +5,8 @@
* Author: borisb
*/
struct gyroDev_s;
#include "drivers/accgyro.h"
bool gyroSyncCheckUpdate(struct gyroDev_s *gyro);
uint8_t gyroMPU6xxxGetDividerDrops(void);
uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator);
bool gyroSyncCheckUpdate(gyroDev_t *gyro);
uint8_t gyroMPU6xxxGetDividerDrops(const gyroDev_t *gyro);
uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenominator, bool gyro_use_32khz);

4
src/main/drivers/nvic.h
View file

@ -6,9 +6,9 @@
#define NVIC_PRIO_MAX NVIC_BUILD_PRIORITY(0, 1)
#define NVIC_PRIO_TIMER NVIC_BUILD_PRIORITY(1, 1)
#define NVIC_PRIO_BARO_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_SONAR_EXTI NVIC_BUILD_PRIORITY(2, 0) // maybe increate slightly
#define NVIC_PRIO_SONAR_EXTI NVIC_BUILD_PRIORITY(2, 0) // maybe increase slightly
#define NVIC_PRIO_TRANSPONDER_DMA NVIC_BUILD_PRIORITY(3, 0)
#define NVIC_PRIO_MPU_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_MPU_INT_EXTI NVIC_BUILD_PRIORITY(2, 0)
#define NVIC_PRIO_MAG_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_WS2811_DMA NVIC_BUILD_PRIORITY(1, 2) // TODO - is there some reason to use high priority? (or to use DMA IRQ at all?)
#define NVIC_PRIO_SERIALUART1_TXDMA NVIC_BUILD_PRIORITY(1, 1)

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

8
src/main/fc/config.c
View file

@ -1046,6 +1046,14 @@ void validateAndFixGyroConfig(void)
float samplingTime = 0.000125f;
if (gyroConfig()->gyro_use_32khz) {
#ifdef GYRO_SUPPORTS_32KHZ
samplingTime = 0.00003125;
#else
gyroConfig()->gyro_use_32khz = false;
#endif
}
if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
pidConfig()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
gyroConfig()->gyro_sync_denom = 1;

11
src/main/fc/fc_msp.c
View file

@ -1140,6 +1140,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU8(dst, motorConfig()->motorPwmProtocol);
sbufWriteU16(dst, motorConfig()->motorPwmRate);
sbufWriteU16(dst, (uint16_t)(motorConfig()->digitalIdleOffsetPercent * 100));
sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
//!!TODO gyro_isr_update to be added pending decision
//sbufWriteU8(dst, gyroConfig()->gyro_isr_update);
break;
case MSP_FILTER_CONFIG :
@ -1483,6 +1486,14 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (dataSize > 7) {
motorConfig()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f;
}
if (sbufBytesRemaining(src)) {
gyroConfig()->gyro_use_32khz = sbufReadU8(src);
}
//!!TODO gyro_isr_update to be added pending decision
/*if (sbufBytesRemaining(src)) {
gyroConfig()->gyro_isr_update = sbufReadU8(src);
}*/
validateAndFixGyroConfig();
break;
case MSP_SET_FILTER_CONFIG:

10
src/main/fc/serial_cli.c
View file

@ -600,7 +600,13 @@ const clivalue_t valueTable[] = {
{ "align_board_yaw", VAR_INT16 | MASTER_VALUE, &boardAlignment()->yawDegrees, .config.minmax = { -180, 360 } },
{ "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 } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyro_sync_denom, .config.minmax = { 1, 32 } },
#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 } },
#ifdef GYRO_SUPPORTS_32KHZ
{ "gyro_use_32khz", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_use_32khz, .config.lookup = { TABLE_OFF_ON } },
#endif
#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 } },
@ -3223,7 +3229,7 @@ static void cliTasks(char *cmdline)
int averageLoadSum = 0;
#ifndef CLI_MINIMAL_VERBOSITY
cliPrintf("Task list rate/hz max/us avg/us maxload avgload total/ms\r\n");
cliPrintf("Task list rate/hz max/us avg/us maxload avgload total/ms\r\n");
#endif
for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; taskId++) {
cfTaskInfo_t taskInfo;

17
src/main/scheduler/scheduler.c
View file

@ -185,6 +185,23 @@ uint32_t getTaskDeltaTime(cfTaskId_e taskId)
}
}
void schedulerResetTaskStatistics(cfTaskId_e taskId)
{
#ifdef SKIP_TASK_STATISTICS
UNUSED(taskId);
#else
if (taskId == TASK_SELF) {
currentTask->movingSumExecutionTime = 0;
currentTask->totalExecutionTime = 0;
currentTask->maxExecutionTime = 0;
} else if (taskId < TASK_COUNT) {
cfTasks[taskId].movingSumExecutionTime = 0;
cfTasks[taskId].totalExecutionTime = 0;
cfTasks[taskId].totalExecutionTime = 0;
}
#endif
}
void schedulerInit(void)
{
queueClear();

1
src/main/scheduler/scheduler.h
View file

@ -144,6 +144,7 @@ void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t *taskInfo);
void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros);
void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState);
uint32_t getTaskDeltaTime(cfTaskId_e taskId);
void schedulerResetTaskStatistics(cfTaskId_e taskId);
void schedulerInit(void);
void scheduler(void);

64
src/main/sensors/gyro.c
View file

@ -54,6 +54,8 @@
#include "io/beeper.h"
#include "io/statusindicator.h"
#include "scheduler/scheduler.h"
#include "sensors/sensors.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
@ -240,7 +242,8 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse)
if (!gyroDetect(&gyro.dev)) {
return false;
}
gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation
// Must set gyro sample rate before initialisation
gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom, gyroConfig->gyro_use_32khz);
gyro.dev.lpf = gyroConfig->gyro_lpf;
gyro.dev.init(&gyro.dev);
gyroInitFilters();
@ -358,26 +361,69 @@ static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
}
if (isOnFinalGyroCalibrationCycle()) {
schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
beeper(BEEPER_GYRO_CALIBRATED);
}
calibratingG--;
}
#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 +431,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);

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

@ -51,6 +51,8 @@ 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;
bool gyro_use_32khz;
uint16_t gyro_soft_notch_hz_1;
uint16_t gyro_soft_notch_cutoff_1;
uint16_t gyro_soft_notch_hz_2;