diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index 0c1eae8bcd..8c60a64171 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -127,8 +127,9 @@ mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro) static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) { #ifdef USE_GYRO_SPI_MPU6500 - if (mpu6500SpiDetect()) { - gyro->mpuDetectionResult.sensor = MPU_65xx_SPI; + uint8_t mpu6500Sensor = mpu6500SpiDetect(); + if (mpu6500Sensor != MPU_NONE) { + gyro->mpuDetectionResult.sensor = mpu6500Sensor; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.read = mpu6500ReadRegister; gyro->mpuConfiguration.write = mpu6500WriteRegister; diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 1a52f423d3..bf15fcd4b2 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -168,7 +168,9 @@ typedef enum { MPU_65xx_I2C, MPU_65xx_SPI, MPU_9250_SPI, - ICM_20689_SPI + ICM_20689_SPI, + ICM_20608_SPI, + ICM_20602_SPI } detectedMPUSensor_e; typedef enum { diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index fa8d97d9b8..f76733d4dc 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -76,7 +76,8 @@ static void mpu6500SpiInit(void) hardwareInitialised = true; } -bool mpu6500SpiDetect(void) +static uint8_t mpuDetected = MPU_NONE; +uint8_t mpu6500SpiDetect(void) { uint8_t tmp; @@ -84,14 +85,23 @@ bool mpu6500SpiDetect(void) mpu6500ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp); - if (tmp == MPU6500_WHO_AM_I_CONST || - tmp == MPU9250_WHO_AM_I_CONST || - tmp == ICM20608G_WHO_AM_I_CONST || - tmp == ICM20602_WHO_AM_I_CONST) { - return true; + switch (tmp) { + case MPU6500_WHO_AM_I_CONST: + mpuDetected = MPU_65xx_SPI; + break; + case MPU9250_WHO_AM_I_CONST: + mpuDetected = MPU_9250_SPI; + break; + case ICM20608G_WHO_AM_I_CONST: + mpuDetected = ICM_20608_SPI; + break; + case ICM20602_WHO_AM_I_CONST: + mpuDetected = ICM_20602_SPI; + break; + default: + mpuDetected = MPU_NONE; } - - return false; + return mpuDetected; } void mpu6500SpiAccInit(accDev_t *acc) @@ -116,7 +126,7 @@ void mpu6500SpiGyroInit(gyroDev_t *gyro) bool mpu6500SpiAccDetect(accDev_t *acc) { - if (acc->mpuDetectionResult.sensor != MPU_65xx_SPI) { + if (acc->mpuDetectionResult.sensor != mpuDetected || !mpuDetected) { return false; } @@ -128,10 +138,10 @@ bool mpu6500SpiAccDetect(accDev_t *acc) bool mpu6500SpiGyroDetect(gyroDev_t *gyro) { - if (gyro->mpuDetectionResult.sensor != MPU_65xx_SPI) { + if (gyro->mpuDetectionResult.sensor != mpuDetected || !mpuDetected) { return false; } - + gyro->init = mpu6500SpiGyroInit; gyro->read = mpuGyroRead; gyro->intStatus = mpuCheckDataReady; diff --git a/src/main/drivers/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro_spi_mpu6500.h index b5c2758fc9..25335743f7 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.h +++ b/src/main/drivers/accgyro_spi_mpu6500.h @@ -17,7 +17,7 @@ #pragma once -bool mpu6500SpiDetect(void); +uint8_t mpu6500SpiDetect(void); void mpu6500SpiAccInit(accDev_t *acc); void mpu6500SpiGyroInit(gyroDev_t *gyro); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 32f9a05d58..82dd2d74fd 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -253,9 +253,9 @@ static const char * const sensorTypeNames[] = { #define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) -static const char * const sensorHardwareNames[4][12] = { - { "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "ICM20689", "FAKE", NULL }, - { "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "ICM20689", "FAKE", NULL }, +static const char * const sensorHardwareNames[4][15] = { + { "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "ICM20689", "ICM20608G", "ICM20602", "FAKE", NULL }, + { "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "ICM20689", "MPU9250", "ICM20608G", "ICM20602", "FAKE", NULL }, { "", "None", "BMP085", "MS5611", "BMP280", NULL }, { "", "None", "HMC5883", "AK8975", "AK8963", NULL } }; @@ -3678,7 +3678,7 @@ static void cliVersion(char *cmdline) { UNUSED(cmdline); - cliPrintf("# %s/%s %s %s / %s (%s)\r\n", + cliPrintf("# %s / %s %s %s / %s (%s)\r\n", FC_FIRMWARE_NAME, targetName, FC_VERSION_STRING, diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 14a474f2f3..325de5988d 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -169,6 +169,9 @@ retry: #endif ; // fallthrough case ACC_MPU6500: + case ACC_ICM20608G: + case ACC_ICM20602: + case ACC_MPU9250: #if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500) #ifdef USE_ACC_SPI_MPU6500 if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) @@ -179,7 +182,19 @@ retry: #ifdef ACC_MPU6500_ALIGN dev->accAlign = ACC_MPU6500_ALIGN; #endif - accHardware = ACC_MPU6500; + switch(dev->mpuDetectionResult.sensor) { + case MPU_9250_SPI: + accHardware = ACC_MPU9250; + break; + case ICM_20608_SPI: + accHardware = ACC_ICM20608G; + break; + case ICM_20602_SPI: + accHardware = ACC_ICM20602; + break; + default: + accHardware = ACC_MPU6500; + } break; } #endif diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 0cc1020ae9..b91db0ee16 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -22,17 +22,20 @@ // Type of accelerometer used/detected typedef enum { - ACC_DEFAULT = 0, - ACC_NONE = 1, - ACC_ADXL345 = 2, - ACC_MPU6050 = 3, - ACC_MMA8452 = 4, - ACC_BMA280 = 5, - ACC_LSM303DLHC = 6, - ACC_MPU6000 = 7, - ACC_MPU6500 = 8, - ACC_ICM20689 = 9, - ACC_FAKE = 10 + ACC_DEFAULT, + ACC_NONE, + ACC_ADXL345, + ACC_MPU6050, + ACC_MMA8452, + ACC_BMA280, + ACC_LSM303DLHC, + ACC_MPU6000, + ACC_MPU6500, + ACC_ICM20689, + ACC_MPU9250, + ACC_ICM20608G, + ACC_ICM20602, + ACC_FAKE } accelerationSensor_e; typedef struct acc_s { diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 811e3a9ddd..8400c0f28e 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -97,9 +97,8 @@ static bool gyroDetect(gyroDev_t *dev) switch(gyroHardware) { case GYRO_DEFAULT: - ; // fallthrough - case GYRO_MPU6050: #ifdef USE_GYRO_MPU6050 + case GYRO_MPU6050: if (mpu6050GyroDetect(dev)) { gyroHardware = GYRO_MPU6050; #ifdef GYRO_MPU6050_ALIGN @@ -108,9 +107,9 @@ static bool gyroDetect(gyroDev_t *dev) break; } #endif - ; // fallthrough - case GYRO_L3G4200D: + #ifdef USE_GYRO_L3G4200D + case GYRO_L3G4200D: if (l3g4200dDetect(dev)) { gyroHardware = GYRO_L3G4200D; #ifdef GYRO_L3G4200D_ALIGN @@ -119,10 +118,9 @@ static bool gyroDetect(gyroDev_t *dev) break; } #endif - ; // fallthrough - case GYRO_MPU3050: #ifdef USE_GYRO_MPU3050 + case GYRO_MPU3050: if (mpu3050Detect(dev)) { gyroHardware = GYRO_MPU3050; #ifdef GYRO_MPU3050_ALIGN @@ -131,10 +129,9 @@ static bool gyroDetect(gyroDev_t *dev) break; } #endif - ; // fallthrough - case GYRO_L3GD20: #ifdef USE_GYRO_L3GD20 + case GYRO_L3GD20: if (l3gd20Detect(dev)) { gyroHardware = GYRO_L3GD20; #ifdef GYRO_L3GD20_ALIGN @@ -143,10 +140,9 @@ static bool gyroDetect(gyroDev_t *dev) break; } #endif - ; // fallthrough - case GYRO_MPU6000: #ifdef USE_GYRO_SPI_MPU6000 + case GYRO_MPU6000: if (mpu6000SpiGyroDetect(dev)) { gyroHardware = GYRO_MPU6000; #ifdef GYRO_MPU6000_ALIGN @@ -155,64 +151,69 @@ static bool gyroDetect(gyroDev_t *dev) break; } #endif - ; // fallthrough - case GYRO_MPU6500: #if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) + case GYRO_MPU6500: + case GYRO_ICM20608G: + case GYRO_ICM20602: #ifdef USE_GYRO_SPI_MPU6500 - if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) + if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) { #else - if (mpu6500GyroDetect(dev)) + if (mpu6500GyroDetect(dev)) { #endif - { - gyroHardware = GYRO_MPU6500; + switch(dev->mpuDetectionResult.sensor) { + case MPU_9250_SPI: + gyroHardware = GYRO_MPU9250; + break; + case ICM_20608_SPI: + gyroHardware = GYRO_ICM20608G; + break; + case ICM_20602_SPI: + gyroHardware = GYRO_ICM20602; + break; + default: + gyroHardware = GYRO_MPU6500; + } #ifdef GYRO_MPU6500_ALIGN dev->gyroAlign = GYRO_MPU6500_ALIGN; #endif - break; } #endif - ; // fallthrough -case GYRO_MPU9250: #ifdef USE_GYRO_SPI_MPU9250 + case GYRO_MPU9250: - if (mpu9250SpiGyroDetect(dev)) - { - gyroHardware = GYRO_MPU9250; + if (mpu9250SpiGyroDetect(dev)) + { + gyroHardware = GYRO_MPU9250; #ifdef GYRO_MPU9250_ALIGN - dev->gyroAlign = GYRO_MPU9250_ALIGN; + dev->gyroAlign = GYRO_MPU9250_ALIGN; #endif - break; } #endif - ; // fallthrough - case GYRO_ICM20689: #ifdef USE_GYRO_SPI_ICM20689 + case GYRO_ICM20689: if (icm20689SpiGyroDetect(dev)) { gyroHardware = GYRO_ICM20689; #ifdef GYRO_ICM20689_ALIGN dev->gyroAlign = GYRO_ICM20689_ALIGN; #endif - break; } #endif - ; // fallthrough - case GYRO_FAKE: #ifdef USE_FAKE_GYRO + case GYRO_FAKE: if (fakeGyroDetect(dev)) { gyroHardware = GYRO_FAKE; break; } #endif - ; // fallthrough - case GYRO_NONE: + default: gyroHardware = GYRO_NONE; } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 1cd410f147..9244fcc638 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -31,6 +31,8 @@ typedef enum { GYRO_MPU6500, GYRO_MPU9250, GYRO_ICM20689, + GYRO_ICM20608G, + GYRO_ICM20602, GYRO_FAKE } gyroSensor_e;