Update the cli `status` command to show all detected sensors (Except on

CJMCU).

Further cleanup of sensor initialisation.

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[] = {
+// FIXME the next time the EEPROM is bumped change the order of acc and gyro names so that "None" is second.
-    "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", "None", NULL
+
+#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]);
+        mask = (1 << i);
-    }
+        if ((detectedSensorsMask & mask) && (mask & SENSOR_NAMES_MASK)) {
-    if (sensors(SENSOR_ACC)) {
+            const char *sensorHardware;
-        printf("ACCHW: %s", accNames[accHardware]);
+            uint8_t sensorHardwareIndex = detectedSensors[i];
-        if (acc.revisionCode)
+            sensorHardware = sensorHardwareNames[i][sensorHardwareIndex];
-            printf(".%c", acc.revisionCode);
+
+            printf(", %s=%s", sensorTypeNames[i], sensorHardware);
+
+            if (mask == SENSOR_ACC && acc.revisionCode) {
+                printf(".%c", acc.revisionCode);
+            }
+        }
     }
+#endif
     cliPrint("\r\n");
 
 #ifdef USE_I2C

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.
 

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;

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)
 {

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 {

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.
 

src/main/sensors/compass.h

@@ -20,9 +20,9 @@
 // Type of accelerometer used/detected
 typedef enum {
     MAG_DEFAULT = 0,
-    MAG_HMC5883 = 1,
+    MAG_NONE = 1,
-    MAG_AK8975 = 2,
+    MAG_HMC5883 = 2,
-    MAG_NONE = 3
+    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;

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;
 

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)) {
+            if (fakeGyroDetect(&gyro, gyroLpf)) {
-        return true;
+                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
+            ; // fallthrough
-            break;
-        case ACC_DEFAULT: // autodetect
         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 && accHardware == ACC_NONE) {
-        if (accHardwareToUse > ACC_DEFAULT && accHardwareToUse < ACC_NONE) {
+        // Nothing was found and we have a forced sensor that isn't present.
-            // Nothing was found and we have a forced sensor that isn't present.
+        accHardwareToUse = ACC_DEFAULT;
-            accHardwareToUse = ACC_DEFAULT;
+        goto retry;
-            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) {
+    if (baroHardware == BARO_NONE) {
-        sensorsSet(SENSOR_BARO);
+        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
+        case MAG_DEFAULT:
-            break;
+            ; // fallthrough
-        case MAG_DEFAULT: // autodetect
 
         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 && magHardware == MAG_NONE) {
-        if (magHardwareToUse > MAG_DEFAULT && magHardwareToUse < MAG_NONE) {
+        // Nothing was found and we have a forced sensor that isn't present.
-            // Nothing was found and we have a forced sensor that isn't present.
+        magHardwareToUse = MAG_DEFAULT;
-            magHardwareToUse = MAG_DEFAULT;
+        goto retry;
-            goto retry;
-        }
     }
 
-    if (magHardware != MAG_NONE) {
+    if (magHardware == MAG_NONE) {
-        sensorsSet(SENSOR_MAG);
+        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();
 

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;