From 8789246110766cb2e8a29db6a1a10bf6151e7b1b Mon Sep 17 00:00:00 2001 From: Mark Hale Date: Thu, 8 Nov 2018 23:22:54 +0000 Subject: [PATCH] Support for disabling individual telemetry sensors. Signed-off-by: Mark Hale --- src/main/interface/settings.c | 2 +- src/main/telemetry/crsf.c | 8 +-- src/main/telemetry/frsky_hub.c | 36 +++++++++---- src/main/telemetry/smartport.c | 56 ++++++++++++++------ src/main/telemetry/telemetry.h | 32 ++++++++--- src/test/unit/telemetry_crsf_msp_unittest.cc | 5 ++ src/test/unit/telemetry_crsf_unittest.cc | 1 + src/test/unit/telemetry_hott_unittest.cc | 5 ++ src/test/unit/telemetry_ibus_unittest.cc | 6 +++ 9 files changed, 112 insertions(+), 39 deletions(-) diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 882bd75ee..d81cb1048 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -969,7 +969,7 @@ const clivalue_t valueTable[] = { // Set to $size_of_battery to get a percentage of battery used. { "mavlink_mah_as_heading_divisor", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 30000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, mavlink_mah_as_heading_divisor) }, #endif - { "telemetry_disabled_sensors", VAR_UINT32 | MASTER_VALUE, .config.minmax = { 0, 15 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, disabledSensors)}, + { "telemetry_disabled_sensors", VAR_UINT32 | MASTER_VALUE, .config.u32_max = SENSOR_ALL, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, disabledSensors)}, #endif // USE_TELEMETRY // PG_LED_STRIP_CONFIG diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index e60643f87..4f4d7408d 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -446,15 +446,17 @@ void initCrsfTelemetry(void) #endif int index = 0; - if (sensors(SENSOR_ACC)) { + if (sensors(SENSOR_ACC) && telemetryIsSensorEnabled(SENSOR_PITCH | SENSOR_ROLL | SENSOR_HEADING)) { crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX); } - if (isBatteryVoltageConfigured() || isAmperageConfigured()) { + if ((isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE)) + || (isAmperageConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT | SENSOR_FUEL))) { crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX); } crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX); #ifdef USE_GPS - if (featureIsEnabled(FEATURE_GPS)) { + if (featureIsEnabled(FEATURE_GPS) + && telemetryIsSensorEnabled(SENSOR_ALTITUDE | SENSOR_LAT_LONG | SENSOR_GROUND_SPEED | SENSOR_HEADING)) { crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX); } #endif diff --git a/src/main/telemetry/frsky_hub.c b/src/main/telemetry/frsky_hub.c index 069c65b08..ddb96bf07 100644 --- a/src/main/telemetry/frsky_hub.c +++ b/src/main/telemetry/frsky_hub.c @@ -519,7 +519,7 @@ void processFrSkyHubTelemetry(timeUs_t currentTimeUs) cycleNum++; - if (sensors(SENSOR_ACC)) { + if (sensors(SENSOR_ACC) && telemetryIsSensorEnabled(SENSOR_ACC_X | SENSOR_ACC_Y | SENSOR_ACC_Z)) { // Sent every 125ms sendAccel(); } @@ -530,11 +530,13 @@ void processFrSkyHubTelemetry(timeUs_t currentTimeUs) // Send vertical speed for opentx. ID_VERT_SPEED // Unit is cm/s #ifdef USE_VARIO - frSkyHubWriteFrame(ID_VERT_SPEED, getEstimatedVario()); + if (telemetryIsSensorEnabled(SENSOR_VARIO)) { + frSkyHubWriteFrame(ID_VERT_SPEED, getEstimatedVario()); + } #endif // Sent every 500ms - if ((cycleNum % 4) == 0) { + if ((cycleNum % 4) == 0 && telemetryIsSensorEnabled(SENSOR_ALTITUDE)) { int32_t altitudeCm = getEstimatedAltitudeCm(); /* Allow 5s to boot correctly othervise send zero to prevent OpenTX @@ -550,7 +552,7 @@ void processFrSkyHubTelemetry(timeUs_t currentTimeUs) #endif #if defined(USE_MAG) - if (sensors(SENSOR_MAG)) { + if (sensors(SENSOR_MAG) && telemetryIsSensorEnabled(SENSOR_HEADING)) { // Sent every 500ms if ((cycleNum % 4) == 0) { sendHeading(); @@ -564,21 +566,33 @@ void processFrSkyHubTelemetry(timeUs_t currentTimeUs) sendThrottleOrBatterySizeAsRpm(); if (isBatteryVoltageConfigured()) { - sendVoltageCells(); - sendVoltageAmp(); + if (telemetryIsSensorEnabled(SENSOR_VOLTAGE)) { + sendVoltageCells(); + sendVoltageAmp(); + } if (isAmperageConfigured()) { - sendAmperage(); - sendFuelLevel(); + if (telemetryIsSensorEnabled(SENSOR_CURRENT)) { + sendAmperage(); + } + if (telemetryIsSensorEnabled(SENSOR_FUEL)) { + sendFuelLevel(); + } } } #if defined(USE_GPS) if (sensors(SENSOR_GPS)) { - sendSpeed(); - sendGpsAltitude(); + if (telemetryIsSensorEnabled(SENSOR_GROUND_SPEED)) { + sendSpeed(); + } + if (telemetryIsSensorEnabled(SENSOR_ALTITUDE)) { + sendGpsAltitude(); + } sendSatalliteSignalQualityAsTemperature2(cycleNum); - sendGPSLatLong(); + if (telemetryIsSensorEnabled(SENSOR_LAT_LONG)) { + sendGPSLatLong(); + } } else #endif #if defined(USE_MAG) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index ad5035b89..ed83b5bec 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -320,10 +320,12 @@ static void initSmartPortSensors(void) { frSkyDataIdTableInfo.index = 0; - ADD_SENSOR(FSSP_DATAID_T1); - ADD_SENSOR(FSSP_DATAID_T2); + if (telemetryIsSensorEnabled(SENSOR_MODE)) { + ADD_SENSOR(FSSP_DATAID_T1); + ADD_SENSOR(FSSP_DATAID_T2); + } - if (isBatteryVoltageConfigured()) { + if (isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE)) { #ifdef USE_ESC_SENSOR_TELEMETRY if (!telemetryIsSensorEnabled(ESC_SENSOR_VOLTAGE)) #endif @@ -334,7 +336,7 @@ static void initSmartPortSensors(void) ADD_SENSOR(FSSP_DATAID_A4); } - if (isAmperageConfigured()) { + if (isAmperageConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT)) { #ifdef USE_ESC_SENSOR_TELEMETRY if (!telemetryIsSensorEnabled(ESC_SENSOR_CURRENT)) #endif @@ -342,28 +344,50 @@ static void initSmartPortSensors(void) ADD_SENSOR(FSSP_DATAID_CURRENT); } - ADD_SENSOR(FSSP_DATAID_FUEL); + if (telemetryIsSensorEnabled(SENSOR_FUEL)) { + ADD_SENSOR(FSSP_DATAID_FUEL); + } } if (sensors(SENSOR_ACC)) { - ADD_SENSOR(FSSP_DATAID_HEADING); - ADD_SENSOR(FSSP_DATAID_ACCX); - ADD_SENSOR(FSSP_DATAID_ACCY); - ADD_SENSOR(FSSP_DATAID_ACCZ); + if (telemetryIsSensorEnabled(SENSOR_HEADING)) { + ADD_SENSOR(FSSP_DATAID_HEADING); + } + if (telemetryIsSensorEnabled(SENSOR_ACC_X)) { + ADD_SENSOR(FSSP_DATAID_ACCX); + } + if (telemetryIsSensorEnabled(SENSOR_ACC_Y)) { + ADD_SENSOR(FSSP_DATAID_ACCY); + } + if (telemetryIsSensorEnabled(SENSOR_ACC_Z)) { + ADD_SENSOR(FSSP_DATAID_ACCZ); + } } if (sensors(SENSOR_BARO)) { - ADD_SENSOR(FSSP_DATAID_ALTITUDE); - ADD_SENSOR(FSSP_DATAID_VARIO); + if (telemetryIsSensorEnabled(SENSOR_ALTITUDE)) { + ADD_SENSOR(FSSP_DATAID_ALTITUDE); + } + if (telemetryIsSensorEnabled(SENSOR_VARIO)) { + ADD_SENSOR(FSSP_DATAID_VARIO); + } } #ifdef USE_GPS if (featureIsEnabled(FEATURE_GPS)) { - ADD_SENSOR(FSSP_DATAID_SPEED); - ADD_SENSOR(FSSP_DATAID_LATLONG); - ADD_SENSOR(FSSP_DATAID_LATLONG); // twice (one for lat, one for long) - ADD_SENSOR(FSSP_DATAID_HOME_DIST); - ADD_SENSOR(FSSP_DATAID_GPS_ALT); + if (telemetryIsSensorEnabled(SENSOR_GROUND_SPEED)) { + ADD_SENSOR(FSSP_DATAID_SPEED); + } + if (telemetryIsSensorEnabled(SENSOR_LAT_LONG)) { + ADD_SENSOR(FSSP_DATAID_LATLONG); + ADD_SENSOR(FSSP_DATAID_LATLONG); // twice (one for lat, one for long) + } + if (telemetryIsSensorEnabled(SENSOR_DISTANCE)) { + ADD_SENSOR(FSSP_DATAID_HOME_DIST); + } + if (telemetryIsSensorEnabled(SENSOR_ALTITUDE)) { + ADD_SENSOR(FSSP_DATAID_GPS_ALT); + } } #endif diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 7a2bf01c0..6bfb1d16f 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -42,14 +42,30 @@ typedef enum { } frskyUnit_e; typedef enum { - ESC_SENSOR_CURRENT = 1 << 0, - ESC_SENSOR_VOLTAGE = 1 << 1, - ESC_SENSOR_RPM = 1 << 2, - ESC_SENSOR_TEMPERATURE = 1 << 3, - ESC_SENSOR_ALL = ESC_SENSOR_CURRENT \ - | ESC_SENSOR_VOLTAGE \ - | ESC_SENSOR_RPM \ - | ESC_SENSOR_TEMPERATURE, + SENSOR_VOLTAGE = 1 << 0, + SENSOR_CURRENT = 1 << 1, + SENSOR_FUEL = 1 << 2, + SENSOR_MODE = 1 << 3, + SENSOR_ACC_X = 1 << 4, + SENSOR_ACC_Y = 1 << 5, + SENSOR_ACC_Z = 1 << 6, + SENSOR_PITCH = 1 << 7, + SENSOR_ROLL = 1 << 8, + SENSOR_HEADING = 1 << 9, + SENSOR_ALTITUDE = 1 << 10, + SENSOR_VARIO = 1 << 11, + SENSOR_LAT_LONG = 1 << 12, + SENSOR_GROUND_SPEED = 1 << 13, + SENSOR_DISTANCE = 1 << 14, + ESC_SENSOR_CURRENT = 1 << 15, + ESC_SENSOR_VOLTAGE = 1 << 16, + ESC_SENSOR_RPM = 1 << 17, + ESC_SENSOR_TEMPERATURE = 1 << 18, + ESC_SENSOR_ALL = ESC_SENSOR_CURRENT \ + | ESC_SENSOR_VOLTAGE \ + | ESC_SENSOR_RPM \ + | ESC_SENSOR_TEMPERATURE, + SENSOR_ALL = (1 << 19) - 1, } sensor_e; typedef struct telemetryConfig_s { diff --git a/src/test/unit/telemetry_crsf_msp_unittest.cc b/src/test/unit/telemetry_crsf_msp_unittest.cc index ddd5225da..cef85b5e2 100644 --- a/src/test/unit/telemetry_crsf_msp_unittest.cc +++ b/src/test/unit/telemetry_crsf_msp_unittest.cc @@ -304,4 +304,9 @@ extern "C" { int32_t getMAhDrawn(void) { return testmAhDrawn; } + + bool telemetryIsSensorEnabled(sensor_e) { + return true; + } + } diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index fe2cf8332..fda07ea6a 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -309,6 +309,7 @@ serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) {return NULL;} bool telemetryDetermineEnabledState(portSharing_e) {return true;} bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return true;} +bool telemetryIsSensorEnabled(sensor_e) {return true;} portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;} diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index 80454c703..ccab8072e 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -255,6 +255,11 @@ bool telemetryDetermineEnabledState(portSharing_e) return true; } +bool telemetryIsSensorEnabled(sensor_e sensor) { + UNUSED(sensor); + return true; +} + portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) { return PORTSHARING_NOT_SHARED; diff --git a/src/test/unit/telemetry_ibus_unittest.cc b/src/test/unit/telemetry_ibus_unittest.cc index e889fad8c..5cd3032bb 100644 --- a/src/test/unit/telemetry_ibus_unittest.cc +++ b/src/test/unit/telemetry_ibus_unittest.cc @@ -184,6 +184,12 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing) } +bool telemetryIsSensorEnabled(sensor_e sensor) { + UNUSED(sensor); + return true; +} + + bool isSerialPortShared(const serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction)