diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index f6fad145c..133cf086d 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -948,15 +948,13 @@ const clivalue_t valueTable[] = { #if defined(USE_TELEMETRY_IBUS) { "ibus_sensor", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = IBUS_SENSOR_COUNT, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, flysky_sensors)}, #endif -#if defined(USE_TELEMETRY_SMARTPORT) - { "smartport_use_extra_sensors", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, smartport_use_extra_sensors)}, -#endif #ifdef USE_TELEMETRY_MAVLINK // Support for misusing the heading field in MAVlink to indicate mAh drawn for Connex Prosight OSD // Set to 10 to show a tenth of your capacity drawn. // 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)}, #endif // USE_TELEMETRY // PG_LED_STRIP_CONFIG diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index b8895ec97..54376cd40 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -153,12 +153,10 @@ static uint16_t frSkyDataIdTable[MAX_DATAIDS]; // number of sensors to send between sending the ESC sensors #define ESC_SENSOR_PERIOD 7 -static uint16_t frSkyEscDataIdTable[] = { - FSSP_DATAID_CURRENT , - FSSP_DATAID_RPM , - FSSP_DATAID_VFAS , - FSSP_DATAID_TEMP -}; +// if adding more esc sensors then increase this value +#define MAX_ESC_DATAIDS 4 + +static uint16_t frSkyEscDataIdTable[MAX_ESC_DATAIDS]; #endif typedef struct frSkyTableInfo_s { @@ -169,9 +167,7 @@ typedef struct frSkyTableInfo_s { static frSkyTableInfo_t frSkyDataIdTableInfo = { frSkyDataIdTable, 0, 0 }; #ifdef USE_ESC_SENSOR_TELEMETRY -#define ESC_DATAID_COUNT ( sizeof(frSkyEscDataIdTable) / sizeof(uint16_t) ) - -static frSkyTableInfo_t frSkyEscDataIdTableInfo = {frSkyEscDataIdTable, ESC_DATAID_COUNT, 0}; +static frSkyTableInfo_t frSkyEscDataIdTableInfo = {frSkyEscDataIdTable, 0, 0}; #endif #define SMARTPORT_BAUD 57600 @@ -317,13 +313,8 @@ static void smartPortSendPackage(uint16_t id, uint32_t val) smartPortWriteFrame(&payload); } -#ifdef USE_ESC_SENSOR_TELEMETRY -static bool reportExtendedEscSensors(void) { - return featureIsEnabled(FEATURE_ESC_SENSOR) && telemetryConfig()->smartport_use_extra_sensors; -} -#endif - #define ADD_SENSOR(dataId) frSkyDataIdTableInfo.table[frSkyDataIdTableInfo.index++] = dataId +#define ADD_ESC_SENSOR(dataId) frSkyEscDataIdTableInfo.table[frSkyEscDataIdTableInfo.index++] = dataId static void initSmartPortSensors(void) { @@ -334,7 +325,7 @@ static void initSmartPortSensors(void) if (isBatteryVoltageConfigured()) { #ifdef USE_ESC_SENSOR_TELEMETRY - if (!reportExtendedEscSensors()) + if (!telemetryIsSensorEnabled(ESC_SENSOR_VOLTAGE)) #endif { ADD_SENSOR(FSSP_DATAID_VFAS); @@ -345,7 +336,7 @@ static void initSmartPortSensors(void) if (isAmperageConfigured()) { #ifdef USE_ESC_SENSOR_TELEMETRY - if (!reportExtendedEscSensors()) + if (!telemetryIsSensorEnabled(ESC_SENSOR_CURRENT)) #endif { ADD_SENSOR(FSSP_DATAID_CURRENT); @@ -380,11 +371,23 @@ static void initSmartPortSensors(void) frSkyDataIdTableInfo.index = 0; #ifdef USE_ESC_SENSOR_TELEMETRY - if (reportExtendedEscSensors()) { - frSkyEscDataIdTableInfo.size = ESC_DATAID_COUNT; - } else { - frSkyEscDataIdTableInfo.size = 0; + frSkyEscDataIdTableInfo.index = 0; + + if (telemetryIsSensorEnabled(ESC_SENSOR_VOLTAGE)) { + ADD_ESC_SENSOR(FSSP_DATAID_VFAS); } + if (telemetryIsSensorEnabled(ESC_SENSOR_CURRENT)) { + ADD_ESC_SENSOR(FSSP_DATAID_CURRENT); + } + if (telemetryIsSensorEnabled(ESC_SENSOR_RPM)) { + ADD_ESC_SENSOR(FSSP_DATAID_RPM); + } + if (telemetryIsSensorEnabled(ESC_SENSOR_TEMPERATURE)) { + ADD_ESC_SENSOR(FSSP_DATAID_TEMP); + } + + frSkyEscDataIdTableInfo.size = frSkyEscDataIdTableInfo.index; + frSkyEscDataIdTableInfo.index = 0; #endif } diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index fce7bc811..cae9bae28 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -58,7 +58,7 @@ #include "telemetry/ibus.h" #include "telemetry/msp_shared.h" -PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 3); PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .telemetry_inverted = false, @@ -76,7 +76,7 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, IBUS_SENSOR_TYPE_RPM_FLYSKY, IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE }, - .smartport_use_extra_sensors = false, + .disabledSensors = ESC_SENSOR_ALL, .mavlink_mah_as_heading_divisor = 0, ); @@ -226,3 +226,8 @@ void telemetryProcess(uint32_t currentTime) #endif } #endif + +bool telemetryIsSensorEnabled(sensor_e sensor) +{ + return ~(telemetryConfigMutable()->disabledSensors) & sensor; +} diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index dd58cb7a1..7a2bf01c0 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -41,6 +41,17 @@ typedef enum { FRSKY_UNIT_IMPERIALS } 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_e; + typedef struct telemetryConfig_s { int16_t gpsNoFixLatitude; int16_t gpsNoFixLongitude; @@ -53,8 +64,8 @@ typedef struct telemetryConfig_s { uint8_t pidValuesAsTelemetry; uint8_t report_cell_voltage; uint8_t flysky_sensors[IBUS_SENSOR_COUNT]; - uint8_t smartport_use_extra_sensors; uint16_t mavlink_mah_as_heading_divisor; + uint32_t disabledSensors; // bit flags } telemetryConfig_t; PG_DECLARE(telemetryConfig_t, telemetryConfig); @@ -68,3 +79,5 @@ void telemetryCheckState(void); void telemetryProcess(uint32_t currentTime); bool telemetryDetermineEnabledState(portSharing_e portSharing); + +bool telemetryIsSensorEnabled(sensor_e sensor);