Fix disabling sensor configuration (when device needs battery power) (b…

…etaflight#13177) * Fix sensor config * Encapsulate code * Add new message for detectedSensors * Add SENSOR_NOT_AVAILABLE * Move comment * Add gyro
KarateBrot · Nov 28, 2023 · 6fe81a9 · 6fe81a9
1 parent a8834ad
commit 6fe81a9
Show file tree

Hide file tree

Showing 5 changed files with 49 additions and 15 deletions.
diff --git a/src/main/flight/imu.c b/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

diff --git a/src/main/msp/msp.c b/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 == ACC_DEFAULT ? detectedSensors[1] : accelerometerConfig()->acc_hardware);
+        sbufWriteU8(dst, 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 == BARO_DEFAULT ? detectedSensors[2] : barometerConfig()->baro_hardware);
+        sbufWriteU8(dst, barometerConfig()->baro_hardware);
 #else
         sbufWriteU8(dst, BARO_NONE);
 #endif
 #ifdef USE_MAG
-        // Changed with API 1.46
-        sbufWriteU8(dst, compassConfig()->mag_hardware == MAG_DEFAULT ? detectedSensors[3] : compassConfig()->mag_hardware);
+        sbufWriteU8(dst, 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:
         {

diff --git a/src/main/msp/msp_protocol_v2_betaflight.h b/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

diff --git a/src/main/sensors/barometer.c b/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
-            DEBUG_SET(DEBUG_BARO, 2, baro.temperature);                 // c°C
-            DEBUG_SET(DEBUG_BARO, 3, lrintf(baro.altitude));            // cm
+            if (debugMode == DEBUG_BARO) {
+                DEBUG_SET(DEBUG_BARO, 1, lrintf(baro.pressure / 100.0f));   // hPa
+                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;

diff --git a/src/main/sensors/initialisation.c b/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