diff --git a/radio/src/telemetry/frsky.h b/radio/src/telemetry/frsky.h index 67097dce4..b983710de 100644 --- a/radio/src/telemetry/frsky.h +++ b/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 { diff --git a/radio/src/telemetry/frsky_d.cpp b/radio/src/telemetry/frsky_d.cpp index bfe775577..d880cdac1 100644 --- a/radio/src/telemetry/frsky_d.cpp +++ b/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; diff --git a/radio/src/tests/frsky.cpp b/radio/src/tests/frsky.cpp index 899e1b188..0278c8cd1 100644 --- a/radio/src/tests/frsky.cpp +++ b/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)