diff --git a/radio/sdcard/FrSky-utilities/FrSky SBEC.lua b/radio/sdcard/FrSky-utilities/FrSky SBEC.lua index 473b6569b..d6c051951 100644 --- a/radio/sdcard/FrSky-utilities/FrSky SBEC.lua +++ b/radio/sdcard/FrSky-utilities/FrSky SBEC.lua @@ -55,9 +55,9 @@ end local settingsFields = { {"SBEC OUTPUT (V)", VALUE, 0x80, nil, 50, 84 }, - --{"Physical ID", VALUE, 0x01, nil, 0, 26 }, - --{"Application IDgroup", VALUE, 0x0D, nil, 0, 15 }, - --{"Data rate(*100ms)", VALUE, 0x22, nil, 1, 255 }, + {"Physical ID", VALUE, 0x01, nil, 0, 26 }, + {"Application IDgroup", VALUE, 0x0D, nil, 0, 15 }, + {"Data rate(*100ms)", VALUE, 0x22, nil, 1, 255 }, } -- Change display attribute to current field @@ -156,19 +156,19 @@ local function redrawFieldsPage() end local function telemetryRead(fieldx) - return sportTelemetryPush(0x08, 0x30, 0x0e50, fieldx) + return sportTelemetryPush(0, 0x30, 0x0e50, fieldx) end local function telemetryIdle(field) - return sportTelemetryPush(0x08,0x21,0x0e50,field) + return sportTelemetryPush(0, 0x21, 0x0e50, field) end local function telemetryUnIdle(field) - return sportTelemetryPush(0x08,0x20,0x0e50,field) + return sportTelemetryPush(0, 0x20, 0x0e50, field) end local function telemetryWrite(fieldx, valuex) - return sportTelemetryPush(0x08, 0x31, 0x0e50, fieldx + valuex*256) + return sportTelemetryPush(0, 0x31, 0x0e50, fieldx + valuex*256) end local telemetryPopTimeout = 0 @@ -188,7 +188,7 @@ local function refreshNext() end elseif refreshState == 1 then local physicalId, primId, dataId, value = sportTelemetryPop() - if primId == 0x32 and dataId >= 0x0e50 and dataId <= 0x0e5f then + if primId == 0x32 and dataId >= 0x0e50 and dataId <= 0x0e5f then local fieldId = value % 256 local field = fields[refreshIndex + 1] if fieldId == field[3] then diff --git a/radio/src/io/frsky_sport.cpp b/radio/src/io/frsky_sport.cpp index adf249382..6dd6e9c77 100644 --- a/radio/src/io/frsky_sport.cpp +++ b/radio/src/io/frsky_sport.cpp @@ -20,12 +20,9 @@ #include "opentx.h" -bool isTelemetryOutputBufferAvailable() -{ - return (outputTelemetryBuffer.size == 0 && outputTelemetryBuffer.trigger == 0x7E); -} +#warning "This need to be adapted" -void sportOutputPushByte(uint8_t byte) +/*void sportOutputPushByte(uint8_t byte) { if (byte == 0x7E || byte == 0x7D) { outputTelemetryBuffer.push(0x7D); @@ -52,3 +49,4 @@ void sportOutputPushPacket(SportTelemetryPacket * packet) outputTelemetryBuffer.setTrigger(packet->raw[0]); // physicalId outputTelemetryBuffer.setDestination(SPORT_MODULE); } +*/ \ No newline at end of file diff --git a/radio/src/io/frsky_sport.h b/radio/src/io/frsky_sport.h index 57a7dabf7..dd648a811 100644 --- a/radio/src/io/frsky_sport.h +++ b/radio/src/io/frsky_sport.h @@ -34,9 +34,6 @@ PACK(union SportTelemetryPacket }; uint8_t raw[8]; }); - -bool isTelemetryOutputBufferAvailable(); -void sportOutputPushPacket(SportTelemetryPacket * packet); #endif #if defined(STM32) diff --git a/radio/src/lua/api_general.cpp b/radio/src/lua/api_general.cpp index 0ab38311d..3765157aa 100644 --- a/radio/src/lua/api_general.cpp +++ b/radio/src/lua/api_general.cpp @@ -422,15 +422,14 @@ When called without parameters, it will only return the status of the output buf static int luaSportTelemetryPush(lua_State * L) { if (lua_gettop(L) == 0) { - lua_pushboolean(L, isTelemetryOutputBufferAvailable()); + lua_pushboolean(L, outputTelemetryBuffer.isAvailable()); } - else if (isTelemetryOutputBufferAvailable()) { - SportTelemetryPacket packet; - packet.physicalId = getDataId(luaL_checkunsigned(L, 1)); - packet.primId = luaL_checkunsigned(L, 2); - packet.dataId = luaL_checkunsigned(L, 3); - packet.value = luaL_checkunsigned(L, 4); - sportOutputPushPacket(&packet); + else if (outputTelemetryBuffer.isAvailable()) { + outputTelemetryBuffer.sport.physicalId = getDataId(luaL_checkunsigned(L, 1)); // ignored + outputTelemetryBuffer.sport.primId = luaL_checkunsigned(L, 2); + outputTelemetryBuffer.sport.dataId = luaL_checkunsigned(L, 3); + outputTelemetryBuffer.sport.value = luaL_checkunsigned(L, 4); + outputTelemetryBuffer.setDestination(TELEMETRY_ENDPOINT_ANY); lua_pushboolean(L, true); } else { @@ -439,62 +438,6 @@ static int luaSportTelemetryPush(lua_State * L) return 1; } -#if defined(PXX2) -/*luadoc -@function pxx2TelemetryPush() - -This functions allows for sending SPORT telemetry data toward the receiver, -and more generally, to anything connected SPORT bus on the receiver or transmitter. - -When called without parameters, it will only return the status of the output buffer without sending anything. - -@param module module index - -@param receiver receiver index - -@param sensorId physical sensor ID - -@param frameId frame ID - -@param dataId data ID - -@param value value - -@retval boolean data queued in output buffer or not. - -@status current Introduced in 2.3.0 -*/ -static int luaPXX2TelemetryPush(lua_State * L) -{ - if (lua_gettop(L) == 0) { - lua_pushboolean(L, isTelemetryOutputBufferAvailable()); - } - else if (isTelemetryOutputBufferAvailable()) { - SportTelemetryPacket packet; - uint8_t module = luaL_checkunsigned(L, 1); - uint8_t receiver = luaL_checkunsigned(L, 2); - uint8_t rx_uid = g_model.moduleData[module].pxx2.getReceiverSlot(receiver); - if (rx_uid > 0) { - // TODO add more controls (module started, module = PXX2, etc.) - packet.physicalId = getDataId(luaL_checkunsigned(L, 3)); - packet.primId = luaL_checkunsigned(L, 4); - packet.dataId = luaL_checkunsigned(L, 5); - packet.value = luaL_checkunsigned(L, 6); - // TODO we could avoid this new copy - pushPXX2TelemetryPacket(module, rx_uid - 1, &packet); - lua_pushboolean(L, true); - } - else { - lua_pushboolean(L, false); - } - } - else { - lua_pushboolean(L, false); - } - return 1; -} -#endif - #if defined(CROSSFIRE) /*luadoc @function crossfireTelemetryPop() @@ -1366,10 +1309,6 @@ const luaL_Reg opentxLib[] = { { "sportTelemetryPop", luaSportTelemetryPop }, { "sportTelemetryPush", luaSportTelemetryPush }, { "setTelemetryValue", luaSetTelemetryValue }, -#if defined(PXX2) - { "sportTelemetryPop", luaSportTelemetryPop }, - { "pxx2TelemetryPush", luaPXX2TelemetryPush }, -#endif #if defined(CROSSFIRE) { "crossfireTelemetryPop", luaCrossfireTelemetryPop }, { "crossfireTelemetryPush", luaCrossfireTelemetryPush }, diff --git a/radio/src/pulses/pxx2.cpp b/radio/src/pulses/pxx2.cpp index 9876d58ab..978cfe29a 100644 --- a/radio/src/pulses/pxx2.cpp +++ b/radio/src/pulses/pxx2.cpp @@ -122,28 +122,28 @@ void Pxx2Pulses::setupChannelsFrame(uint8_t module) void Pxx2Pulses::setupTelemetryFrame(uint8_t module) { addFrameType(PXX2_TYPE_C_MODULE, PXX2_TYPE_ID_TELEMETRY); - for (uint8_t i = 0; i < outputTelemetryBuffer.size; i++) { + Pxx2Transport::addByte(outputTelemetryBuffer.destination.rxUid); + for (uint8_t i = 0; i < sizeof(SportTelemetryPacket); i++) { Pxx2Transport::addByte(outputTelemetryBuffer.data[i]); } - outputTelemetryBuffer.reset(); } void Pxx2Pulses::setupHardwareInfoFrame(uint8_t module) { - if (reusableBuffer.hardwareAndSettings.modules[module].current <= reusableBuffer.hardwareAndSettings.modules[module].maximum) { - if (reusableBuffer.hardwareAndSettings.modules[module].timeout == 0) { + if (reusableBuffer.hardwareAndSettings.modules[module].timeout == 0) { + if (reusableBuffer.hardwareAndSettings.modules[module].current <= reusableBuffer.hardwareAndSettings.modules[module].maximum) { addFrameType(PXX2_TYPE_C_MODULE, PXX2_TYPE_ID_HW_INFO); Pxx2Transport::addByte(reusableBuffer.hardwareAndSettings.modules[module].current); reusableBuffer.hardwareAndSettings.modules[module].timeout = 20; reusableBuffer.hardwareAndSettings.modules[module].current++; } else { - reusableBuffer.hardwareAndSettings.modules[module].timeout--; + moduleSettings[module].mode = MODULE_MODE_NORMAL; setupChannelsFrame(module); } } else { - moduleSettings[module].mode = MODULE_MODE_NORMAL; + reusableBuffer.hardwareAndSettings.modules[module].timeout--; setupChannelsFrame(module); } } @@ -319,8 +319,9 @@ void Pxx2Pulses::setupFrame(uint8_t module) setupShareMode(module); break; default: - if (outputTelemetryBuffer.size > 0 && outputTelemetryBuffer.destination == module) { + if (outputTelemetryBuffer.destination.module == module) { setupTelemetryFrame(module); + outputTelemetryBuffer.reset(); } else { setupChannelsFrame(module); diff --git a/radio/src/targets/taranis/telemetry_driver.cpp b/radio/src/targets/taranis/telemetry_driver.cpp index 31c632c05..67056ee10 100644 --- a/radio/src/targets/taranis/telemetry_driver.cpp +++ b/radio/src/targets/taranis/telemetry_driver.cpp @@ -158,8 +158,9 @@ extern "C" void TELEMETRY_USART_IRQHandler(void) #if defined(LUA) if (telemetryProtocol == PROTOCOL_TELEMETRY_FRSKY_SPORT) { static uint8_t prevdata; - if (prevdata == 0x7E && outputTelemetryBuffer.size > 0 && data == outputTelemetryBuffer.trigger && outputTelemetryBuffer.destination == SPORT_MODULE) { - sportSendBuffer(outputTelemetryBuffer.data, outputTelemetryBuffer.size); + if (prevdata == 0x7E && outputTelemetryBuffer.destination.value == TELEMETRY_ENDPOINT_SPORT && data == outputTelemetryBuffer.sport.physicalId) { + #warning "bytestuffing + buffer send" + // sportSendBuffer(outputTelemetryBuffer.data, outputTelemetryBuffer.size); } prevdata = data; } diff --git a/radio/src/telemetry/frsky.cpp b/radio/src/telemetry/frsky.cpp index 8098a8a74..055faa093 100644 --- a/radio/src/telemetry/frsky.cpp +++ b/radio/src/telemetry/frsky.cpp @@ -20,76 +20,6 @@ #include "opentx.h" -bool checkPXX2PacketCRC(const uint8_t * packet) -{ - // TODO ... - - return true; - // TRACE("checkPXX2PacketCRC(): checksum error "); - // 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; - - switch (dataState) { - case STATE_DATA_IDLE: - if (data == START_STOP) { - telemetryRxBufferCount = 0; - dataState = STATE_DATA_START; - } - break; - - case STATE_DATA_START: - if (telemetryRxBufferCount == 0 && data > 0x0C) { - // wrong length - telemetryRxBufferCount = 0; - dataState = STATE_DATA_IDLE; - } - else if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) { - telemetryRxBuffer[telemetryRxBufferCount++] = data; - if (telemetryRxBuffer[0] + 3 /* 1 byte for length, 2 bytes for CRC */ == telemetryRxBufferCount) { - if (checkPXX2PacketCRC(telemetryRxBuffer)) { - R9ModuleStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected - uint8_t R9Region = (telemetryRxBuffer[3] & 0x30) >> 4; - if (R9Region != 0x04) { - g_model.moduleData[EXTERNAL_MODULE].r9m.region = R9Region; - } - if (telemetryRxBuffer[2] & 0x80) { - createFrSkyPXX2Sensor(RSSI_ID, telemetryRxBuffer[2] & 0x7f); - } - else { - createFrSkyPXX2Sensor(BATT_ID, telemetryRxBuffer[2] & 0x7f); - } - if((telemetryRxBuffer[3] & 0x04) == 0) { - createFrSkyPXX2Sensor(R9_PWR_ID, (telemetryRxBuffer[3] & 0x03)); - } - if (telemetryRxBuffer[0] == 0x0C) { - sportProcessTelemetryPacketWithoutCrc(telemetryRxBuffer + 5 /* LEN, TYPE, RSSI/BAT, TP/SS/FW_T, FW_VER, Data ID */); - } - } - telemetryRxBufferCount = 0; - dataState = STATE_DATA_IDLE; - } - } - else { - // overflow guard - telemetryRxBufferCount = 0; - dataState = STATE_DATA_IDLE; - } - break; - } -} - void processFrskyTelemetryData(uint8_t data) { #if defined(PCBSKY9X) && defined(BLUETOOTH) diff --git a/radio/src/telemetry/frsky.h b/radio/src/telemetry/frsky.h index b665f4fdf..bd16f585d 100644 --- a/radio/src/telemetry/frsky.h +++ b/radio/src/telemetry/frsky.h @@ -284,7 +284,7 @@ void frskyDProcessPacket(const uint8_t *packet); // FrSky S.PORT Telemetry Protocol void sportProcessTelemetryPacket(const uint8_t * packet); -void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet); +void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packet); void telemetryWakeup(); void telemetryReset(); @@ -315,6 +315,5 @@ extern TelemetryData telemetryData; bool pushFrskyTelemetryData(uint8_t data); // returns true when end of frame detected void processFrskyTelemetryData(uint8_t data); -void processFrskyPXX2Data(uint8_t data); #endif // _FRSKY_H_ diff --git a/radio/src/telemetry/frsky_pxx2.cpp b/radio/src/telemetry/frsky_pxx2.cpp index 8486dc4fd..7b9bcf7b9 100644 --- a/radio/src/telemetry/frsky_pxx2.cpp +++ b/radio/src/telemetry/frsky_pxx2.cpp @@ -166,7 +166,10 @@ void processBindFrame(uint8_t module, uint8_t * frame) void processTelemetryFrame(uint8_t module, uint8_t * frame) { - sportProcessTelemetryPacketWithoutCrc(&frame[4]); + TelemetryEndpoint origin; + origin.module = module; + origin.rxUid = frame[3] & 0x0F; + sportProcessTelemetryPacketWithoutCrc(origin.value, &frame[4]); } void processSpectrumAnalyserFrame(uint8_t module, uint8_t * frame) @@ -260,16 +263,3 @@ void processPXX2Frame(uint8_t module, uint8_t *frame) break; } } - -void pushPXX2TelemetryPacket(uint8_t module, uint8_t rx_uid, SportTelemetryPacket * packet) -{ - // Flag0 - outputTelemetryBuffer.push(rx_uid); - - for (uint8_t i=0; iraw[i]; - outputTelemetryBuffer.push(byte); - } - - outputTelemetryBuffer.setDestination(module); -} diff --git a/radio/src/telemetry/frsky_sport.cpp b/radio/src/telemetry/frsky_sport.cpp index 3bc4edde9..29df3bd8c 100644 --- a/radio/src/telemetry/frsky_sport.cpp +++ b/radio/src/telemetry/frsky_sport.cpp @@ -147,19 +147,23 @@ void sportProcessTelemetryPacket(const uint8_t * packet) return; } - sportProcessTelemetryPacketWithoutCrc(packet); + sportProcessTelemetryPacketWithoutCrc(TELEMETRY_ENDPOINT_SPORT, packet); } -void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet) +void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packet) { uint8_t physicalId = packet[0] & 0x1F; uint8_t primId = packet[1]; - uint16_t id = *((uint16_t *)(packet+2)); + uint16_t dataId = *((uint16_t *)(packet+2)); uint32_t data = SPORT_DATA_S32(packet); if (primId == DATA_FRAME) { + if (outputTelemetryBuffer.destination.value == TELEMETRY_ENDPOINT_ANY && dataId == outputTelemetryBuffer.sport.dataId) { + outputTelemetryBuffer.sport.physicalId = packet[0]; + outputTelemetryBuffer.destination.value = origin; + } uint8_t instance = physicalId + 1; - if (id == RSSI_ID && isValidIdAndInstance(RSSI_ID, instance)) { + if (dataId == RSSI_ID && isValidIdAndInstance(RSSI_ID, instance)) { telemetryStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected data = SPORT_DATA_U8(packet); if (data == 0) @@ -167,17 +171,17 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet) else telemetryData.rssi.set(data); } - else if (id == R9_PWR_ID) { + else if (dataId == R9_PWR_ID) { uint32_t r9pwr[] = {100, 200, 500, 1000}; data = r9pwr[SPORT_DATA_U8(packet) & 0x03]; } - else if (id == XJT_VERSION_ID) { + else if (dataId == XJT_VERSION_ID) { telemetryData.xjtVersion = HUB_DATA_U16(packet); if (!IS_RAS_VALUE_VALID()) { telemetryData.swr.set(0x00); } } - else if (id == RAS_ID) { + else if (dataId == RAS_ID) { if (IS_RAS_VALUE_VALID()) telemetryData.swr.set(SPORT_DATA_U8(packet)); else @@ -185,33 +189,33 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet) } if (TELEMETRY_STREAMING()/* because when Rx is OFF it happens that some old A1/A2 values are sent from the XJT module*/) { - if ((id >> 8) == 0) { + if ((dataId >> 8) == 0) { // The old FrSky IDs - processHubPacket(id, HUB_DATA_U16(packet)); + processHubPacket(dataId, HUB_DATA_U16(packet)); } - else if (!IS_HIDDEN_TELEMETRY_VALUE(id)) { - if (id == ADC1_ID || id == ADC2_ID || id == BATT_ID || id == RAS_ID) { + else if (!IS_HIDDEN_TELEMETRY_VALUE(dataId)) { + if (dataId == ADC1_ID || dataId == ADC2_ID || dataId == BATT_ID || dataId == RAS_ID) { data = SPORT_DATA_U8(packet); } - if (id >= GPS_LONG_LATI_FIRST_ID && id <= GPS_LONG_LATI_LAST_ID) { + if (dataId >= GPS_LONG_LATI_FIRST_ID && dataId <= GPS_LONG_LATI_LAST_ID) { int32_t value = (data & 0x3fffffff); if (data & (1 << 30)) value = -value; value = (value * 5) / 3; // min/10000 => deg/1000000 if (data & (1 << 31)) - sportProcessTelemetryPacket(id, 0, instance, value, UNIT_GPS_LONGITUDE); + sportProcessTelemetryPacket(dataId, 0, instance, value, UNIT_GPS_LONGITUDE); else - sportProcessTelemetryPacket(id, 0, instance, value, UNIT_GPS_LATITUDE); + sportProcessTelemetryPacket(dataId, 0, instance, value, UNIT_GPS_LATITUDE); } - else if (id >= RBOX_BATT1_FIRST_ID && id <= RBOX_BATT2_LAST_ID) { - sportProcessTelemetryPacket(id, 0, instance, data & 0xffff); - sportProcessTelemetryPacket(id, 1, instance, data >> 16); + else if (dataId >= RBOX_BATT1_FIRST_ID && dataId <= RBOX_BATT2_LAST_ID) { + sportProcessTelemetryPacket(dataId, 0, instance, data & 0xffff); + sportProcessTelemetryPacket(dataId, 1, instance, data >> 16); } - else if (id >= RBOX_CNSP_FIRST_ID && id <= RBOX_CNSP_LAST_ID) { - sportProcessTelemetryPacket(id, 0, instance, data & 0xffff); - sportProcessTelemetryPacket(id, 1, instance, data >> 16); + else if (dataId >= RBOX_CNSP_FIRST_ID && dataId <= RBOX_CNSP_LAST_ID) { + sportProcessTelemetryPacket(dataId, 0, instance, data & 0xffff); + sportProcessTelemetryPacket(dataId, 1, instance, data >> 16); } - else if (id >= RBOX_STATE_FIRST_ID && id <= RBOX_STATE_LAST_ID) { + else if (dataId >= RBOX_STATE_FIRST_ID && dataId <= RBOX_STATE_LAST_ID) { bool static isRB10 = false; uint16_t newServosState; @@ -233,31 +237,31 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet) } servosState = newServosState; rboxState = newRboxState; - sportProcessTelemetryPacket(id, 0, instance, servosState); - sportProcessTelemetryPacket(id, 1, instance, rboxState); + sportProcessTelemetryPacket(dataId, 0, instance, servosState); + sportProcessTelemetryPacket(dataId, 1, instance, rboxState); } - else if (id >= ESC_POWER_FIRST_ID && id <= ESC_POWER_LAST_ID) { - sportProcessTelemetryPacket(id, 0, instance, data & 0xffff); - sportProcessTelemetryPacket(id, 1, instance, data >> 16); + else if (dataId >= ESC_POWER_FIRST_ID && dataId <= ESC_POWER_LAST_ID) { + sportProcessTelemetryPacket(dataId, 0, instance, data & 0xffff); + sportProcessTelemetryPacket(dataId, 1, instance, data >> 16); } - else if (id >= ESC_RPM_CONS_FIRST_ID && id <= ESC_RPM_CONS_LAST_ID) { - sportProcessTelemetryPacket(id, 0, instance, 100 * (data & 0xffff)); - sportProcessTelemetryPacket(id, 1, instance, data >> 16); + else if (dataId >= ESC_RPM_CONS_FIRST_ID && dataId <= ESC_RPM_CONS_LAST_ID) { + sportProcessTelemetryPacket(dataId, 0, instance, 100 * (data & 0xffff)); + sportProcessTelemetryPacket(dataId, 1, instance, data >> 16); } - else if (id >= ESC_TEMPERATURE_FIRST_ID && id <= ESC_TEMPERATURE_LAST_ID) { - sportProcessTelemetryPacket(id, 0, instance, data & 0x00ff); + else if (dataId >= ESC_TEMPERATURE_FIRST_ID && dataId <= ESC_TEMPERATURE_LAST_ID) { + sportProcessTelemetryPacket(dataId, 0, instance, data & 0x00ff); } - else if (id >= SBEC_POWER_FIRST_ID && id <= SBEC_POWER_LAST_ID) { - sportProcessTelemetryPacket(id, 0, instance, (data & 0xffff) / 10); - sportProcessTelemetryPacket(id, 1, instance, (data >> 16) / 10); + else if (dataId >= SBEC_POWER_FIRST_ID && dataId <= SBEC_POWER_LAST_ID) { + sportProcessTelemetryPacket(dataId, 0, instance, (data & 0xffff) / 10); + sportProcessTelemetryPacket(dataId, 1, instance, (data >> 16) / 10); } - else if (id >= DIY_STREAM_FIRST_ID && id <= DIY_STREAM_LAST_ID) { + else if (dataId >= DIY_STREAM_FIRST_ID && dataId <= DIY_STREAM_LAST_ID) { #if defined(LUA) if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(sizeof(SportTelemetryPacket))) { SportTelemetryPacket luaPacket; luaPacket.physicalId = physicalId; luaPacket.primId = primId; - luaPacket.dataId = id; + luaPacket.dataId = dataId; luaPacket.value = data; for (uint8_t i=0; ipush(luaPacket.raw[i]); @@ -266,7 +270,7 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet) #endif } else { - sportProcessTelemetryPacket(id, 0, instance, data); + sportProcessTelemetryPacket(dataId, 0, instance, data); } } } @@ -277,7 +281,7 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet) SportTelemetryPacket luaPacket; luaPacket.physicalId = physicalId; luaPacket.primId = primId; - luaPacket.dataId = id; + luaPacket.dataId = dataId; luaPacket.value = data; for (uint8_t i=0; ipush(luaPacket.raw[i]); diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index 04b37cf0b..b3183dae8 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -63,11 +63,6 @@ void processTelemetryData(uint8_t data) } #endif - if (telemetryProtocol == PROTOCOL_TELEMETRY_PXX2) { - processFrskyPXX2Data(data); - return; - } - processFrskyTelemetryData(data); } diff --git a/radio/src/telemetry/telemetry.h b/radio/src/telemetry/telemetry.h index bdab8d178..4cba60cc3 100644 --- a/radio/src/telemetry/telemetry.h +++ b/radio/src/telemetry/telemetry.h @@ -155,7 +155,17 @@ void logTelemetryWriteByte(uint8_t data); #define LOG_TELEMETRY_WRITE_BYTE(data) #endif -#define TELEMETRY_OUTPUT_FIFO_SIZE 20 +#define TELEMETRY_ENDPOINT_ANY 0xFF +#define TELEMETRY_ENDPOINT_NONE 0xFE +#define TELEMETRY_ENDPOINT_SPORT 0xFD + +union TelemetryEndpoint { + PACK(struct { + uint8_t module:4; + uint8_t rxUid:4; + }); + uint8_t value; +}; class OutputTelemetryBuffer { public: @@ -163,29 +173,24 @@ class OutputTelemetryBuffer { reset(); } - void push(uint8_t byte) { - data[size++] = byte; - } - - void setTrigger(uint8_t byte) { - trigger = byte; - } - - void setDestination(uint8_t module) { - destination = module; + void setDestination(uint8_t value) { + destination.value = value; } void reset() { - size = 0; - trigger = 0x7E; - destination = 0xFF; + destination.value = TELEMETRY_ENDPOINT_NONE; + } + + bool isAvailable() { + return destination.value == TELEMETRY_ENDPOINT_NONE; } public: - uint8_t data[TELEMETRY_OUTPUT_FIFO_SIZE]; - uint8_t size; - uint8_t trigger; - uint8_t destination; + union { + SportTelemetryPacket sport; + uint8_t data[16]; + }; + TelemetryEndpoint destination; }; extern OutputTelemetryBuffer outputTelemetryBuffer __DMA; @@ -202,6 +207,5 @@ extern Fifo * luaInputTelemetryFifo; #endif void processPXX2Frame(uint8_t module, uint8_t *frame); -void pushPXX2TelemetryPacket(uint8_t module, uint8_t rx_uid, SportTelemetryPacket * packet); #endif // _TELEMETRY_H_