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

Added support for 32kHz gyro updates

This commit is contained in:
Martin Budden 2017-01-05 18:15:37 +00:00
parent a1c14320bf
commit 95111483d8
14 changed files with 83 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
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,6 +39,12 @@
#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
@ -44,7 +54,9 @@ typedef struct gyroDev_s {
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;

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

22
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,35 @@ 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)
{
#ifndef GYRO_SUPPORTS_32KHZ
if (gyro_use_32khz) {
gyro_use_32khz = false;
}
#endif
int gyroSamplePeriod;
if (lpf == GYRO_LPF_256HZ || lpf == GYRO_LPF_NONE) {
gyro->gyroRateKHz = GYRO_RATE_8_kHz;
gyroSamplePeriod = 125;
} else {
gyro->gyroRateKHz = GYRO_RATE_1_kHz;
gyroSamplePeriod = 1000;
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;
uint32_t targetLooptime = gyroSyncDenominator * gyroSamplePeriod;
if (gyro_use_32khz) {
gyro->gyroRateKHz = GYRO_RATE_32_kHz;
targetLooptime /= 4;
}
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)

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

@ -1043,6 +1043,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));
//!!TODO gyro_isr_update and gyro_use_32khz to be added once we decide to add them to configurator
//sbufWriteU8(dst, gyroConfig()->gyro_isr_update);
//sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
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;
}
//!!TODO gyro_isr_update and gyro_use_32khz to be added once we decide to add them to configurator
/*if (sbufBytesRemaining(src)) {
gyroConfig()->gyro_isr_update = sbufReadU8(src);
}
if (sbufBytesRemaining(src)) {
gyroConfig()->gyro_use_32khz = sbufReadU8(src);
}*/
validateAndFixGyroConfig();
break;
case MSP_SET_FILTER_CONFIG:

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

@ -600,9 +600,12 @@ 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 } },

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

@ -240,7 +240,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();

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

@ -52,6 +52,7 @@ typedef struct gyroConfig_s {
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;