Add a few more telemetry sensors to SmartPort (rotorflight#172)

* Add model id, PID and Rates Profile sensors * Add BEC voltage with unused ADC3 sensor * Use telemetrySensorValue for the new sensors
bob01 · Oct 2, 2024 · b7ae36c · b7ae36c
1 parent 014bac5
commit b7ae36c
Show file tree

Hide file tree

Showing 3 changed files with 50 additions and 5 deletions.
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
@@ -1241,6 +1241,10 @@ const clivalue_t valueTable[] = {
     { "telemetry_enable_cap_used",        VAR_UINT32  | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_CAP_USED),        PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
     { "telemetry_enable_adjustment",      VAR_UINT32  | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_ADJUSTMENT),      PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
     { "telemetry_enable_gov_mode",        VAR_UINT32  | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_GOV_MODE),        PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
+    { "telemetry_enable_model_id",        VAR_UINT32  | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_MODEL_ID),        PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
+    { "telemetry_enable_pid_profile",     VAR_UINT32  | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_PID_PROFILE),     PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
+    { "telemetry_enable_rates_profile",   VAR_UINT32  | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_RATES_PROFILE),   PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
+    { "telemetry_enable_bec_voltage",     VAR_UINT32  | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_BEC_VOLTAGE),     PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
 #else
     { "telemetry_enable_sensors",         VAR_UINT32 | MASTER_VALUE, .config.u32Max = SENSOR_ALL, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
 #endif

diff --git a/src/main/pg/telemetry.h b/src/main/pg/telemetry.h
@@ -48,6 +48,10 @@ typedef enum {
     SENSOR_CAP_USED        = BIT(20),
     SENSOR_ADJUSTMENT      = BIT(21),
     SENSOR_GOV_MODE        = BIT(22),
+    SENSOR_MODEL_ID        = BIT(23),
+    SENSOR_PID_PROFILE     = BIT(24),
+    SENSOR_RATES_PROFILE   = BIT(25),
+    SENSOR_BEC_VOLTAGE     = BIT(26),
 } sensor_e;
 
 typedef enum {

diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
@@ -132,7 +132,10 @@ enum
     FSSP_DATAID_ADJFUNC    = 0x5110 , // custom
     FSSP_DATAID_ADJVALUE   = 0x5111 , // custom
     FSSP_DATAID_CAP_USED   = 0x5250 ,
-    FSSP_DATAID_GOV_MODE   = 0x5450 , //custom
+    FSSP_DATAID_GOV_MODE   = 0x5450 , // custom
+    FSSP_DATAID_MODEL_ID   = 0x5460 , // custom
+    FSSP_DATAID_PID_PROFILE   = 0x5471 , // custom
+    FSSP_DATAID_RATES_PROFILE = 0x5472 , // custom
 #if defined(USE_ACC)
     FSSP_DATAID_PITCH      = 0x5230 , // custom
     FSSP_DATAID_ROLL       = 0x5240 , // custom
@@ -160,7 +163,7 @@ enum
 };
 
 // if adding more sensors then increase this value (should be equal to the maximum number of ADD_SENSOR calls)
-#define MAX_DATAIDS 25
+#define MAX_DATAIDS 29
 
 static uint16_t frSkyDataIdTable[MAX_DATAIDS];
 
@@ -336,8 +339,24 @@ static void initSmartPortSensors(void)
 
     //prob need configurator option for these?
     if (telemetryIsSensorEnabled(SENSOR_GOV_MODE)) {
-    ADD_SENSOR(FSSP_DATAID_GOV_MODE);
-    }    
+        ADD_SENSOR(FSSP_DATAID_GOV_MODE);
+    }
+
+    if (telemetryIsSensorEnabled(SENSOR_MODEL_ID)) {
+        ADD_SENSOR(FSSP_DATAID_MODEL_ID);
+    }
+
+    if (telemetryIsSensorEnabled(SENSOR_PID_PROFILE)) {
+        ADD_SENSOR(FSSP_DATAID_PID_PROFILE);
+    }
+
+    if (telemetryIsSensorEnabled(SENSOR_RATES_PROFILE)) {
+        ADD_SENSOR(FSSP_DATAID_RATES_PROFILE);
+    }
+
+    if (telemetryIsSensorEnabled(SENSOR_BEC_VOLTAGE)) {
+        ADD_SENSOR(FSSP_DATAID_A3);
+    }
 
     if (telemetryIsSensorEnabled(SENSOR_MODE)) {
         ADD_SENSOR(FSSP_DATAID_T1);
@@ -662,7 +681,19 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
                     smartPortSendPackage(id, getGovernorState());
                 }
                 *clearToSend = false;
-                break;           
+                break;
+            case FSSP_DATAID_MODEL_ID   :
+                smartPortSendPackage(id, telemetrySensorValue(TELEM_MODEL_ID));
+                *clearToSend = false;
+                break;
+            case FSSP_DATAID_PID_PROFILE :
+                smartPortSendPackage(id, telemetrySensorValue(TELEM_PID_PROFILE));
+                *clearToSend = false;
+                break;
+            case FSSP_DATAID_RATES_PROFILE :
+                smartPortSendPackage(id, telemetrySensorValue(TELEM_RATES_PROFILE));
+                *clearToSend = false;
+                break;
             case FSSP_DATAID_VFAS       :
                 vfasVoltage = telemetryConfig()->report_cell_voltage ? getBatteryAverageCellVoltage() : getBatteryVoltage();
                 smartPortSendPackage(id, vfasVoltage); // in 0.01V according to SmartPort spec
@@ -925,6 +956,12 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
                 }
                 break;
 #endif
+            case FSSP_DATAID_A3         :
+                vfasVoltage = telemetrySensorValue(TELEM_BEC_VOLTAGE); // in 0.01V according to SmartPort spec
+                smartPortSendPackage(id, vfasVoltage);
+                *clearToSend = false;
+                break;
+
             case FSSP_DATAID_A4         :
                 vfasVoltage = getBatteryAverageCellVoltage(); // in 0.01V according to SmartPort spec
                 smartPortSendPackage(id, vfasVoltage);