Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +03:00
Code Issues Wiki Activity

8khz Sampling option / preparation

Browse source
This commit is contained in:
borisbstyle 2015-11-24 16:06:59 +01:00
parent e3b77b2ee5
commit b39beceb62
17 changed files with 39 additions and 75 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/config/config.c
View file

@ -400,6 +400,7 @@ static void resetConf(void)
masterConfig.current_profile_index = 0; // default profile
masterConfig.dcm_kp = 2500; // 1.0 * 10000
masterConfig.dcm_ki = 0; // 0.003 * 10000
masterConfig.gyro_lpf = 1; // 1KHZ or 8KHZ
resetAccelerometerTrims(&masterConfig.accZero);

2
src/main/config/config_master.h
View file

@ -47,7 +47,7 @@ typedef struct master_t {
int8_t yaw_control_direction; // change control direction of yaw (inverted, normal)
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
uint16_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_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)

24
src/main/drivers/accgyro_mpu.c
View file

@ -291,30 +291,6 @@ void mpuIntExtiInit(void)
mpuExtiInitDone = true;
}
uint8_t determineMPULPF(uint16_t lpf)
{
uint8_t mpuLowPassFilter;
if (lpf == 256)
mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
else if (lpf >= 188)
mpuLowPassFilter = INV_FILTER_188HZ;
else if (lpf >= 98)
mpuLowPassFilter = INV_FILTER_98HZ;
else if (lpf >= 42)
mpuLowPassFilter = INV_FILTER_42HZ;
else if (lpf >= 20)
mpuLowPassFilter = INV_FILTER_20HZ;
else if (lpf >= 10)
mpuLowPassFilter = INV_FILTER_10HZ;
else if (lpf > 0)
mpuLowPassFilter = INV_FILTER_5HZ;
else
mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
return mpuLowPassFilter;
}
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data)
{
bool ack = i2cRead(MPU_ADDRESS, reg, length, data);

12
src/main/drivers/accgyro_mpu.h
View file

@ -126,17 +126,6 @@ typedef struct mpuConfiguration_s {
extern mpuConfiguration_t mpuConfiguration;
enum lpf_e {
INV_FILTER_256HZ_NOLPF2 = 0,
INV_FILTER_188HZ,
INV_FILTER_98HZ,
INV_FILTER_42HZ,
INV_FILTER_20HZ,
INV_FILTER_10HZ,
INV_FILTER_5HZ,
INV_FILTER_2100HZ_NOLPF,
NUM_FILTER
};
enum gyro_fsr_e {
INV_FSR_250DPS = 0,
INV_FSR_500DPS,
@ -178,7 +167,6 @@ typedef struct mpuDetectionResult_s {
extern mpuDetectionResult_t mpuDetectionResult;
uint8_t determineMPULPF(uint16_t lpf);
void configureMPUDataReadyInterruptHandling(void);
void mpuIntExtiInit(void);
bool mpuAccRead(int16_t *accData);

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

@ -47,7 +47,7 @@
#define MPU3050_USER_RESET 0x01
#define MPU3050_CLK_SEL_PLL_GX 0x01
static void mpu3050Init(uint16_t lpf);
static void mpu3050Init(uint8_t lpf);
static bool mpu3050ReadTemp(int16_t *tempData);
bool mpu3050Detect(gyro_t *gyro)
@ -66,19 +66,17 @@ bool mpu3050Detect(gyro_t *gyro)
return true;
}
static void mpu3050Init(uint16_t lpf)
static void mpu3050Init(uint8_t lpf)
{
bool ack;
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
ack = mpuConfiguration.write(MPU3050_SMPLRT_DIV, 0);
if (!ack)
failureMode(FAILURE_ACC_INIT);
mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter);
mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | lpf);
mpuConfiguration.write(MPU3050_INT_CFG, 0);
mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET);
mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);

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

@ -52,7 +52,7 @@ extern uint8_t mpuLowPassFilter;
#define MPU6050_SMPLRT_DIV 0 // 8000Hz
static void mpu6050AccInit(void);
static void mpu6050GyroInit(uint16_t lpf);
static void mpu6050GyroInit(uint8_t lpf);
bool mpu6050AccDetect(acc_t *acc)
{
@ -96,20 +96,18 @@ static void mpu6050AccInit(void)
}
}
static void mpu6050GyroInit(uint16_t lpf)
static void mpu6050GyroInit(uint8_t lpf)
{
bool ack;
mpuIntExtiInit();
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100);
ack = 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)
ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //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
ack = mpuConfiguration.write(MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
ack = mpuConfiguration.write(MPU_RA_CONFIG, lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
ack = mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
// ACC Init stuff.

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

@ -71,7 +71,7 @@ void mpu6500AccInit(void)
acc_1G = 512 * 8;
}
void mpu6500GyroInit(uint16_t lpf)
void mpu6500GyroInit(uint8_t lpf)
{
mpuIntExtiInit();
@ -90,8 +90,6 @@ void mpu6500GyroInit(uint16_t lpf)
mpuIntExtiInit();
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100);
mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07);
@ -101,7 +99,7 @@ void mpu6500GyroInit(uint16_t lpf)
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
mpuConfiguration.write(MPU_RA_CONFIG, mpuLowPassFilter);
mpuConfiguration.write(MPU_RA_CONFIG, lpf);
mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
// Data ready interrupt configuration

2
src/main/drivers/accgyro_mpu6500.h
View file

@ -25,4 +25,4 @@ bool mpu6500AccDetect(acc_t *acc);
bool mpu6500GyroDetect(gyro_t *gyro);
void mpu6500AccInit(void);
void mpu6500GyroInit(uint16_t lpf);
void mpu6500GyroInit(uint8_t lpf);

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

@ -120,12 +120,10 @@ bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
return true;
}
void mpu6000SpiGyroInit(uint16_t lpf)
void mpu6000SpiGyroInit(uint8_t lpf)
{
mpuIntExtiInit();
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
mpu6000AccAndGyroInit();
mpuIntExtiInit();
@ -135,7 +133,7 @@ void mpu6000SpiGyroInit(uint16_t lpf)
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
// Accel and Gyro DLPF Setting
mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter);
mpu6000WriteRegister(MPU6000_CONFIG, lpf);
delayMicroseconds(1);
int16_t data[3];

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

@ -41,15 +41,19 @@ bool gyroSyncCheckUpdate(void) {
return getMpuDataStatus(&gyro);
}
void gyroUpdateSampleRate(void) {
int gyroSamplePeriod;
int minLooptime;
void gyroUpdateSampleRate(uint8_t lpf) {
int gyroSamplePeriod, gyroSyncDenominator;
gyroSamplePeriod = 1000; // gyro sampling rate 1khz
minLooptime = 1000; // Full 1khz sampling
if (!lpf) {
gyroSamplePeriod = 125;
gyroSyncDenominator = 8; // Sample every 8th gyro measurement
} else {
gyroSamplePeriod = 1000;
gyroSyncDenominator = 1; // Full Sampling
}
// calculate gyro divider and targetLooptime (expected cycleTime)
mpuDividerDrops = (minLooptime + gyroSamplePeriod -1 ) / gyroSamplePeriod - 1;
mpuDividerDrops = gyroSyncDenominator - 1;
targetLooptime = (mpuDividerDrops + 1) * gyroSamplePeriod;
}

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

@ -9,4 +9,4 @@ extern uint32_t targetLooptime;
bool gyroSyncCheckUpdate(void);
uint8_t gyroMPU6xxxGetDividerDrops(void);
void gyroUpdateSampleRate(void);
void gyroUpdateSampleRate(uint8_t lpf);

2
src/main/drivers/sensor.h
View file

@ -19,5 +19,5 @@
typedef void (*sensorInitFuncPtr)(void); // sensor init prototype
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef void (*sensorGyroInitFuncPtr)(uint16_t lpf); // gyro sensor init prototype
typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf); // gyro sensor init prototype
typedef void (*sensorInterruptFuncPtr)(bool *data); // sensor Interrupt Data Ready

13
src/main/io/serial_cli.c
View file

@ -351,7 +351,12 @@ static const char * const lookupTableSerialRX[] = {
};
static const char * const lookupTableGyroFilter[] = {
"OFF", "LOW", "MEDIUM", "HIGH"
"LOW", "MEDIUM", "HIGH"
};
static const char * const lookupTableGyroSampling[] = {
"8KHZ",
"1KHZ"
};
@ -373,6 +378,7 @@ typedef enum {
TABLE_PID_CONTROLLER,
TABLE_SERIAL_RX,
TABLE_GYRO_FILTER,
TABLE_GYRO_SAMPLING,
} lookupTableIndex_e;
static const lookupTableEntry_t lookupTables[] = {
@ -387,7 +393,8 @@ static const lookupTableEntry_t lookupTables[] = {
{ lookupTableGimbalMode, sizeof(lookupTableGimbalMode) / sizeof(char *) },
{ lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) },
{ lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) },
{ lookupTableGyroFilter, sizeof(lookupTableGyroFilter) / sizeof(char *) }
{ lookupTableGyroFilter, sizeof(lookupTableGyroFilter) / sizeof(char *) },
{ lookupTableGyroSampling, sizeof(lookupTableGyroSampling) / sizeof(char *) }
};
#define VALUE_TYPE_OFFSET 0
@ -527,7 +534,7 @@ const clivalue_t valueTable[] = {
{ "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } },
{ "gyro_lpf", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_lpf, .config.minmax = { 0, 256 } },
{ "gyro_sampling", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_SAMPLING } },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } },
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } },

2
src/main/main.c
View file

@ -378,7 +378,7 @@ void init(void)
}
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) {
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination, masterConfig.gyro_lpf)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}

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

@ -29,8 +29,6 @@ typedef enum {
GYRO_FAKE
} gyroSensor_e;
#define GYRO_LPF 188
extern gyro_t gyro;
extern sensor_align_e gyroAlign;

6
src/main/sensors/initialisation.c
View file

@ -644,12 +644,10 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
}
}
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig)
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf)
{
int16_t deg, min;
uint16_t gyroLpf = GYRO_LPF;
memset(&acc, 0, sizeof(acc));
memset(&gyro, 0, sizeof(gyro));
@ -672,7 +670,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a
if (sensors(SENSOR_ACC))
acc.init();
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyroUpdateSampleRate(); // Set gyro refresh rate before initialisation
gyroUpdateSampleRate(gyroLpf); // Set gyro refresh rate before initialisation
gyro.init(gyroLpf);
detectMag(magHardwareToUse);

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

@ -17,4 +17,4 @@
#pragma once
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig);
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf);