diff --git a/radio/src/telemetry/frsky_d.cpp b/radio/src/telemetry/frsky_d.cpp index 0ddcd133e..9f1ed8e3a 100644 --- a/radio/src/telemetry/frsky_d.cpp +++ b/radio/src/telemetry/frsky_d.cpp @@ -600,6 +600,8 @@ void processHubPacket(uint8_t id, int16_t value) } else if (id == VOLTS_ID) { unit = UNIT_CELLS; + uint32_t cellData = (uint32_t)data; + data = ((cellData & 0x00F0) << 12) + (((((cellData & 0xFF00) >> 8) + ((cellData & 0x000F) << 8))) / 5); } else if (id == GPS_DAY_MONTH_ID) { id = GPS_HOUR_MIN_ID; @@ -636,8 +638,6 @@ void frskyDSetDefault(int index, uint16_t id) const FrSkyDSensor * sensor = getFrSkyDSensor(id); if (sensor) { TelemetryUnit unit = sensor->unit; - if (unit == UNIT_CELLS) - unit = UNIT_VOLTS; uint8_t prec = min(2, sensor->prec); telemetrySensor.init(sensor->name, unit, prec); if (id >= D_A1_ID && id <= D_A2_ID) { diff --git a/radio/src/telemetry/frsky_sport.cpp b/radio/src/telemetry/frsky_sport.cpp index 6fe24f7f0..5a5a35a20 100644 --- a/radio/src/telemetry/frsky_sport.cpp +++ b/radio/src/telemetry/frsky_sport.cpp @@ -236,7 +236,19 @@ void processSportPacket(uint8_t *packet) unit = sensor->unit; precision = sensor->prec; } - setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, appId, dataId, data, unit, precision); + if (unit == UNIT_CELLS) { + uint8_t cellsCount = (data & 0xF0) >> 4; + uint8_t cellIndex = (data & 0x0F); + uint32_t mask = (cellsCount << 24) + (cellIndex << 16); + setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, appId, dataId, mask + (((data & 0x000FFF00) >> 8) / 5), unit, precision); + if (cellIndex+1 < cellsCount) { + mask += (1 << 16); + setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, appId, dataId, mask + (((data & 0xFFF00000) >> 20) / 5), unit, precision); + } + } + else { + setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, appId, dataId, data, unit, precision); + } } } } diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index 11eecb1d2..d8b3dca97 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -20,19 +20,22 @@ void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32 if (unit == UNIT_CELLS) { uint32_t data = uint32_t(newVal); - uint8_t cellIndex = data & 0xF; - uint8_t count = (data & 0xF0) >> 4; - if (count != cells.count) { + uint8_t cellsCount = (data >> 24); + uint8_t cellIndex = ((data >> 16) & 0x0F); + uint16_t cellValue = (data & 0xFFFF); + if (cellsCount == 0) { + cellsCount = (cellIndex >= cells.count ? cellIndex + 1 : cells.count); + } + if (cellsCount != cells.count) { clear(); - cells.count = count; + cells.count = cellsCount; + // we skip this round + return; } - cells.values[cellIndex].set(((data & 0x000FFF00) >> 8) / 5); - if (cellIndex+1 < cells.count) { - cells.values[cellIndex+1].set(((data & 0xFFF00000) >> 20) / 5); - } - if (cellIndex+2 >= cells.count) { + cells.values[cellIndex].set(cellValue); + if (cellIndex+1 == cells.count) { newVal = 0; - for (int i=0; i