Merge pull request #477 from projectkk2glider/projectkk2glider/issue_316_frsky_voltages

Fixes #316: better arithmetics for FrSky CELL voltages, VFAS battery vol...

docs/Telemetry.md

@@ -47,10 +47,25 @@ For 2 and 3 use the CLI command as follows:
 set telemetry_inversion = 1
 ```
 
+### Precision setting for VFAS
+
+Cleanflight can send VFAS (FrSky Ampere Sensor Voltage) in two ways:
+
+```
+set frsky_hiprec_vfas  = 0
+```
+This is default setting which supports VFAS resolution of 0.2 volts and is supported on all FrSky hardware.
+
+```
+set frsky_hiprec_vfas  = 1
+```
+This is new setting which supports VFAS resolution of 0.1 volts and is only supported by OpenTX radios (this method uses custom ID 0x39).
+
+
 ### Notes
 
 RPM shows throttle output when armed.
-RPM shows when diarmed.
+RPM shows when disarmed.
 TEMP2 shows Satellite Signal Quality when GPS is enabled.
 
 RPM requires that the 'blades' setting is set to 12 on your receiver/display - tested with Taranis/OpenTX.

src/main/config/config.c

@@ -226,6 +226,7 @@ void resetTelemetryConfig(telemetryConfig_t *telemetryConfig)
     telemetryConfig->gpsNoFixLongitude = 0;
     telemetryConfig->frsky_coordinate_format = FRSKY_FORMAT_DMS;
     telemetryConfig->frsky_unit = FRSKY_UNIT_METRICS;
+    telemetryConfig->frsky_hiprec_vfas = 0;
 }
 
 void resetBatteryConfig(batteryConfig_t *batteryConfig)

src/main/io/serial_cli.c

@@ -346,6 +346,7 @@ const clivalue_t valueTable[] = {
     { "frsky_default_longitude",    VAR_FLOAT  | MASTER_VALUE,  &masterConfig.telemetryConfig.gpsNoFixLongitude, -180.0, 180.0 },
     { "frsky_coordinates_format",   VAR_UINT8  | MASTER_VALUE,  &masterConfig.telemetryConfig.frsky_coordinate_format, 0, FRSKY_FORMAT_NMEA },
     { "frsky_unit",                 VAR_UINT8  | MASTER_VALUE,  &masterConfig.telemetryConfig.frsky_unit, 0, FRSKY_UNIT_IMPERIALS },
+    { "frsky_hiprec_vfas",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.telemetryConfig.frsky_hiprec_vfas, 0, 1 },
 
     { "battery_capacity",           VAR_UINT16 | MASTER_VALUE,  &masterConfig.batteryConfig.batteryCapacity, 0, 20000 },
     { "vbat_scale",                 VAR_UINT8  | MASTER_VALUE,  &masterConfig.batteryConfig.vbatscale, VBAT_SCALE_MIN, VBAT_SCALE_MAX },

src/main/sensors/battery.c

@@ -48,7 +48,7 @@ uint16_t batteryAdcToVoltage(uint16_t src)
 {
     // calculate battery voltage based on ADC reading
     // result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 110 = 11:1 voltage divider (10k:1k) * 10 for 0.1V
-    return (((src) * 3.3f) / 0xFFF) * batteryConfig->vbatscale;
+    return ((uint32_t)src * batteryConfig->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * 10);
 }
 
 #define BATTERY_SAMPLE_COUNT 8

src/main/telemetry/frsky.c

@@ -109,6 +109,7 @@ extern int16_t telemTemperature1; // FIXME dependency on mw.c
 #define ID_ACC_X              0x24
 #define ID_ACC_Y              0x25
 #define ID_ACC_Z              0x26
+#define ID_VOLTAGE_AMP        0x39
 #define ID_VOLTAGE_AMP_BP     0x3A
 #define ID_VOLTAGE_AMP_AP     0x3B
 #define ID_CURRENT            0x28
@@ -334,7 +335,6 @@ static void sendVario(void)
 static void sendVoltage(void)
 {
     static uint16_t currentCell = 0;
-    uint16_t cellNumber;
     uint32_t cellVoltage;
     uint16_t payload;
 
@@ -345,16 +345,14 @@ static void sendVoltage(void)
      *  l: Low voltage bits
      *  h: High voltage bits
      *  c: Cell number (starting at 0)
+     *
+     * The actual value sent for cell voltage has resolution of 0.002 volts
+     * Since vbat has resolution of 0.1 volts it has to be multiplied by 50
      */
-    cellVoltage = vbat / batteryCellCount;
+    cellVoltage = ((uint32_t)vbat * 100 + batteryCellCount) / (batteryCellCount * 2);
-
-    // Map to 12 bit range
-    cellVoltage = (cellVoltage * 2100) / 42;
-
-    cellNumber = currentCell % batteryCellCount;
 
     // Cell number is at bit 9-12
-    payload = (cellNumber << 4);
+    payload = (currentCell << 4);
 
     // Lower voltage bits are at bit 0-8
     payload |= ((cellVoltage & 0x0ff) << 8);
@@ -374,6 +372,13 @@ static void sendVoltage(void)
  */
 static void sendVoltageAmp(void)
 {
+    if (telemetryConfig->frsky_hiprec_vfas) {
+        /*
+         * Use new ID 0x39 to send voltage directly in 0.1 volts resolution
+         */
+        sendDataHead(ID_VOLTAGE_AMP);
+        serialize16(vbat);
+    } else {
         uint16_t voltage = (vbat * 110) / 21;
 
         sendDataHead(ID_VOLTAGE_AMP_BP);
@@ -381,6 +386,7 @@ static void sendVoltageAmp(void)
         sendDataHead(ID_VOLTAGE_AMP_AP);
         serialize16(((voltage % 100) + 5) / 10);
     }
+}
 
 static void sendAmperage(void)
 {

src/main/telemetry/telemetry.h

@@ -42,6 +42,7 @@ typedef struct telemetryConfig_s {
     float gpsNoFixLongitude;  
     frskyGpsCoordFormat_e frsky_coordinate_format;   
     frskyUnit_e frsky_unit;
+    uint8_t frsky_hiprec_vfas;
 } telemetryConfig_t;
 
 void checkTelemetryState(void);

src/test/unit/battery_unittest.cc

@@ -53,14 +53,14 @@ TEST(BatteryTest, BatteryADCToVoltage)
     batteryInit(&batteryConfig);
 
     batteryAdcToVoltageExpectation_t batteryAdcToVoltageExpectations[] = {
-            {1420, 125, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
+            {1420, 126 /*125.88*/, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
-            {1430, 126, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
+            {1430, 127 /*126.76*/, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
-            {1440, 127, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
+            {1440, 128 /*127.65*/, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
-            {1890, 167, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
+            {1890, 168 /*167.54*/, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
-            {1900, 168, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
+            {1900, 168 /*168.42*/, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
-            {1910, 169, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
+            {1910, 169 /*169.31*/, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
-            {   0,   0, VBAT_SCALE_MAX},
+            {   0,   0 /*  0.00*/, VBAT_SCALE_MAX},
-            {4096, 841, VBAT_SCALE_MAX}
+            {4096, 842 /*841.71*/, VBAT_SCALE_MAX}
     };
     uint8_t testIterationCount = sizeof(batteryAdcToVoltageExpectations) / sizeof(batteryAdcToVoltageExpectation_t);
 
@@ -77,7 +77,7 @@ TEST(BatteryTest, BatteryADCToVoltage)
 #endif
         uint16_t pointOneVoltSteps = batteryAdcToVoltage(batteryAdcToVoltageExpectation->adcReading);
 
-        EXPECT_EQ(pointOneVoltSteps, batteryAdcToVoltageExpectation->expectedVoltageInDeciVoltSteps);
+        EXPECT_EQ(batteryAdcToVoltageExpectation->expectedVoltageInDeciVoltSteps, pointOneVoltSteps);
     }
 }