Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 20:10:18 +03:00
Code Issues Wiki Activity

Add gyro rates and configurable FSR for ICM20649 (#4077)

Browse source
This commit is contained in:
brianlbalogh 2017-09-07 09:05:11 -04:00 • committed by Martin Budden
parent ba4587e949
commit b295a159b0
9 changed files with 67 additions and 21 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -42,8 +42,10 @@
typedef enum {
GYRO_RATE_1_kHz,
GYRO_RATE_1100_Hz,
GYRO_RATE_3200_Hz,
GYRO_RATE_8_kHz,
GYRO_RATE_9_kHz,
GYRO_RATE_32_kHz,
} gyroRateKHz_e;
@ -70,6 +72,7 @@ typedef struct gyroDev_s {
mpuDetectionResult_t mpuDetectionResult;
ioTag_t mpuIntExtiTag;
mpuConfiguration_t mpuConfiguration;
bool gyro_high_fsr;
} gyroDev_t;
typedef struct accDev_s {
@ -86,6 +89,7 @@ typedef struct accDev_s {
sensor_align_e accAlign;
mpuDetectionResult_t mpuDetectionResult;
mpuConfiguration_t mpuConfiguration;
bool acc_high_fsr;
} accDev_t;
static inline void accDevLock(accDev_t *acc)

13
src/main/drivers/accgyro/accgyro_spi_icm20649.c
View file

@ -35,9 +35,6 @@
#include "accgyro_mpu.h"
#include "accgyro_spi_icm20649.h"
static bool use4kDps = true; // TODO: make these configurable for testing
static bool use30g = true;
static void icm20649SpiInit(const busDevice_t *bus)
{
static bool hardwareInitialised = false;
@ -94,13 +91,13 @@ void icm20649AccInit(accDev_t *acc)
{
// 2,048 LSB/g 16g
// 1,024 LSB/g 30g
acc->acc_1G = use30g ? 1024 : 2048;
acc->acc_1G = acc->acc_high_fsr ? 1024 : 2048;
spiSetDivisor(acc->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD);
acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
delay(15);
const uint8_t acc_fsr = use30g ? ICM20649_FSR_30G : ICM20649_FSR_16G;
const uint8_t acc_fsr = acc->acc_high_fsr ? ICM20649_FSR_30G : ICM20649_FSR_16G;
acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1);
delay(15);
acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0
@ -134,8 +131,8 @@ void icm20649GyroInit(gyroDev_t *gyro)
delay(15);
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
delay(15);
const uint8_t gyro_fsr = use4kDps ? ICM20649_FSR_4000DPS : ICM20649_FSR_2000DPS;
uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_1_kHz ? 0 : 1; // deactivate GYRO_FCHOICE for sample rates over 1kHz (opposite of other invensense chips)
const uint8_t gyro_fsr = gyro->gyro_high_fsr ? ICM20649_FSR_4000DPS : ICM20649_FSR_2000DPS;
uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_1100_Hz ? 0 : 1; // deactivate GYRO_FCHOICE for sample rates over 1kHz (opposite of other invensense chips)
raGyroConfigData |= gyro_fsr << 1 | gyro->lpf << 3;
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData);
delay(15);
@ -164,7 +161,7 @@ bool icm20649SpiGyroDetect(gyroDev_t *gyro)
// 16.4 dps/lsb 2kDps
// 8.2 dps/lsb 4kDps
gyro->scale = 1.0f / (use4kDps ? 8.2f : 16.4f);
gyro->scale = 1.0f / (gyro->gyro_high_fsr ? 8.2f : 16.4f);
return true;
}

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

@ -36,17 +36,32 @@ uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenomin
gyro->gyroRateKHz = GYRO_RATE_32_kHz;
gyroSamplePeriod = 31.5f;
} else {
#ifdef USE_ACCGYRO_BMI160
switch (gyro->mpuDetectionResult.sensor) {
case BMI_160_SPI:
gyro->gyroRateKHz = GYRO_RATE_3200_Hz;
gyroSamplePeriod = 312.0f;
#else
break;
case ICM_20649_SPI:
gyro->gyroRateKHz = GYRO_RATE_9_kHz;
gyroSamplePeriod = 1000000.0f / 9000.0f;
break;
default:
gyro->gyroRateKHz = GYRO_RATE_8_kHz;
gyroSamplePeriod = 125.0f;
#endif
break;
}
}
} else {
switch (gyro->mpuDetectionResult.sensor) {
case ICM_20649_SPI:
gyro->gyroRateKHz = GYRO_RATE_1100_Hz;
gyroSamplePeriod = 1000000.0f / 1100.0f;
break;
default:
gyro->gyroRateKHz = GYRO_RATE_1_kHz;
gyroSamplePeriod = 1000.0f;
break;
}
gyroSyncDenominator = 1; // Always full Sampling 1khz
}

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

@ -507,13 +507,31 @@ void validateAndFixGyroConfig(void)
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
}
float samplingTime = 0.000125f;
float samplingTime;
switch (gyroMpuDetectionResult()->sensor) {
case ICM_20649_SPI:
samplingTime = 1.0f / 9000.0f;
break;
case BMI_160_SPI:
samplingTime = 0.0003125f;
break;
default:
samplingTime = 0.000125f;
break;
}
if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
gyroConfigMutable()->gyro_sync_denom = 1;
gyroConfigMutable()->gyro_use_32khz = false;
switch (gyroMpuDetectionResult()->sensor) {
case ICM_20649_SPI:
samplingTime = 1.0f / 1100.0f;
break;
default:
samplingTime = 0.001f;
break;
}
}
if (gyroConfig()->gyro_use_32khz) {

6
src/main/fc/settings.c
View file

@ -301,6 +301,9 @@ const clivalue_t valueTable[] = {
// PG_GYRO_CONFIG
{ "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_align) },
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_LPF }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lpf) },
#if defined(USE_GYRO_SPI_ICM20649)
{ "gyro_high_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_high_fsr) },
#endif
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 32 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_sync_denom) },
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf_type) },
{ "gyro_lowpass_hz", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf_hz) },
@ -321,6 +324,9 @@ const clivalue_t valueTable[] = {
// PG_ACCELEROMETER_CONFIG
{ "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) },
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ACC_HARDWARE }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_hardware) },
#if defined(USE_GYRO_SPI_ICM20649)
{ "acc_high_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_high_fsr) },
#endif
{ "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 400 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_lpf_hz) },
{ "acc_trim_pitch", VAR_INT16 | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.pitch) },
{ "acc_trim_roll", VAR_INT16 | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.roll) },

4
src/main/sensors/acceleration.c
View file

@ -113,7 +113,8 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
RESET_CONFIG_2(accelerometerConfig_t, instance,
.acc_lpf_hz = 10,
.acc_align = ALIGN_DEFAULT,
.acc_hardware = ACC_DEFAULT
.acc_hardware = ACC_DEFAULT,
.acc_high_fsr = false,
);
resetRollAndPitchTrims(&instance->accelerometerTrims);
resetFlightDynamicsTrims(&instance->accZero);
@ -317,6 +318,7 @@ bool accInit(uint32_t gyroSamplingInverval)
acc.dev.bus = *gyroSensorBus();
acc.dev.mpuConfiguration = *gyroMpuConfiguration();
acc.dev.mpuDetectionResult = *gyroMpuDetectionResult();
acc.dev.acc_high_fsr = accelerometerConfig()->acc_high_fsr;
if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) {
return false;
}

1
src/main/sensors/acceleration.h
View file

@ -66,6 +66,7 @@ typedef struct accelerometerConfig_s {
uint16_t acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
sensor_align_e acc_align; // acc alignment
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
bool acc_high_fsr;
flightDynamicsTrims_t accZero;
rollAndPitchTrims_t accelerometerTrims;
} accelerometerConfig_t;

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

@ -127,6 +127,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lpf = GYRO_LPF_256HZ,
.gyro_soft_lpf_type = FILTER_PT1,
.gyro_soft_lpf_hz = 90,
.gyro_high_fsr = false,
.gyro_use_32khz = false,
.gyro_to_use = 0,
.gyro_soft_notch_hz_1 = 400,
@ -335,6 +336,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor)
mpuDetect(&gyroSensor->gyroDev);
mpuResetFn = gyroSensor->gyroDev.mpuConfiguration.resetFn; // must be set after mpuDetect
#endif
gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;
const gyroSensor_e gyroHardware = gyroDetect(&gyroSensor->gyroDev);
if (gyroHardware == GYRO_NONE) {

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

@ -55,6 +55,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_high_fsr;
bool gyro_use_32khz;
uint8_t gyro_to_use;
uint16_t gyro_soft_notch_hz_1;