From ddcb104372309b543e9d7706548444cb8900beb5 Mon Sep 17 00:00:00 2001 From: 3djc Date: Sun, 2 Dec 2018 10:30:43 +0100 Subject: [PATCH] Create RSSI and VBAT sensor from pxx1.5 data stream --- radio/src/telemetry/frsky.cpp | 12 ++++++++++-- radio/src/telemetry/frsky_sport.cpp | 2 ++ 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/radio/src/telemetry/frsky.cpp b/radio/src/telemetry/frsky.cpp index 94349eaed..c863e2ee2 100644 --- a/radio/src/telemetry/frsky.cpp +++ b/radio/src/telemetry/frsky.cpp @@ -29,6 +29,14 @@ bool checkPXX2PacketCRC(const uint8_t * packet) // DUMP(packet, FRSKY_SPORT_PACKET_SIZE); } +void createFrSkyPXX2Sensor(uint16_t type, uint8_t value){ + uint8_t buffer[5] = {0x18, 0x10, 0, 0, 0}; + buffer[2] = type & 0xF; + buffer[3] = type >> 8; + buffer[4] = value; + sportProcessTelemetryPacketWithoutCrc(buffer); +} + void processFrskyPXX2Data(uint8_t data) { static uint8_t dataState = STATE_DATA_IDLE; @@ -52,10 +60,10 @@ void processFrskyPXX2Data(uint8_t data) if (telemetryRxBuffer[0] + 3 /* 1 byte for length, 2 bytes for CRC */ == telemetryRxBufferCount) { if (checkPXX2PacketCRC(telemetryRxBuffer)) { if (telemetryRxBuffer[2] & 0x80) { - telemetryData.rssi.set(telemetryRxBuffer[2] & 0x7f); + createFrSkyPXX2Sensor(RSSI_ID, telemetryRxBuffer[2] & 0x7f); } else { - // TODO : vbat handling + createFrSkyPXX2Sensor(BATT_ID, telemetryRxBuffer[2] & 0x7f); } sportProcessTelemetryPacketWithoutCrc(telemetryRxBuffer + 6 /* LEN, TYPE, RSSI/BAT, TP/SS/FW_T, FW_VER, Data ID */); } diff --git a/radio/src/telemetry/frsky_sport.cpp b/radio/src/telemetry/frsky_sport.cpp index a0b8fc33f..0b5224de5 100644 --- a/radio/src/telemetry/frsky_sport.cpp +++ b/radio/src/telemetry/frsky_sport.cpp @@ -154,6 +154,8 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet) uint16_t id = *((uint16_t *)(packet+2)); uint32_t data = SPORT_DATA_S32(packet); + TRACE("%x %x %x %x\n", physicalId, primId, id,SPORT_DATA_U8(packet) ); + if (primId == DATA_FRAME) { uint8_t instance = physicalId + 1; if (id == RSSI_ID && isValidIdAndInstance(RSSI_ID, instance)) {