Merge pull request #11045 from functionpointer/smartport_batt_sensor

New feature: SmartPort battery sensor

Author: sensei-hacker

docs/Settings.md

@@ -634,7 +634,7 @@ This sets the output voltage to current scaling for the current sensor in 0.1 mV
 
 ### current_meter_type
 
-ADC , VIRTUAL, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position.
+ADC, VIRTUAL, FAKE, ESC, SMARTPORT, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position.
 
 | Default | Min | Max |
 | --- | --- | --- |
@@ -6514,7 +6514,7 @@ Maximum voltage per cell in 0.01V units, default is 4.20V
 
 ### vbat_meter_type
 
-Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running
+Vbat voltage source. Possible values: `NONE`, `ADC`, `SMARTPORT`, `ESC`. `ESC` requires ESC telemetry enabled and running. `SMARTPORT` requires SmartPort Master enabled and running.
 
 | Default | Min | Max |
 | --- | --- | --- |

src/main/fc/settings.yaml

@@ -33,10 +33,10 @@ tables:
   - name: failsafe_procedure
     values: ["LAND", "DROP", "RTH", "NONE"]
   - name: current_sensor
-    values: ["NONE", "ADC", "VIRTUAL", "FAKE", "ESC"]
+    values: ["NONE", "ADC", "VIRTUAL", "FAKE", "ESC", "SMARTPORT"]
     enum: currentSensor_e
   - name: voltage_sensor
-    values: ["NONE", "ADC", "ESC", "FAKE"]
+    values: ["NONE", "ADC", "ESC", "FAKE", "SMARTPORT"]
     enum: voltageSensor_e
   - name: imu_inertia_comp_method
     values: ["VELNED", "TURNRATE","ADAPTIVE"]
@@ -976,7 +976,7 @@ groups:
     headers: ["sensors/battery_config_structs.h"]
     members:
       - name: vbat_meter_type
-        description: "Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running"
+        description: "Vbat voltage source. Possible values: `NONE`, `ADC`, `SMARTPORT`, `ESC`. `ESC` requires ESC telemetry enabled and running. `SMARTPORT` requires SmartPort Master enabled and running."
         condition: USE_ADC
         default_value: ADC
         field: voltage.type
@@ -1008,7 +1008,7 @@ groups:
         min: -32768
         max: 32767
       - name: current_meter_type
-        description: "ADC , VIRTUAL, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position."
+        description: "ADC, VIRTUAL, FAKE, ESC, SMARTPORT, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position."
         default_value: "ADC"
         field: current.type
         table: current_sensor

src/main/io/smartport_master.c

@@ -98,7 +98,7 @@ enum
 #define SMARTPORT_BYTESTUFFING_MARKER 0x7D
 #define SMARTPORT_BYTESTUFFING_XOR_VALUE 0x20
 
-#define SMARTPORT_SENSOR_DATA_TIMEOUT 500 // ms
+#define SMARTPORT_SENSOR_DATA_TIMEOUT (500*1000) // us
 
 #define SMARTPORT_FORWARD_REQUESTS_MAX 10
 
@@ -128,6 +128,10 @@ typedef struct {
     timeUs_t altitudeTimestamp;
     int16_t vario;
     timeUs_t varioTimestamp;
+    int16_t current;
+    timeUs_t currentTimestamp;
+    int16_t voltage;
+    timeUs_t voltageTimestamp;
 } smartportSensorsData_t;
 
 typedef struct {
@@ -378,6 +382,17 @@ static void decodeVarioData(uint32_t sdata)
     sensorsData.vario = sdata * 2; // mm/s
 }
 
+static void decodeCurrentData(uint32_t sdata)
+{
+    // data comes in 100mA steps
+    sensorsData.current = ((int16_t)sdata) * 10;
+}
+
+static void decodeVoltageData(uint32_t sdata)
+{
+    sensorsData.voltage = (int16_t)sdata;
+}
+
 static void processSensorPayload(smartPortPayload_t *payload, timeUs_t currentTimeUs)
 {
     switch (payload->valueId) {
@@ -400,6 +415,16 @@ static void processSensorPayload(smartPortPayload_t *payload, timeUs_t currentTi
             decodeVarioData(payload->data);
             sensorsData.varioTimestamp = currentTimeUs;
             break;
+
+        case DATAID_CURRENT:
+            decodeCurrentData(payload->data);
+            sensorsData.currentTimestamp = currentTimeUs;
+            break;
+
+        case DATAID_VFAS:
+            decodeVoltageData(payload->data);
+            sensorsData.voltageTimestamp = currentTimeUs;
+            break;
     }
     sensorPayloadCache[currentPolledPhyID] = *payload;
 }
@@ -568,6 +593,24 @@ vs600Data_t *smartportMasterGetVS600Data(void)
     return &sensorsData.vs600;
 }
 
+int16_t *smartportMasterGetCurrentData(void)
+{
+    if (micros() - sensorsData.currentTimestamp > SMARTPORT_SENSOR_DATA_TIMEOUT) {
+        return NULL;
+    }
+
+    return &sensorsData.current;
+}
+
+int16_t *smartportMasterGetVoltageData(void)
+{
+    if (micros() - sensorsData.voltageTimestamp > SMARTPORT_SENSOR_DATA_TIMEOUT) {
+        return NULL;
+    }
+
+    return &sensorsData.voltage;
+}
+
 bool smartportMasterPhyIDIsActive(uint8_t phyID)
 {
     return phyIDIsActive(phyID);

src/main/io/smartport_master.h

@@ -43,6 +43,11 @@ typedef struct {
     int16_t voltage[6];
 } cellsData_t;
 
+typedef struct {
+    int16_t amperage;
+    int16_t vfas;
+} batteryData_t;
+
 typedef enum {
     VS600_BAND_A,
     VS600_BAND_B,
@@ -83,5 +88,7 @@ bool smartportMasterNextForwardResponse(uint8_t phyID, smartPortPayload_t *paylo
 // Returns latest Cells data or NULL if the data is too old
 cellsData_t *smartportMasterGetCellsData(void);
 vs600Data_t *smartportMasterGetVS600Data(void);
+int16_t *smartportMasterGetCurrentData(void);
+int16_t *smartportMasterGetVoltageData(void);
 
 #endif /* USE_SMARTPORT_MASTER */

src/main/sensors/battery.c

@@ -62,6 +62,9 @@
 #if defined(USE_FAKE_BATT_SENSOR)
 #include "sensors/battery_sensor_fake.h"
 #endif
+#if defined(USE_SMARTPORT_MASTER)
+#include "io/smartport_master.h"
+#endif
 
 #define ADCVREF 3300                            // in mV (3300 = 3.3V)
 
@@ -291,6 +294,17 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected)
         vbat = fakeBattSensorGetVBat();
         break;
 #endif
+
+#if defined(USE_SMARTPORT_MASTER)
+    case VOLTAGE_SENSOR_SMARTPORT:
+        int16_t * smartportVoltageData = smartportMasterGetVoltageData();
+        if (smartportVoltageData) {
+            vbat = *smartportVoltageData;
+        } else {
+            vbat = 0;
+        }
+        break;
+#endif
     case VOLTAGE_SENSOR_NONE:
         default:
             vbat = 0;
@@ -598,7 +612,16 @@ void currentMeterUpdate(timeUs_t timeDelta)
             }
             break;
 #endif
-
+#if defined(USE_SMARTPORT_MASTER)
+        case CURRENT_SENSOR_SMARTPORT:
+            int16_t * smartportCurrentData = smartportMasterGetCurrentData();
+            if (smartportCurrentData) {
+                amperage = *smartportCurrentData;
+            } else {
+                amperage = 0;
+            }
+            break;
+#endif
 #if defined(USE_FAKE_BATT_SENSOR)
         case CURRENT_SENSOR_FAKE:
             amperage = fakeBattSensorGetAmerperage();

src/main/sensors/battery_config_structs.h

@@ -31,14 +31,17 @@ typedef enum {
     CURRENT_SENSOR_VIRTUAL,
     CURRENT_SENSOR_FAKE,
     CURRENT_SENSOR_ESC,
-    CURRENT_SENSOR_MAX = CURRENT_SENSOR_FAKE
+    CURRENT_SENSOR_SMARTPORT,
+    CURRENT_SENSOR_MAX = CURRENT_SENSOR_SMARTPORT
 } currentSensor_e;
 
 typedef enum {
     VOLTAGE_SENSOR_NONE = 0,
     VOLTAGE_SENSOR_ADC,
     VOLTAGE_SENSOR_ESC,
-    VOLTAGE_SENSOR_FAKE
+    VOLTAGE_SENSOR_FAKE,
+    VOLTAGE_SENSOR_SMARTPORT,
+    VOLTAGE_SENSOR_MAX = VOLTAGE_SENSOR_SMARTPORT
 } voltageSensor_e;
 
 typedef enum {