From 1de72b11ce770f4437cee789cf0ac1b08f88d09a Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 19 Feb 2015 21:04:06 +0000 Subject: [PATCH] Update the cli `status` command to show all detected sensors (Except on CJMCU). Further cleanup of sensor initialisation. --- src/main/io/serial_cli.c | 52 ++++--- src/main/sensors/acceleration.c | 1 - src/main/sensors/acceleration.h | 1 - src/main/sensors/barometer.c | 1 - src/main/sensors/barometer.h | 2 - src/main/sensors/compass.c | 1 - src/main/sensors/compass.h | 7 +- src/main/sensors/gyro.h | 12 ++ src/main/sensors/initialisation.c | 217 ++++++++++++++++++------------ src/main/sensors/sensors.h | 10 ++ 10 files changed, 190 insertions(+), 114 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 276c9310a..7098e0610 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -146,14 +146,23 @@ static const char * const featureNames[] = { "BLACKBOX", NULL }; +#ifndef CJMCU // sync this with sensors_e -static const char * const sensorNames[] = { +static const char * const sensorTypeNames[] = { "GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL }; -static const char * const accNames[] = { - "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", "None", NULL +// FIXME the next time the EEPROM is bumped change the order of acc and gyro names so that "None" is second. + +#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) + +static const char * const sensorHardwareNames[4][11] = { + { "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE", NULL }, + { "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", "None", NULL }, + { "", "None", "BMP085", "MS5611", NULL }, + { "", "None", "HMC5883", "AK8975", NULL } }; +#endif typedef struct { const char *name; @@ -1357,27 +1366,38 @@ static void cliGet(char *cmdline) static void cliStatus(char *cmdline) { - uint8_t i; - uint32_t mask; - UNUSED(cmdline); printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery)\r\n", millis() / 1000, vbat, batteryCellCount); - mask = sensorsMask(); - printf("CPU %dMHz, detected sensors: ", (SystemCoreClock / 1000000)); + + printf("CPU Clock=%dMHz", (SystemCoreClock / 1000000)); + +#ifndef CJMCU + uint8_t i; + uint32_t mask; + uint32_t detectedSensorsMask = sensorsMask(); + for (i = 0; ; i++) { - if (sensorNames[i] == NULL) + + if (sensorTypeNames[i] == NULL) break; - if (mask & (1 << i)) - printf("%s ", sensorNames[i]); - } - if (sensors(SENSOR_ACC)) { - printf("ACCHW: %s", accNames[accHardware]); - if (acc.revisionCode) - printf(".%c", acc.revisionCode); + + mask = (1 << i); + if ((detectedSensorsMask & mask) && (mask & SENSOR_NAMES_MASK)) { + const char *sensorHardware; + uint8_t sensorHardwareIndex = detectedSensors[i]; + sensorHardware = sensorHardwareNames[i][sensorHardwareIndex]; + + printf(", %s=%s", sensorTypeNames[i], sensorHardware); + + if (mask == SENSOR_ACC && acc.revisionCode) { + printf(".%c", acc.revisionCode); + } + } } +#endif cliPrint("\r\n"); #ifdef USE_I2C diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index b43711f0b..c265b7595 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -37,7 +37,6 @@ int16_t accADC[XYZ_AXIS_COUNT]; acc_t acc; // acc access functions -accelerationSensor_e accHardware = ACC_DEFAULT; // which accel chip is used/detected sensor_align_e accAlign = 0; uint16_t acc_1G = 256; // this is the 1G measured acceleration. diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 709f4415a..5b925b01d 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -31,7 +31,6 @@ typedef enum { ACC_NONE = 9 } accelerationSensor_e; -extern uint8_t accHardware; extern sensor_align_e accAlign; extern acc_t acc; extern uint16_t acc_1G; diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 719c83eed..f019dd628 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -41,7 +41,6 @@ static int32_t baroGroundPressure = 0; static uint32_t baroPressureSum = 0; barometerConfig_t *barometerConfig; -baroSensor_e baroHardware; void useBarometerConfig(barometerConfig_t *barometerConfigToUse) { diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index fed38d0aa..685f6b843 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -24,8 +24,6 @@ typedef enum { BARO_MS5611 = 3 } baroSensor_e; -extern baroSensor_e baroHardware; - #define BARO_SAMPLE_COUNT_MAX 48 typedef struct barometerConfig_s { diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 7c5f11f6a..31d7688c6 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -40,7 +40,6 @@ #endif mag_t mag; // mag access functions -magSensor_e magHardware = MAG_DEFAULT; extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead. diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 2effbabc9..502c18728 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -20,9 +20,9 @@ // Type of accelerometer used/detected typedef enum { MAG_DEFAULT = 0, - MAG_HMC5883 = 1, - MAG_AK8975 = 2, - MAG_NONE = 3 + MAG_NONE = 1, + MAG_HMC5883 = 2, + MAG_AK8975 = 3, } magSensor_e; #ifdef MAG @@ -32,6 +32,5 @@ void updateCompass(flightDynamicsTrims_t *magZero); extern int16_t magADC[XYZ_AXIS_COUNT]; -extern uint8_t magHardware; extern sensor_align_e magAlign; extern mag_t mag; diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index caa34281d..f09e2af5e 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -17,6 +17,18 @@ #pragma once +typedef enum { + GYRO_NONE = 0, + GYRO_DEFAULT, + GYRO_MPU6050, + GYRO_L3G4200D, + GYRO_MPU3050, + GYRO_L3GD20, + GYRO_SPI_MPU6000, + GYRO_SPI_MPU6500, + GYRO_FAKE +} gyroSensor_e; + extern gyro_t gyro; extern sensor_align_e gyroAlign; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 883c253f1..cd90c28f3 100644 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -71,6 +71,8 @@ extern gyro_t gyro; extern baro_t baro; extern acc_t acc; +uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE }; + const mpu6050Config_t *selectMPU6050Config(void) { #ifdef NAZE @@ -141,82 +143,121 @@ bool fakeAccDetect(acc_t *acc) bool detectGyro(uint16_t gyroLpf) { + gyroSensor_e gyroHardware = GYRO_DEFAULT; + gyroAlign = ALIGN_DEFAULT; + switch(gyroHardware) { + case GYRO_DEFAULT: + ; // fallthrough + case GYRO_MPU6050: #ifdef USE_GYRO_MPU6050 - if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) { + if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) { #ifdef GYRO_MPU6050_ALIGN - gyroAlign = GYRO_MPU6050_ALIGN; + gyroHardware = GYRO_MPU6050; + gyroAlign = GYRO_MPU6050_ALIGN; #endif - return true; - } + break; + } #endif - + ; // fallthrough + case GYRO_L3G4200D: #ifdef USE_GYRO_L3G4200D - if (l3g4200dDetect(&gyro, gyroLpf)) { + if (l3g4200dDetect(&gyro, gyroLpf)) { #ifdef GYRO_L3G4200D_ALIGN - gyroAlign = GYRO_L3G4200D_ALIGN; + gyroHardware = GYRO_L3G4200D; + gyroAlign = GYRO_L3G4200D_ALIGN; #endif - return true; - } + break; + } #endif + ; // fallthrough + case GYRO_MPU3050: #ifdef USE_GYRO_MPU3050 - if (mpu3050Detect(&gyro, gyroLpf)) { + if (mpu3050Detect(&gyro, gyroLpf)) { #ifdef GYRO_MPU3050_ALIGN - gyroAlign = GYRO_MPU3050_ALIGN; + gyroHardware = GYRO_MPU3050; + gyroAlign = GYRO_MPU3050_ALIGN; #endif - return true; - } + break; + } #endif + ; // fallthrough + case GYRO_L3GD20: #ifdef USE_GYRO_L3GD20 - if (l3gd20Detect(&gyro, gyroLpf)) { + if (l3gd20Detect(&gyro, gyroLpf)) { #ifdef GYRO_L3GD20_ALIGN - gyroAlign = GYRO_L3GD20_ALIGN; + gyroHardware = GYRO_L3GD20; + gyroAlign = GYRO_L3GD20_ALIGN; #endif - return true; - } + break; + } #endif + ; // fallthrough + case GYRO_SPI_MPU6000: #ifdef USE_GYRO_SPI_MPU6000 - if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) { + if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) { #ifdef GYRO_SPI_MPU6000_ALIGN - gyroAlign = GYRO_SPI_MPU6000_ALIGN; + gyroHardware = GYRO_SPI_MPU6000; + gyroAlign = GYRO_SPI_MPU6000_ALIGN; #endif - return true; - } + break; + } #endif + ; // fallthrough + case GYRO_SPI_MPU6500: #ifdef USE_GYRO_SPI_MPU6500 #ifdef NAZE - if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) { + if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) { #ifdef GYRO_SPI_MPU6500_ALIGN - gyroAlign = GYRO_SPI_MPU6500_ALIGN; + gyroHardware = GYRO_SPI_MPU6500; + gyroAlign = GYRO_SPI_MPU6500_ALIGN; #endif - return true; - } + break; + } #else - if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) { + if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) { #ifdef GYRO_SPI_MPU6500_ALIGN - gyroAlign = GYRO_SPI_MPU6500_ALIGN; + gyroHardware = GYRO_SPI_MPU6500; + gyroAlign = GYRO_SPI_MPU6500_ALIGN; #endif - return true; - } + break; + } #endif #endif + ; // fallthrough + case GYRO_FAKE: #ifdef USE_FAKE_GYRO - if (fakeGyroDetect(&gyro, gyroLpf)) { - return true; - } + if (fakeGyroDetect(&gyro, gyroLpf)) { + gyroHardware = GYRO_FAKE; + break; + } #endif - return false; + ; // fallthrough + case GYRO_NONE: + gyroHardware = GYRO_NONE; + } + + if (gyroHardware == GYRO_NONE) { + return false; + } + + detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; + sensorsSet(SENSOR_GYRO); + + return true; } -static void detectAcc(uint8_t accHardwareToUse) +static void detectAcc(accelerationSensor_e accHardwareToUse) { -#ifdef USE_ACC_ADXL345 + accelerationSensor_e accHardware; + + #ifdef USE_ACC_ADXL345 drv_adxl345_config_t acc_params; #endif @@ -224,17 +265,16 @@ retry: accAlign = ALIGN_DEFAULT; switch (accHardwareToUse) { + case ACC_DEFAULT: + ; // fallthrough + case ACC_FAKE: #ifdef USE_FAKE_ACC - default: if (fakeAccDetect(&acc)) { accHardware = ACC_FAKE; - if (accHardwareToUse == ACC_FAKE) - break; + break; } #endif - case ACC_NONE: // disable ACC - break; - case ACC_DEFAULT: // autodetect + ; // fallthrough case ACC_ADXL345: // ADXL345 #ifdef USE_ACC_ADXL345 acc_params.useFifo = false; @@ -248,8 +288,7 @@ retry: accAlign = ACC_ADXL345_ALIGN; #endif accHardware = ACC_ADXL345; - if (accHardwareToUse == ACC_ADXL345) - break; + break; } #endif ; // fallthrough @@ -260,8 +299,7 @@ retry: accAlign = ACC_LSM303DLHC_ALIGN; #endif accHardware = ACC_LSM303DLHC; - if (accHardwareToUse == ACC_LSM303DLHC) - break; + break; } #endif ; // fallthrough @@ -272,8 +310,7 @@ retry: accAlign = ACC_MPU6050_ALIGN; #endif accHardware = ACC_MPU6050; - if (accHardwareToUse == ACC_MPU6050) - break; + break; } #endif ; // fallthrough @@ -289,8 +326,7 @@ retry: accAlign = ACC_MMA8452_ALIGN; #endif accHardware = ACC_MMA8452; - if (accHardwareToUse == ACC_MMA8452) - break; + break; } #endif ; // fallthrough @@ -301,8 +337,7 @@ retry: accAlign = ACC_BMA280_ALIGN; #endif accHardware = ACC_BMA280; - if (accHardwareToUse == ACC_BMA280) - break; + break; } #endif ; // fallthrough @@ -313,8 +348,7 @@ retry: accAlign = ACC_SPI_MPU6000_ALIGN; #endif accHardware = ACC_SPI_MPU6000; - if (accHardwareToUse == ACC_SPI_MPU6000) - break; + break; } #endif ; // fallthrough @@ -329,25 +363,30 @@ retry: accAlign = ACC_SPI_MPU6500_ALIGN; #endif accHardware = ACC_SPI_MPU6500; - if (accHardwareToUse == ACC_SPI_MPU6500) - break; + break; } #endif ; // fallthrough + case ACC_NONE: // disable ACC + accHardware = ACC_NONE; + break; + } // Found anything? Check if error or ACC is really missing. - if (accHardware == ACC_DEFAULT) { - if (accHardwareToUse > ACC_DEFAULT && accHardwareToUse < ACC_NONE) { - // Nothing was found and we have a forced sensor that isn't present. - accHardwareToUse = ACC_DEFAULT; - goto retry; - } + if (accHardwareToUse != ACC_DEFAULT && accHardware == ACC_NONE) { + // Nothing was found and we have a forced sensor that isn't present. + accHardwareToUse = ACC_DEFAULT; + goto retry; } - if (accHardware != ACC_NONE) { - sensorsSet(SENSOR_ACC); + + if (accHardware == ACC_NONE) { + return; } + + detectedSensors[SENSOR_INDEX_ACC] = accHardware; + sensorsSet(SENSOR_ACC); } static void detectBaro() @@ -355,7 +394,7 @@ static void detectBaro() #ifdef BARO // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function - baroHardware = BARO_DEFAULT; + baroSensor_e baroHardware = BARO_DEFAULT; #ifdef USE_BARO_BMP085 @@ -399,19 +438,24 @@ static void detectBaro() break; } #endif - baroHardware = BARO_NONE; // nothing detected or enabled. case BARO_NONE: + baroHardware = BARO_NONE; break; } - if (baroHardware != BARO_NONE) { - sensorsSet(SENSOR_BARO); + if (baroHardware == BARO_NONE) { + return; } - #endif + + detectedSensors[SENSOR_INDEX_BARO] = baroHardware; + sensorsSet(SENSOR_BARO); +#endif } static void detectMag(magSensor_e magHardwareToUse) { + magSensor_e magHardware; + #ifdef USE_MAG_HMC5883 static hmc5883Config_t *hmc5883Config = 0; @@ -448,21 +492,17 @@ retry: magAlign = ALIGN_DEFAULT; switch(magHardwareToUse) { - case MAG_NONE: // disable MAG - break; - case MAG_DEFAULT: // autodetect + case MAG_DEFAULT: + ; // fallthrough case MAG_HMC5883: #ifdef USE_MAG_HMC5883 if (hmc5883lDetect(&mag, hmc5883Config)) { - sensorsSet(SENSOR_MAG); #ifdef MAG_HMC5883_ALIGN magAlign = MAG_HMC5883_ALIGN; #endif magHardware = MAG_HMC5883; - if (magHardwareToUse == MAG_HMC5883) - break; - + break; } #endif ; // fallthrough @@ -470,31 +510,32 @@ retry: case MAG_AK8975: #ifdef USE_MAG_AK8975 if (ak8975detect(&mag)) { - - sensorsSet(SENSOR_MAG); #ifdef MAG_AK8975_ALIGN magAlign = MAG_AK8975_ALIGN; #endif magHardware = MAG_AK8975; - if (magHardwareToUse == MAG_AK8975) - break; + break; } #endif ; // fallthrough + + case MAG_NONE: + magHardware = MAG_NONE; + break; } - if (magHardware == MAG_DEFAULT) { - if (magHardwareToUse > MAG_DEFAULT && magHardwareToUse < MAG_NONE) { - // Nothing was found and we have a forced sensor that isn't present. - magHardwareToUse = MAG_DEFAULT; - goto retry; - } + if (magHardwareToUse != MAG_DEFAULT && magHardware == MAG_NONE) { + // Nothing was found and we have a forced sensor that isn't present. + magHardwareToUse = MAG_DEFAULT; + goto retry; } - if (magHardware != MAG_NONE) { - sensorsSet(SENSOR_MAG); + if (magHardware == MAG_NONE) { + return; } + detectedSensors[SENSOR_INDEX_MAG] = magHardware; + sensorsSet(SENSOR_MAG); } void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) @@ -513,13 +554,13 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, int16_t magDeclinationFromConfig) { int16_t deg, min; + memset(&acc, sizeof(acc), 0); memset(&gyro, sizeof(gyro), 0); if (!detectGyro(gyroLpf)) { return false; } - sensorsSet(SENSOR_GYRO); detectAcc(accHardwareToUse); detectBaro(); diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index 805043d5c..78c452f46 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -17,6 +17,16 @@ #pragma once +typedef enum { + SENSOR_INDEX_GYRO = 0, + SENSOR_INDEX_ACC, + SENSOR_INDEX_BARO, + SENSOR_INDEX_MAG +} sensorIndex_e; + +#define MAX_SENSORS_TO_DETECT (SENSOR_INDEX_MAG + 1) + +extern uint8_t detectedSensors[MAX_SENSORS_TO_DETECT]; typedef struct int16_flightDynamicsTrims_s { int16_t roll;