From 6fe81a94459c0ae58c637ee7c6a014e210772791 Mon Sep 17 00:00:00 2001 From: Mark Haslinghuis Date: Tue, 28 Nov 2023 10:42:31 +0100 Subject: [PATCH] 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 | 2 + src/main/msp/msp.c | 45 +++++++++++++++++++---- src/main/msp/msp_protocol_v2_betaflight.h | 1 + src/main/sensors/barometer.c | 8 ++-- src/main/sensors/initialisation.c | 8 ++-- 5 files changed, 49 insertions(+), 15 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index aa295dcb1b2..3e431c8d9dd 100644 --- 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 index 33759885cfd..5d8a8de3d2f 100644 --- 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 index 2a7d681a866..b039defb71e 100644 --- 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 index 2820888b13f..f88c0d70ba3 100644 --- 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 index 89352f74f50..f20936d6ff7 100644 --- 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