Fixes #2685: D-type altitude fixes (negative values, hi/lo precision vario)

radio/src/telemetry/frsky.h

@@ -330,6 +330,7 @@ struct FrskyData {
   FrskyValueWithMin swr; // TODO Min not needed
   FrskyValueWithMin rssi; // TODO Min not needed
   uint16_t xjtVersion;
+  bool varioHighPrecision;
 };
 #else
 struct FrskyData {

radio/src/telemetry/frsky_d.cpp

@@ -521,7 +521,7 @@ const FrSkyDSensor frskyDSensors[] = {
   { ACCEL_Z_ID, ZSTR_ACCZ, UNIT_G, 3 },
   { VARIO_ID, ZSTR_VSPD, UNIT_METERS_PER_SECOND, 2 },
   { VFAS_ID, ZSTR_VFAS, UNIT_VOLTS, 2 },
-  { BARO_ALT_AP_ID, ZSTR_ALT, UNIT_METERS, 2 },
+  { BARO_ALT_AP_ID, ZSTR_ALT, UNIT_METERS, 1 },   // we map hi precision vario into PREC1!
   { VOLTS_AP_ID, ZSTR_VFAS, UNIT_VOLTS, 2 },
   { GPS_SPEED_BP_ID, ZSTR_GSPD, UNIT_KTS, 0 },
   { GPS_COURS_BP_ID, ZSTR_HDG, UNIT_DEGREE, 0 },
@@ -591,9 +591,13 @@ void processHubPacket(uint8_t id, int16_t value)
   }
   else if (id == BARO_ALT_AP_ID) {
     if (lastId == BARO_ALT_BP_ID) {
-      data += lastValue * 100;
+      if (data > 9 || frskyData.varioHighPrecision) {
+        frskyData.varioHighPrecision = true;
+        data /= 10;    // map hi precision vario into low precision. Altitude is stored in 0.1m anyways
+      }
+      data = (int16_t)lastValue * 10 + (((int16_t)lastValue < 0) ? -data : data);
       unit = UNIT_METERS;
-      precision = 2;
+      precision = 1;
     }
     else {
       return;

radio/src/tests/frsky.cpp

@@ -141,6 +141,39 @@ TEST(FrSky, Vfas_0x39_HiPrecision)
   processHubPacket(VFAS_ID, VFAS_D_HIPREC_OFFSET + 30012);  // set value of 300.12V
   EXPECT_EQ(telemetryItems[0].value, 30012);
 }
+
+TEST(FrSky, HubAltNegative)
+{
+  MODEL_RESET();
+  TELEMETRY_RESET();
+  EXPECT_EQ(telemetryItems[0].value, 0);
+
+  // low precision alitmeter, bp always less than 10
+  processHubPacket(BARO_ALT_BP_ID, 12);  // set value of 12.3m
+  processHubPacket(BARO_ALT_AP_ID, 3);
+  EXPECT_EQ(telemetryItems[0].value, 123);      // altitued stored has resolution of 0.1m
+
+  processHubPacket(BARO_ALT_BP_ID, -12);  // set value of -12.3m
+  processHubPacket(BARO_ALT_AP_ID, 3);
+  EXPECT_EQ(telemetryItems[0].value, -123);
+
+  // hi precision alitmeter, bp can be two decimals
+  MODEL_RESET();
+  TELEMETRY_RESET();
+  // first trigger hi precision, by setting AP above 9
+  processHubPacket(BARO_ALT_BP_ID, -1);  // set value of -1.35m
+  processHubPacket(BARO_ALT_AP_ID, 35);
+  EXPECT_EQ(telemetryItems[0].value, -13); 
+
+  processHubPacket(BARO_ALT_BP_ID, 12);  // set value of 12.35m
+  processHubPacket(BARO_ALT_AP_ID, 35);
+  EXPECT_EQ(telemetryItems[0].value, 123); 
+
+  // now test with the AP less than 10 to check if hiprecision is still active
+  processHubPacket(BARO_ALT_BP_ID, 12);  // set value of 12.05m
+  processHubPacket(BARO_ALT_AP_ID, 05);
+  EXPECT_EQ(telemetryItems[0].value, 120); 
+}
 #endif  // #if defined(FRSKY) && defined(CPUARM)
 
 #if defined(FRSKY_SPORT)