Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +03:00
Code Issues Wiki Activity

Change MPU/ICM gyro drivers to use 16G acceleration scale

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2018-05-12 22:12:41 +10:00
parent fd790f1235
commit d8288f6821
5 changed files with 10 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/drivers/accgyro/accgyro_mpu6000.c
View file

@ -107,7 +107,7 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
delayMicroseconds(15);
// Accel +/- 8 G Full Scale
busWrite(busDev, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
busWrite(busDev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delayMicroseconds(15);
busWrite(busDev, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR
@ -133,7 +133,7 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
static void mpu6000AccInit(accDev_t *acc)
{
acc->acc_1G = 512 * 8;
acc->acc_1G = 512 * 4;
}
bool mpu6000AccDetect(accDev_t *acc)

6
src/main/drivers/accgyro/accgyro_mpu6050.c
View file

@ -88,7 +88,7 @@ static void mpu6050AccAndGyroInit(gyroDev_t *gyro)
delayMicroseconds(15);
// Accel +/- 8 G Full Scale
busWrite(gyro->busDev, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
busWrite(gyro->busDev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delayMicroseconds(15);
busWrite(gyro->busDev, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
@ -109,10 +109,10 @@ static void mpu6050AccInit(accDev_t *acc)
{
mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
if (ctx->chipMagicNumber == 0x6850) {
acc->acc_1G = 512 * 8;
acc->acc_1G = 512 * 4;
}
else {
acc->acc_1G = 256 * 8;
acc->acc_1G = 256 * 4;
}
}

4
src/main/drivers/accgyro/accgyro_mpu6500.c
View file

@ -43,7 +43,7 @@
static void mpu6500AccInit(accDev_t *acc)
{
acc->acc_1G = 512 * 8;
acc->acc_1G = 512 * 4;
}
bool mpu6500AccDetect(accDev_t *acc)
@ -89,7 +89,7 @@ static void mpu6500AccAndGyroInit(gyroDev_t *gyro)
busWrite(dev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED);
delay(15);
busWrite(dev, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
busWrite(dev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
busWrite(dev, MPU_RA_CONFIG, config->gyroConfigValues[0]);

4
src/main/drivers/accgyro/accgyro_mpu9250.c
View file

@ -43,7 +43,7 @@
static void mpu9250AccInit(accDev_t *acc)
{
acc->acc_1G = 512 * 8;
acc->acc_1G = 512 * 4;
}
bool mpu9250AccDetect(accDev_t *acc)
@ -89,7 +89,7 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro)
busWrite(dev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED);
delay(15);
busWrite(dev, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
busWrite(dev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
busWrite(dev, MPU_RA_CONFIG, config->gyroConfigValues[0]);

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

@ -427,10 +427,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
case MSP_RAW_IMU:
{
// Hack scale due to choice of units for sensor data in multiwii
const uint8_t scale = (acc.dev.acc_1G > 1024) ? 8 : 1;
for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, (int16_t)lrintf(acc.accADCf[i] * acc.dev.acc_1G / scale));
sbufWriteU16(dst, (int16_t)lrintf(acc.accADCf[i] * 512));
}
for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, gyroRateDps(i));