diff --git a/radio/src/telemetry/frsky.cpp b/radio/src/telemetry/frsky.cpp index aad4cc96d..185f1dd1a 100644 --- a/radio/src/telemetry/frsky.cpp +++ b/radio/src/telemetry/frsky.cpp @@ -607,3 +607,24 @@ void telemetryInit(void) // we don't reset the telemetry here as we would also reset the consumption after model load } + + +#if defined(CPUARM) +void frskySetCellVoltage(uint8_t battnumber, uint16_t cellVolts) { +#elif defined(FRSKY_HUB) +void frskySetCellVoltage(uint8_t battnumber, uint8_t cellVolts) { +#endif + frskyData.hub.cellVolts[battnumber] = cellVolts; + if (!frskyData.hub.minCellVolts || cellVolts> 8) + ((frskyData.hub.volts & 0x000F) << 8))) / 10); #endif - frskyData.hub.cellVolts[battnumber] = cellVolts; - if (!frskyData.hub.minCellVolts || cellVolts> 8) + ((frskyData.hub.volts & 0x000F) << 8))) / 10); #endif - frskyData.hub.cellVolts[battnumber] = cellVolts; - if (!frskyData.hub.minCellVolts || cellVolts= CELLS_FIRST_ID && appId <= CELLS_LAST_ID) { uint32_t cells = SPORT_DATA_U32(packet); uint8_t battnumber = cells & 0xF; - uint32_t minCell, minCellNum; - - //TODO: Use reported total voltages (bits 4-7)? - frskyData.hub.cellVolts[battnumber] = ((cells & 0x000FFF00) >> 8) / 5; - frskyData.hub.cellVolts[battnumber+1] = ((cells & 0xFFF00000) >> 20) / 5; - - if (frskyData.hub.cellsCount < battnumber+2) - frskyData.hub.cellsCount = battnumber+2; - if (frskyData.hub.cellVolts[battnumber+1] == 0) - frskyData.hub.cellsCount--; - - if ((frskyData.hub.cellVolts[battnumber] < frskyData.hub.cellVolts[battnumber+1]) || (frskyData.hub.cellVolts[battnumber+1] == 0)) { - minCell = frskyData.hub.cellVolts[battnumber]; - minCellNum = battnumber; - } - else { - minCell = frskyData.hub.cellVolts[battnumber+1]; - minCellNum = battnumber+1; - } - - if (!frskyData.hub.minCellVolts || minCell < frskyData.hub.minCellVolts || minCellNum==frskyData.hub.minCellIdx) { - frskyData.hub.minCellIdx = minCellNum; - frskyData.hub.minCellVolts = minCell; - if (!frskyData.hub.minCell || frskyData.hub.minCellVolts> 8) / 5); + frskySetCellVoltage(battnumber+1, (uint16_t) ((cells & 0xFFF00000) >> 20) / 5); } break; }