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

Gyro Sync separated by target

Browse source
This commit is contained in:
borisbstyle 2015-09-18 13:50:30 +02:00
parent aadbc94c9a
commit 074e389789
9 changed files with 65 additions and 37 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -157,5 +157,9 @@ void checkMPU3050Interrupt(bool *gyroIsUpdated) {
i2cRead(MPU3050_ADDRESS, MPU3050_INT_STATUS, 1, &mpuIntStatus); i2cRead(MPU3050_ADDRESS, MPU3050_INT_STATUS, 1, &mpuIntStatus);
(mpuIntStatus) ? (*gyroIsUpdated= true) : (*gyroIsUpdated= false); if (mpuIntStatus) {
*gyroIsUpdated = true;
} else {
*gyroIsUpdated = false;
}
} }

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

@ -167,7 +167,7 @@ static void mpu6050GyroInit(void);
static bool mpu6050GyroRead(int16_t *gyroADC); static bool mpu6050GyroRead(int16_t *gyroADC);
static void checkMPU6050DataReady(bool *mpuDataReadyPtr); static void checkMPU6050DataReady(bool *mpuDataReadyPtr);
static bool mpuDataReady; static volatile bool mpuDataReady;
typedef enum { typedef enum {
MPU_6050_HALF_RESOLUTION, MPU_6050_HALF_RESOLUTION,
@ -425,9 +425,8 @@ static void mpu6050GyroInit(void)
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100); delay(100);
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //SMPLRT_DIV -- SMPLRT_DIV = 7 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDivider()); //SMPLRT_DIV -- SMPLRT_DIV = 7 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 = i2cWrite(MPU6050_ADDRESS, 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 = i2cWrite(MPU6050_ADDRESS, 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 = i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec

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

@ -42,7 +42,7 @@ enum lpf_e {
INV_FILTER_10HZ, INV_FILTER_10HZ,
INV_FILTER_5HZ, INV_FILTER_5HZ,
INV_FILTER_2100HZ_NOLPF, INV_FILTER_2100HZ_NOLPF,
NUM_FILTER INV_FILTER_COUNT
}; };
bool mpu6050AccDetect(const mpu6050Config_t *config,acc_t *acc); bool mpu6050AccDetect(const mpu6050Config_t *config,acc_t *acc);

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

@ -298,7 +298,7 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
// Determine the new sample divider // Determine the new sample divider
mpu6000WriteRegister(MPU6000_SMPLRT_DIV, gyroMPU6xxxGetDivider()); mpu6000WriteRegister(MPU6000_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops());
delayMicroseconds(1); delayMicroseconds(1);
// Accel and Gyro DLPF Setting // Accel and Gyro DLPF Setting
@ -356,7 +356,9 @@ void checkMPU6000Interrupt(bool *gyroIsUpdated) {
mpu6000ReadRegister(MPU6000_INT_STATUS, &mpuIntStatus, 1); mpu6000ReadRegister(MPU6000_INT_STATUS, &mpuIntStatus, 1);
delayMicroseconds(5); if (mpuIntStatus) {
*gyroIsUpdated = true;
(mpuIntStatus) ? (*gyroIsUpdated= true) : (*gyroIsUpdated= false); } else {
*gyroIsUpdated = false;
}
} }

12
src/main/drivers/accgyro_spi_mpu6500.c
View file

@ -231,7 +231,7 @@ static void mpu6500GyroInit(void)
mpu6500WriteRegister(MPU6500_RA_GYRO_CFG, INV_FSR_2000DPS << 3); mpu6500WriteRegister(MPU6500_RA_GYRO_CFG, INV_FSR_2000DPS << 3);
mpu6500WriteRegister(MPU6500_RA_ACCEL_CFG, INV_FSR_8G << 3); mpu6500WriteRegister(MPU6500_RA_ACCEL_CFG, INV_FSR_8G << 3);
mpu6500WriteRegister(MPU6500_RA_LPF, mpuLowPassFilter); mpu6500WriteRegister(MPU6500_RA_LPF, mpuLowPassFilter);
mpu6500WriteRegister(MPU6500_RA_RATE_DIV, 0); // 1kHz S/R mpu6500WriteRegister(MPU6500_RA_RATE_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider drop count
} }
static bool mpu6500GyroRead(int16_t *gyroADC) static bool mpu6500GyroRead(int16_t *gyroADC)
@ -250,9 +250,11 @@ static bool mpu6500GyroRead(int16_t *gyroADC)
void checkMPU6500Interrupt(bool *gyroIsUpdated) { void checkMPU6500Interrupt(bool *gyroIsUpdated) {
uint8_t mpuIntStatus; uint8_t mpuIntStatus;
mpu6500ReadRegister(MPU6500_INT_STATUS, &mpuIntStatus, 1); mpu6500ReadRegister(MPU6500_RA_INT_STATUS, &mpuIntStatus, 1);
delayMicroseconds(5); if (mpuIntStatus) {
*gyroIsUpdated = true;
(mpuIntStatus) ? (*gyroIsUpdated= true) : (*gyroIsUpdated= false); } else {
*gyroIsUpdated = false;
}
} }

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

@ -25,7 +25,7 @@
#define MPU6500_RA_ACCEL_CFG (0x1C) #define MPU6500_RA_ACCEL_CFG (0x1C)
#define MPU6500_RA_LPF (0x1A) #define MPU6500_RA_LPF (0x1A)
#define MPU6500_RA_RATE_DIV (0x19) #define MPU6500_RA_RATE_DIV (0x19)
#define MPU6500_INT_STATUS (0x38) #define MPU6500_RA_INT_STATUS (0x38)
#define MPU6500_WHO_AM_I_CONST (0x70) #define MPU6500_WHO_AM_I_CONST (0x70)

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

@ -29,7 +29,7 @@
extern gyro_t gyro; extern gyro_t gyro;
uint32_t targetLooptime; uint32_t targetLooptime;
static uint8_t mpuDivider; static uint8_t mpuDividerDrops;
bool getMpuDataStatus(gyro_t *gyro) bool getMpuDataStatus(gyro_t *gyro)
{ {
@ -48,52 +48,73 @@ void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf, uint8_t syncGyroToLoop
int minLooptime; int minLooptime;
if (syncGyroToLoop) { if (syncGyroToLoop) {
#ifdef STM32F303xC #if defined(SPRACINGF3) || defined(ALIENWIIF3) || defined(NAZE32PRO) || defined(STM32F3DISCOVERY) || defined(CHEBUZZF3) || defined(EUSTM32F103RC) || defined(PORT103R) || defined(MOTOLAB)
if (lpf == INV_FILTER_256HZ_NOLPF2) { if (lpf == INV_FILTER_256HZ_NOLPF2) {
gyroSamplePeriod = 125; gyroSamplePeriod = 125;
if(!sensors(SENSOR_ACC)) { if(!sensors(SENSOR_ACC)) {
minLooptime = 500; // Max refresh 2khz minLooptime = 500; // Max refresh 2khz
} } else {
else {
minLooptime = 625; // Max refresh 1,6khz minLooptime = 625; // Max refresh 1,6khz
} }
} } else {
else {
gyroSamplePeriod = 1000; gyroSamplePeriod = 1000;
minLooptime = 1000; // Full sampling minLooptime = 1000; // Full sampling
} }
#elif STM32F10X #elif defined(CC3D)
if (lpf == INV_FILTER_256HZ_NOLPF2) { if (lpf == INV_FILTER_256HZ_NOLPF2) {
gyroSamplePeriod = 125; gyroSamplePeriod = 125;
if(!sensors(SENSOR_ACC)) { if(!sensors(SENSOR_ACC)) {
minLooptime = 625; // Max refresh 1,33khz minLooptime = 890; // Max refresh 1,12khz
} else {
minLooptime = 1000; // Max refresh 1khz
} }
else { } else {
gyroSamplePeriod = 1000;
minLooptime = 1000; // Full sampling
}
#elif defined(COLIBRI_RACE)
if (lpf == INV_FILTER_256HZ_NOLPF2) {
gyroSamplePeriod = 125;
if(!sensors(SENSOR_ACC)) { // TODO - increase to 8khz when oneshot125 can be limited
minLooptime = 250; // Max refresh 4khz
} else {
minLooptime = 250; // Max refresh 4khz
}
} else {
gyroSamplePeriod = 1000;
minLooptime = 1000; // Full sampling
}
#else
if (lpf == INV_FILTER_256HZ_NOLPF2) {
gyroSamplePeriod = 125;
if(!sensors(SENSOR_ACC)) {
minLooptime = 625; // Max refresh 1,6khz
} else {
minLooptime = 1625; // Max refresh 615hz minLooptime = 1625; // Max refresh 615hz
} }
} } else {
else {
gyroSamplePeriod = 1000; gyroSamplePeriod = 1000;
if(!sensors(SENSOR_ACC)) { if(!sensors(SENSOR_ACC)) {
minLooptime = 1000; // Full sampling without ACC minLooptime = 1000; // Full sampling without ACC
} } else {
else {
minLooptime = 2000; minLooptime = 2000;
} }
} }
#endif #endif
looptime = constrain(looptime, minLooptime, 4000); looptime = constrain(looptime, minLooptime, 4000);
mpuDivider = (looptime + gyroSamplePeriod -1 ) / gyroSamplePeriod - 1; mpuDividerDrops = (looptime + gyroSamplePeriod -1 ) / gyroSamplePeriod - 1;
targetLooptime = (mpuDivider + 1) * gyroSamplePeriod; targetLooptime = (mpuDividerDrops + 1) * gyroSamplePeriod;
} } else {
else { mpuDividerDrops = 0;
mpuDivider = 0;
targetLooptime = looptime; targetLooptime = looptime;
} }
} }
uint8_t gyroMPU6xxxGetDivider(void) { uint8_t gyroMPU6xxxGetDividerDrops(void) {
return mpuDivider; return mpuDividerDrops;
} }

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

@ -8,5 +8,5 @@
extern uint32_t targetLooptime; extern uint32_t targetLooptime;
bool gyroSyncCheckUpdate(void); bool gyroSyncCheckUpdate(void);
uint8_t gyroMPU6xxxGetDivider(void); uint8_t gyroMPU6xxxGetDividerDrops(void);
void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf, uint8_t syncGyroToLoop); void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf, uint8_t syncGyroToLoop);

2
src/main/mw.c
View file

@ -746,7 +746,7 @@ bool runLoop(uint32_t loopTime) {
if (gyroSyncCheckUpdate() || (int32_t)(currentTime - (loopTime + GYRO_WATCHDOG_DELAY)) >= 0) { if (gyroSyncCheckUpdate() || (int32_t)(currentTime - (loopTime + GYRO_WATCHDOG_DELAY)) >= 0) {
loopTrigger = true; loopTrigger = true;
} }
} else if ((int32_t)(currentTime - loopTime) >= 0){ } else if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0){
loopTrigger = true; loopTrigger = true;
} }