Fix disabling sensor configuration (when device needs battery power) (#13177)

* Fix sensor config

* Encapsulate code

* Add new message for detectedSensors

* Add SENSOR_NOT_AVAILABLE

* Move comment

* Add gyro

src/main/flight/imu.c

@@ -262,6 +262,7 @@ STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt, float gx, float gy, float
     fpVector3_t mag_ef;
     matrixVectorMul(&mag_ef, (const fpMat33_t*)&rMat, &mag_bf); // BF->EF true north
 
+#ifdef USE_GPS_RESCUE
     // Encapsulate additional operations in a block so that it is only executed when the according debug mode is used
     // Only re-calculate magYaw when there is a new Mag data reading, to avoid spikes
     if (debugMode == DEBUG_GPS_RESCUE_HEADING && mag.isNewMagADCFlag) {
@@ -279,6 +280,7 @@ STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt, float gx, float gy, float
         // note that if the debug doesn't run, this reset will not occur, and we won't waste cycles on the comparison
         mag.isNewMagADCFlag = false;
     }
+#endif
 
     if (useMag && magNormSquared > 0.01f) {
         // Normalise magnetometer measurement

src/main/msp/msp.c

@@ -2049,24 +2049,21 @@ case MSP_NAME:
         sbufWriteU8(dst, currentPidProfile->tpa_rate);
         sbufWriteU16(dst, currentPidProfile->tpa_breakpoint);   // was currentControlRateProfile->tpa_breakpoint
         break;
+
     case MSP_SENSOR_CONFIG:
-        // if sensor name is default setting, use name in runtime config
         // use sensorIndex_e index: 0:GyroHardware, 1:AccHardware, 2:BaroHardware, 3:MagHardware, 4:RangefinderHardware
 #if defined(USE_ACC)
-        // Changed with API 1.46
+        sbufWriteU8(dst, accelerometerConfig()->acc_hardware);
-        sbufWriteU8(dst, accelerometerConfig()->acc_hardware == ACC_DEFAULT ? detectedSensors[1] : accelerometerConfig()->acc_hardware);
 #else
-        sbufWriteU8(dst, 0);
+        sbufWriteU8(dst, ACC_NONE);
 #endif
 #ifdef USE_BARO
-        // Changed with API 1.46
+        sbufWriteU8(dst, barometerConfig()->baro_hardware);
-        sbufWriteU8(dst, barometerConfig()->baro_hardware == BARO_DEFAULT ? detectedSensors[2] : barometerConfig()->baro_hardware);
 #else
         sbufWriteU8(dst, BARO_NONE);
 #endif
 #ifdef USE_MAG
-        // Changed with API 1.46
+        sbufWriteU8(dst, compassConfig()->mag_hardware);
-        sbufWriteU8(dst, compassConfig()->mag_hardware == MAG_DEFAULT ? detectedSensors[3] : compassConfig()->mag_hardware);
 #else
         sbufWriteU8(dst, MAG_NONE);
 #endif
@@ -2078,6 +2075,38 @@ case MSP_NAME:
 #endif
         break;
 
+    // Added in MSP API 1.46
+    case MSP2_SENSOR_CONFIG_ACTIVE:
+
+#define SENSOR_NOT_AVAILABLE 0xFF
+
+#if defined(USE_GYRO)
+        sbufWriteU8(dst, detectedSensors[SENSOR_INDEX_GYRO]);
+#else
+        sbufWriteU8(dst, SENSOR_NOT_AVAILABLE);
+#endif
+#if defined(USE_ACC)
+        sbufWriteU8(dst, detectedSensors[SENSOR_INDEX_ACC]);
+#else
+        sbufWriteU8(dst, SENSOR_NOT_AVAILABLE);
+#endif
+#ifdef USE_BARO
+        sbufWriteU8(dst, detectedSensors[SENSOR_INDEX_BARO]);
+#else
+        sbufWriteU8(dst, SENSOR_NOT_AVAILABLE);
+#endif
+#ifdef USE_MAG
+        sbufWriteU8(dst, detectedSensors[SENSOR_INDEX_MAG]);
+#else
+        sbufWriteU8(dst, SENSOR_NOT_AVAILABLE);
+#endif
+#ifdef USE_RANGEFINDER
+        sbufWriteU8(dst, detectedSensors[SENSOR_INDEX_RANGEFINDER]);
+#else
+        sbufWriteU8(dst, SENSOR_NOT_AVAILABLE);
+#endif
+        break;
+
 #if defined(USE_VTX_COMMON)
     case MSP_VTX_CONFIG:
         {

src/main/msp/msp_protocol_v2_betaflight.h

@@ -28,6 +28,7 @@
 #define MSP2_SET_TEXT                       0x3007
 #define MSP2_GET_LED_STRIP_CONFIG_VALUES    0x3008
 #define MSP2_SET_LED_STRIP_CONFIG_VALUES    0x3009
+#define MSP2_SENSOR_CONFIG_ACTIVE           0x300A
 
 // MSP2_SET_TEXT and MSP2_GET_TEXT variable types
 #define MSP2TEXT_PILOT_NAME                      1

src/main/sensors/barometer.c

@@ -466,9 +466,11 @@ uint32_t baroUpdate(timeUs_t currentTimeUs)
                 }
             }
 
-            DEBUG_SET(DEBUG_BARO, 1, lrintf(baro.pressure / 100.0f));   // hPa
+            if (debugMode == DEBUG_BARO) {
-            DEBUG_SET(DEBUG_BARO, 2, baro.temperature);                 // c°C
+                DEBUG_SET(DEBUG_BARO, 1, lrintf(baro.pressure / 100.0f));   // hPa
-            DEBUG_SET(DEBUG_BARO, 3, lrintf(baro.altitude));            // cm
+                DEBUG_SET(DEBUG_BARO, 2, baro.temperature);                 // c°C
+                DEBUG_SET(DEBUG_BARO, 3, lrintf(baro.altitude));            // cm
+            }
 
             if (baro.dev.combined_read) {
                 state = BARO_STATE_PRESSURE_START;

src/main/sensors/initialisation.c

@@ -77,14 +77,14 @@ bool sensorsAutodetect(void)
     }
 #endif
 
-#ifdef USE_MAG
-    compassInit();
-#endif
-
 #ifdef USE_BARO
     baroInit();
 #endif
 
+#ifdef USE_MAG
+    compassInit();
+#endif
+
 #ifdef USE_RANGEFINDER
     rangefinderInit();
 #endif