diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index fccab9ab5..d12325048 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -60,6 +60,7 @@ uint8_t dynP8[3], dynI8[3], dynD8[3];
 static int32_t errorGyroI[3] = { 0, 0, 0 };
 static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
 static int32_t errorAngleI[2] = { 0, 0 };
+static float errorAngleIf[2] = { 0.0f, 0.0f };
 
 static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);
@@ -73,6 +74,9 @@ void resetErrorAngle(void)
 {
     errorAngleI[ROLL] = 0;
     errorAngleI[PITCH] = 0;
+
+    errorAngleIf[ROLL] = 0.0f;
+    errorAngleIf[PITCH] = 0.0f;
 }
 
 void resetErrorGyro(void)
@@ -81,9 +85,9 @@ void resetErrorGyro(void)
     errorGyroI[PITCH] = 0;
     errorGyroI[YAW] = 0;
 
-    errorGyroIf[ROLL] = 0;
-    errorGyroIf[PITCH] = 0;
-    errorGyroIf[YAW] = 0;
+    errorGyroIf[ROLL] = 0.0f;
+    errorGyroIf[PITCH] = 0.0f;
+    errorGyroIf[YAW] = 0.0f;
 }
 
 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
@@ -518,14 +522,11 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
 {
     UNUSED(rxConfig);
 
-    float delta, RCfactor, rcCommandAxis, MainDptCut;
-    float PTerm = 0, ITerm = 0, DTerm = 0, PTermACC = 0, ITermACC = 0, ITermGYRO = 0, error = 0, prop = 0;
-    static float lastGyro[2] = {0, 0}, lastDTerm[2] = {0, 0};
-    float tmp0flt;
-    int32_t tmp0;
+    float delta, RCfactor, rcCommandAxis, MainDptCut, gyroDataQuant;
+    float PTerm, ITerm, DTerm, PTermACC = 0.0f, ITermACC = 0.0f, ITermGYRO, error, prop = 0.0f;
+    static float lastGyro[2] = { 0.0f, 0.0f }, lastDTerm[2] = { 0.0f, 0.0f };
     uint8_t axis;
-    float ACCDeltaTimeINS = 0;
-    float FLOATcycleTime = 0;
+    float ACCDeltaTimeINS, FLOATcycleTime, Mwii3msTimescale;
 
 //    MainDptCut = RCconstPI / (float)cfg.maincuthz;                           // Initialize Cut off frequencies for mainpid D
     MainDptCut = RCconstPI / MAIN_CUT_HZ;                                      // maincuthz (default 12Hz, Range 1-50Hz), hardcoded for now
@@ -538,6 +539,8 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
     }
 
     for (axis = 0; axis < 2; axis++) {
+        int32_t tmp = (int32_t)((float)gyroData[axis] * 0.3125f);             // Multiwii masks out the last 2 bits, this has the same idea
+        gyroDataQuant = (float)tmp * 3.2f;                                    // but delivers more accuracy and also reduces jittery flight
         rcCommandAxis = (float)rcCommand[axis];                                // Calculate common values for pid controllers
         if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 #ifdef GPS
@@ -552,21 +555,21 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
             }
 #endif
             PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f;
-            tmp0flt = (float)pidProfile->D8[PIDLEVEL] * 5.0f;
-            PTermACC = constrain(PTermACC, -tmp0flt, +tmp0flt);
-            errorAngleI[axis] = constrain(errorAngleI[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f);
-            ITermACC = errorAngleI[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f;
+            float limitf = (float)pidProfile->D8[PIDLEVEL] * 5.0f;
+            PTermACC = constrain(PTermACC, -limitf, +limitf);
+            errorAngleIf[axis] = constrain(errorAngleIf[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f);
+            ITermACC = errorAngleIf[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f;
         }
 
         if (!FLIGHT_MODE(ANGLE_MODE)) {
             if (ABS((int16_t)gyroData[axis]) > 2560) {
-                errorGyroI[axis] = 0.0f;
+                errorGyroIf[axis] = 0.0f;
             } else {
-                error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroData[axis];
-                errorGyroI[axis] = constrain(errorGyroI[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f);
+                error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroDataQuant;
+                errorGyroIf[axis] = constrainf(errorGyroIf[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f);
             }
 
-            ITermGYRO = errorGyroI[axis] * (float)pidProfile->I8[axis] * 0.01f;
+            ITermGYRO = errorGyroIf[axis] * (float)pidProfile->I8[axis] * 0.01f;
 
             if (FLIGHT_MODE(HORIZON_MODE)) {
                 PTerm = PTermACC + prop * (rcCommandAxis - PTermACC);
@@ -580,10 +583,10 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
             ITerm = ITermACC;
         }
 
-        PTerm -= gyroData[axis] * dynP8[axis] * 0.003f;
-        delta = (gyroData[axis] - lastGyro[axis]) / ACCDeltaTimeINS;
+        PTerm -= gyroDataQuant * dynP8[axis] * 0.003f;
+        delta = (gyroDataQuant - lastGyro[axis]) / ACCDeltaTimeINS;
 
-        lastGyro[axis] = gyroData[axis];
+        lastGyro[axis] = gyroDataQuant;
         lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]);
         DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f;
 
@@ -596,37 +599,37 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
 #endif
     }
 
-    tmp0flt = (int32_t)FLOATcycleTime & (int32_t)~3;                          // Filter last 2 bit jitter
-    tmp0flt /= 3000.0f;
+    Mwii3msTimescale = (int32_t)FLOATcycleTime & (int32_t)~3;                  // Filter last 2 bit jitter
+    Mwii3msTimescale /= 3000.0f;
 
     if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now
         PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100;
-        tmp0 = lrintf(gyroData[FD_YAW] * 0.25f);
-        PTerm = rcCommand[FD_YAW] - tmp0 * PTerm / 80;
-        if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) {
+        int32_t tmp = lrintf(gyroData[FD_YAW] * 0.25f);
+        PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80;
+        if ((ABS(tmp) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) {
             errorGyroI[FD_YAW] = 0;
         } else {
-            error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp0;
-            errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * tmp0flt), -16000, +16000); // WindUp
+            error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp;
+            errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * Mwii3msTimescale), -16000, +16000); // WindUp
             ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6;
         }
     } else {
-        tmp0 = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5;
-        error = tmp0 - lrintf(gyroData[FD_YAW] * 0.25f);                       // Less Gyrojitter works actually better
+        int32_t tmp = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5;
+        error = tmp - lrintf(gyroData[FD_YAW] * 0.25f);                       // Less Gyrojitter works actually better
 
-        if (ABS(tmp0) > 50) {
+        if (ABS(tmp) > 50) {
             errorGyroI[FD_YAW] = 0;
         } else {
-            errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454);
+            errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * Mwii3msTimescale), -268435454, +268435454);
         }
 
         ITerm = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX);
         PTerm = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6;
 
         if (motorCount >= 4) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply
-            tmp0 = 300;
-            if (pidProfile->D8[FD_YAW]) tmp0 -= (int32_t)pidProfile->D8[FD_YAW];
-            PTerm = constrain(PTerm, -tmp0, tmp0);
+            int32_t limit = 300;
+            if (pidProfile->D8[FD_YAW]) limit -= (int32_t)pidProfile->D8[FD_YAW];
+            PTerm = constrain(PTerm, -limit, limit);
         }
     }
     axisPID[FD_YAW] = PTerm + ITerm;
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 067c3e933..b82ff9231 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -148,14 +148,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;
@@ -1353,27 +1362,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/main.c b/src/main/main.c
index d75e06b1c..94bc2a1b5 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -296,9 +296,6 @@ void init(void)
     }
 #endif
 
-    // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
-    sensorsSet(SENSORS_SET);
-    // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
     sensorsOK = sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination);
 
     // if gyro was not detected due to whatever reason, we give up now.
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index f02e0e992..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
-uint8_t 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 159393aa1..5b925b01d 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -18,7 +18,7 @@
 #pragma once
 
 // Type of accelerometer used/detected
-typedef enum AccelSensors {
+typedef enum {
     ACC_DEFAULT = 0,
     ACC_ADXL345 = 1,
     ACC_MPU6050 = 2,
@@ -29,9 +29,8 @@ typedef enum AccelSensors {
     ACC_SPI_MPU6500 = 7,
     ACC_FAKE = 8,
     ACC_NONE = 9
-} accelSensor_e;
+} 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.h b/src/main/sensors/barometer.h
index 9edd7c6a5..685f6b843 100644
--- a/src/main/sensors/barometer.h
+++ b/src/main/sensors/barometer.h
@@ -17,6 +17,13 @@
 
 #pragma once
 
+typedef enum {
+    BARO_NONE = 0,
+    BARO_DEFAULT = 1,
+    BARO_BMP085 = 2,
+    BARO_MS5611 = 3
+} baroSensor_e;
+
 #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 e3c18036a..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
-uint8_t 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 374cac185..502c18728 100644
--- a/src/main/sensors/compass.h
+++ b/src/main/sensors/compass.h
@@ -18,11 +18,11 @@
 #pragma once
 
 // Type of accelerometer used/detected
-typedef enum MagSensors {
+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 f278893e7..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,20 +265,18 @@ 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
-            sensorsClear(SENSOR_ACC);
-            break;
-        case ACC_DEFAULT: // autodetect
-#ifdef USE_ACC_ADXL345
+            ; // fallthrough
         case ACC_ADXL345: // ADXL345
+#ifdef USE_ACC_ADXL345
             acc_params.useFifo = false;
             acc_params.dataRate = 800; // unused currently
 #ifdef NAZE
@@ -249,37 +288,34 @@ retry:
                 accAlign = ACC_ADXL345_ALIGN;
 #endif
                 accHardware = ACC_ADXL345;
-                if (accHardwareToUse == ACC_ADXL345)
-                    break;
+                break;
             }
-            ; // fallthrough
 #endif
-#ifdef USE_ACC_LSM303DLHC
+            ; // fallthrough
         case ACC_LSM303DLHC:
+#ifdef USE_ACC_LSM303DLHC
             if (lsm303dlhcAccDetect(&acc)) {
 #ifdef ACC_LSM303DLHC_ALIGN
                 accAlign = ACC_LSM303DLHC_ALIGN;
 #endif
                 accHardware = ACC_LSM303DLHC;
-                if (accHardwareToUse == ACC_LSM303DLHC)
-                    break;
+                break;
             }
-            ; // fallthrough
 #endif
-#ifdef USE_ACC_MPU6050
+            ; // fallthrough
         case ACC_MPU6050: // MPU6050
+#ifdef USE_ACC_MPU6050
             if (mpu6050AccDetect(selectMPU6050Config(), &acc)) {
 #ifdef ACC_MPU6050_ALIGN
                 accAlign = ACC_MPU6050_ALIGN;
 #endif
                 accHardware = ACC_MPU6050;
-                if (accHardwareToUse == ACC_MPU6050)
-                    break;
+                break;
             }
-            ; // fallthrough
 #endif
-#ifdef USE_ACC_MMA8452
+            ; // fallthrough
         case ACC_MMA8452: // MMA8452
+#ifdef USE_ACC_MMA8452
 #ifdef NAZE
             // Not supported with this frequency
             if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) {
@@ -290,37 +326,34 @@ retry:
                 accAlign = ACC_MMA8452_ALIGN;
 #endif
                 accHardware = ACC_MMA8452;
-                if (accHardwareToUse == ACC_MMA8452)
-                    break;
+                break;
             }
-            ; // fallthrough
 #endif
-#ifdef USE_ACC_BMA280
+            ; // fallthrough
         case ACC_BMA280: // BMA280
+#ifdef USE_ACC_BMA280
             if (bma280Detect(&acc)) {
 #ifdef ACC_BMA280_ALIGN
                 accAlign = ACC_BMA280_ALIGN;
 #endif
                 accHardware = ACC_BMA280;
-                if (accHardwareToUse == ACC_BMA280)
-                    break;
+                break;
             }
-            ; // fallthrough
 #endif
-#ifdef USE_ACC_SPI_MPU6000
+            ; // fallthrough
         case ACC_SPI_MPU6000:
+#ifdef USE_ACC_SPI_MPU6000
             if (mpu6000SpiAccDetect(&acc)) {
 #ifdef ACC_SPI_MPU6000_ALIGN
                 accAlign = ACC_SPI_MPU6000_ALIGN;
 #endif
                 accHardware = ACC_SPI_MPU6000;
-                if (accHardwareToUse == ACC_SPI_MPU6000)
-                    break;
+                break;
             }
-            ; // fallthrough
 #endif
-#ifdef USE_ACC_SPI_MPU6500
+            ; // fallthrough
         case ACC_SPI_MPU6500:
+#ifdef USE_ACC_SPI_MPU6500
 #ifdef NAZE
             if (hardwareRevision == NAZE32_SP && mpu6500SpiAccDetect(&acc)) {
 #else
@@ -330,32 +363,39 @@ retry:
                 accAlign = ACC_SPI_MPU6500_ALIGN;
 #endif
                 accHardware = ACC_SPI_MPU6500;
-                if (accHardwareToUse == ACC_SPI_MPU6500)
-                    break;
+                break;
             }
-            ; // fallthrough
 #endif
-            ; // prevent compiler error
+            ; // 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;
-        } else {
-            // No ACC was detected
-            sensorsClear(SENSOR_ACC);
-        }
+    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) {
+        return;
+    }
+
+    detectedSensors[SENSOR_INDEX_ACC] = accHardware;
+    sensorsSet(SENSOR_ACC);
 }
 
 static void detectBaro()
 {
+#ifdef BARO
     // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
 
-    #ifdef BARO
+    baroSensor_e baroHardware = BARO_DEFAULT;
+
 #ifdef USE_BARO_BMP085
 
     const bmp085Config_t *bmp085Config = NULL;
@@ -379,23 +419,43 @@ static void detectBaro()
 
 #endif
 
-#ifdef USE_BARO_MS5611
-    if (ms5611Detect(&baro)) {
-        return;
-    }
-#endif
+    switch (baroHardware) {
+        case BARO_DEFAULT:
+            ; // fallthough
 
+        case BARO_MS5611:
+#ifdef USE_BARO_MS5611
+            if (ms5611Detect(&baro)) {
+                baroHardware = BARO_MS5611;
+                break;
+            }
+#endif
+            ; // fallthough
+        case BARO_BMP085:
 #ifdef USE_BARO_BMP085
-    if (bmp085Detect(bmp085Config, &baro)) {
+            if (bmp085Detect(bmp085Config, &baro)) {
+                baroHardware = BARO_BMP085;
+                break;
+            }
+#endif
+        case BARO_NONE:
+            baroHardware = BARO_NONE;
+            break;
+    }
+
+    if (baroHardware == BARO_NONE) {
         return;
     }
+
+    detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
+    sensorsSet(SENSOR_BARO);
 #endif
-#endif
-    sensorsClear(SENSOR_BARO);
 }
 
-static void detectMag(uint8_t magHardwareToUse)
+static void detectMag(magSensor_e magHardwareToUse)
 {
+    magSensor_e magHardware;
+
 #ifdef USE_MAG_HMC5883
     static hmc5883Config_t *hmc5883Config = 0;
 
@@ -432,52 +492,50 @@ retry:
     magAlign = ALIGN_DEFAULT;
 
     switch(magHardwareToUse) {
-        case MAG_NONE: // disable MAG
-            sensorsClear(SENSOR_MAG);
-            break;
-        case MAG_DEFAULT: // autodetect
+        case MAG_DEFAULT:
+            ; // fallthrough
 
-#ifdef USE_MAG_HMC5883
         case MAG_HMC5883:
+#ifdef USE_MAG_HMC5883
             if (hmc5883lDetect(&mag, hmc5883Config)) {
 #ifdef MAG_HMC5883_ALIGN
                 magAlign = MAG_HMC5883_ALIGN;
 #endif
                 magHardware = MAG_HMC5883;
-                if (magHardwareToUse == MAG_HMC5883)
-                    break;
-
+                break;
             }
-            ; // fallthrough
 #endif
+            ; // fallthrough
 
-#ifdef USE_MAG_AK8975
         case MAG_AK8975:
+#ifdef USE_MAG_AK8975
             if (ak8975detect(&mag)) {
-
 #ifdef MAG_AK8975_ALIGN
                 magAlign = MAG_AK8975_ALIGN;
 #endif
                 magHardware = MAG_AK8975;
-                if (magHardwareToUse == MAG_AK8975)
-                    break;
+                break;
             }
-            ; // fallthrough
 #endif
-            ; // prevent compiler error.
+            ; // 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;
-        } else {
-            // No MAG was detected
-            sensorsClear(SENSOR_MAG);
-        }
+    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) {
+        return;
+    }
+
+    detectedSensors[SENSOR_INDEX_MAG] = magHardware;
+    sensorsSet(SENSOR_MAG);
 }
 
 void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
@@ -496,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;
diff --git a/src/main/target/ALIENWIIF3/target.h b/src/main/target/ALIENWIIF3/target.h
index 009782a69..924a9306f 100644
--- a/src/main/target/ALIENWIIF3/target.h
+++ b/src/main/target/ALIENWIIF3/target.h
@@ -86,9 +86,6 @@
 #define I2C2_SDA_PIN_SOURCE  GPIO_PinSource10
 #define I2C2_SDA_CLK_SOURCE  RCC_AHBPeriph_GPIOA
 
-
-#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
-
 #define BLACKBOX
 #define SERIAL_RX
 #define GPS
diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h
index a06d3540a..b0e1a2f7a 100644
--- a/src/main/target/CC3D/target.h
+++ b/src/main/target/CC3D/target.h
@@ -98,8 +98,6 @@
 #define RSSI_ADC_CHANNEL            ADC_Channel_1
 
 
-#define SENSORS_SET (SENSOR_ACC)
-
 #define GPS
 #define LED_STRIP
 #define LED_STRIP_TIMER TIM3
diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h
index e27951b9f..d8a0f704a 100644
--- a/src/main/target/CHEBUZZF3/target.h
+++ b/src/main/target/CHEBUZZF3/target.h
@@ -90,9 +90,6 @@
 #define EXTERNAL1_ADC_GPIO_PIN      GPIO_Pin_3
 #define EXTERNAL1_ADC_CHANNEL       ADC_Channel_9
 
-
-#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
-
 #define GPS
 #define LED_STRIP
 #if 1
diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h
index 2bdabd78e..ac4088e08 100644
--- a/src/main/target/CJMCU/target.h
+++ b/src/main/target/CJMCU/target.h
@@ -60,8 +60,6 @@
 // #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011)
 // #define SOFT_I2C_PB67
 
-#define SENSORS_SET (SENSOR_ACC | SENSOR_MAG)
-
 #define SERIAL_RX
 
 #define SPEKTRUM_BIND
diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h
index ccc9bebaa..48ac2fd27 100644
--- a/src/main/target/EUSTM32F103RC/target.h
+++ b/src/main/target/EUSTM32F103RC/target.h
@@ -116,8 +116,6 @@
 #define EXTERNAL1_ADC_GPIO_PIN      GPIO_Pin_5
 #define EXTERNAL1_ADC_CHANNEL       ADC_Channel_5
 
-#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
-
 #define GPS
 #define LED_STRIP
 #define LED_STRIP_TIMER TIM3
diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index c2109e5c2..7d53b0c4e 100644
--- a/src/main/target/NAZE/target.h
+++ b/src/main/target/NAZE/target.h
@@ -135,8 +135,6 @@
 #define EXTERNAL1_ADC_GPIO_PIN      GPIO_Pin_5
 #define EXTERNAL1_ADC_CHANNEL       ADC_Channel_5
 
-#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
-
 #define GPS
 
 #define LED_STRIP
diff --git a/src/main/target/NAZE32PRO/target.h b/src/main/target/NAZE32PRO/target.h
index 19f806d71..7435a8e99 100644
--- a/src/main/target/NAZE32PRO/target.h
+++ b/src/main/target/NAZE32PRO/target.h
@@ -40,8 +40,6 @@
 #define USE_I2C
 #define I2C_DEVICE (I2CDEV_1)
 
-#define SENSORS_SET (SENSOR_ACC)
-
 #define GPS
 #define BLACKBOX
 #define TELEMETRY
diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h
index 87dd9aadd..6288c6b40 100644
--- a/src/main/target/OLIMEXINO/target.h
+++ b/src/main/target/OLIMEXINO/target.h
@@ -103,9 +103,6 @@
 #define EXTERNAL1_ADC_GPIO_PIN      GPIO_Pin_5
 #define EXTERNAL1_ADC_CHANNEL       ADC_Channel_5
 
-
-#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
-
 #define GPS
 #define LED_STRIP
 #define LED_STRIP_TIMER TIM3
diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h
index 0f4aa3904..f3af089d0 100644
--- a/src/main/target/PORT103R/target.h
+++ b/src/main/target/PORT103R/target.h
@@ -126,8 +126,6 @@
 #define EXTERNAL1_ADC_GPIO_PIN      GPIO_Pin_5
 #define EXTERNAL1_ADC_CHANNEL       ADC_Channel_5
 
-#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
-
 #define LED0
 #define GPS
 #define LED_STRIP
diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h
index d9bfe31e3..2c6101d7c 100644
--- a/src/main/target/SPARKY/target.h
+++ b/src/main/target/SPARKY/target.h
@@ -86,9 +86,6 @@
 #define I2C2_SDA_PIN_SOURCE  GPIO_PinSource10
 #define I2C2_SDA_CLK_SOURCE  RCC_AHBPeriph_GPIOA
 
-
-#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
-
 #define BLACKBOX
 #define SERIAL_RX
 #define GPS
diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h
index ae1feff44..4af92665a 100644
--- a/src/main/target/SPRACINGF3/target.h
+++ b/src/main/target/SPRACINGF3/target.h
@@ -120,8 +120,6 @@
 #define WS2811_DMA_CHANNEL              DMA1_Channel2
 #define WS2811_IRQ                      DMA1_Channel2_IRQn
 
-#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
-
 #define GPS
 #define BLACKBOX
 #define TELEMETRY
diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h
index 29f5f39e8..a98f03d84 100644
--- a/src/main/target/STM32F3DISCOVERY/target.h
+++ b/src/main/target/STM32F3DISCOVERY/target.h
@@ -81,8 +81,6 @@
 #define EXTERNAL1_ADC_GPIO_PIN      GPIO_Pin_3
 #define EXTERNAL1_ADC_CHANNEL       ADC_Channel_9
 
-#define SENSORS_SET (SENSOR_ACC | SENSOR_MAG)
-
 #define BLACKBOX
 #define GPS
 #define LED_STRIP