From 5c8da849ad169560d2adaabaa5afa4138e054af3 Mon Sep 17 00:00:00 2001 From: Jye <14170229+JyeSmith@users.noreply.github.com> Date: Mon, 11 Nov 2024 17:15:54 +1000 Subject: [PATCH] Add tlmConfirm to tlm_dl ota packet-structure (#2991) * add-tlmConfirm-to-tlm_dl-ota-packet-structure * clean up naming * hardcode packageIndex and count --- src/lib/OTA/OTA.cpp | 3 +- src/lib/OTA/OTA.h | 31 ++++++------ src/lib/TelemetryProtocol/telemetry_protocol.h | 2 - src/src/rx_main.cpp | 66 ++++++++++++++------------ src/src/tx_main.cpp | 66 ++++++++++++++------------ 5 files changed, 89 insertions(+), 79 deletions(-) diff --git a/src/lib/OTA/OTA.cpp b/src/lib/OTA/OTA.cpp index 219b9941..51e34df9 100644 --- a/src/lib/OTA/OTA.cpp +++ b/src/lib/OTA/OTA.cpp @@ -573,7 +573,7 @@ void OtaUpdateSerializers(OtaSwitchMode_e const switchMode, uint8_t packetSize) void OtaPackAirportData(OTA_Packet_s * const otaPktPtr, FIFO *inputBuffer) { - otaPktPtr->std.type = PACKET_TYPE_TLM; + otaPktPtr->std.type = PACKET_TYPE_DATA; inputBuffer->lock(); uint8_t count = inputBuffer->size(); @@ -588,7 +588,6 @@ void OtaPackAirportData(OTA_Packet_s * const otaPktPtr, FIFO *in count = std::min(count, (uint8_t)ELRS4_TELEMETRY_BYTES_PER_CALL); otaPktPtr->std.airport.count = count; inputBuffer->popBytes(otaPktPtr->std.airport.payload, count); - otaPktPtr->std.airport.type = ELRS_TELEMETRY_TYPE_DATA; } inputBuffer->unlock(); } diff --git a/src/lib/OTA/OTA.h b/src/lib/OTA/OTA.h index 8a62df84..1f225a30 100644 --- a/src/lib/OTA/OTA.h +++ b/src/lib/OTA/OTA.h @@ -14,11 +14,13 @@ #define OTA8_PACKET_SIZE 13U #define OTA8_CRC_CALC_LEN offsetof(OTA_Packet8_s, crc) -// Packet header types (ota.std.type) -#define PACKET_TYPE_RCDATA 0b00 -#define PACKET_TYPE_MSPDATA 0b01 -#define PACKET_TYPE_TLM 0b11 -#define PACKET_TYPE_SYNC 0b10 +// Packet header types +#define PACKET_TYPE_DATA 0b01 +// Uplink only header types +#define PACKET_TYPE_RCDATA 0b00 +#define PACKET_TYPE_SYNC 0b10 +// Downlink only header types +#define PACKET_TYPE_LINKSTATS 0b00 // Mask used to XOR the ModelId into the SYNC packet for ModelMatch #define MODELMATCH_MASK 0x3f @@ -41,7 +43,7 @@ typedef struct { uint8_t uplink_RSSI_2:7, modelMatch:1; uint8_t lq:7, - mspConfirm:1; + tlmConfirm:1; int8_t SNR; } PACKED OTA_LinkStats_s; @@ -68,15 +70,16 @@ typedef struct { /** PACKET_TYPE_MSP **/ struct { uint8_t packageIndex:7, - tlmFlag:1; + tlmConfirm:1; uint8_t payload[ELRS4_MSP_BYTES_PER_CALL]; } msp_ul; /** PACKET_TYPE_SYNC **/ OTA_Sync_s sync; /** PACKET_TYPE_TLM **/ struct { - uint8_t type:ELRS4_TELEMETRY_SHIFT, - packageIndex:(8 - ELRS4_TELEMETRY_SHIFT); + uint8_t free:1, + tlmConfirm: 1, + packageIndex:6; union { struct { OTA_LinkStats_s stats; @@ -87,8 +90,8 @@ typedef struct { } tlm_dl; // PACKET_TYPE_TLM /** PACKET_TYPE_AIRPORT **/ struct { - uint8_t type:ELRS4_TELEMETRY_SHIFT, - count:(8 - ELRS4_TELEMETRY_SHIFT); + uint8_t free:2, + count:6; uint8_t payload[ELRS4_TELEMETRY_BYTES_PER_CALL]; } PACKED airport; }; @@ -119,7 +122,7 @@ typedef struct { struct { uint8_t packetType: 2, packageIndex: 5, - tlmFlag: 1; + tlmConfirm: 1; uint8_t payload[ELRS8_MSP_BYTES_PER_CALL]; } msp_ul; /** PACKET_TYPE_SYNC **/ @@ -131,7 +134,7 @@ typedef struct { /** PACKET_TYPE_TLM **/ struct { uint8_t packetType: 2, - containsLinkStats: 1, + tlmConfirm: 1, packageIndex: 5; union { struct { @@ -144,7 +147,7 @@ typedef struct { /** PACKET_TYPE_AIRPORT **/ struct { uint8_t packetType: 2, - containsLinkStats: 1, + free: 1, count: 5; uint8_t payload[ELRS8_TELEMETRY_BYTES_PER_CALL]; } PACKED airport; diff --git a/src/lib/TelemetryProtocol/telemetry_protocol.h b/src/lib/TelemetryProtocol/telemetry_protocol.h index 048cc649..0c242287 100644 --- a/src/lib/TelemetryProtocol/telemetry_protocol.h +++ b/src/lib/TelemetryProtocol/telemetry_protocol.h @@ -1,7 +1,5 @@ #pragma once -#define ELRS_TELEMETRY_TYPE_LINK 0x01 -#define ELRS_TELEMETRY_TYPE_DATA 0x02 #define ELRS4_TELEMETRY_SHIFT 2 #define ELRS4_TELEMETRY_BYTES_PER_CALL 5 #define ELRS4_TELEMETRY_MAX_PACKAGES (255 >> ELRS4_TELEMETRY_SHIFT) diff --git a/src/src/rx_main.cpp b/src/src/rx_main.cpp index 13cef57e..5bb0af33 100644 --- a/src/src/rx_main.cpp +++ b/src/src/rx_main.cpp @@ -160,7 +160,7 @@ uint8_t MspData[ELRS_MSP_BUFFER]; uint8_t mavlinkSSBuffer[CRSF_MAX_PACKET_LEN]; // Buffer for current stubbon sender packet (mavlink only) static bool tlmSent = false; -static uint8_t NextTelemetryType = ELRS_TELEMETRY_TYPE_LINK; +static uint8_t NextTelemetryType = PACKET_TYPE_LINKSTATS; static bool telemBurstValid; /// PFD Filters //////////////// LPF LPF_Offset(2); @@ -440,7 +440,7 @@ void ICACHE_RAM_ATTR LinkStatsToOta(OTA_LinkStats_s * const ls) ls->antenna = antenna; ls->modelMatch = connectionHasModelMatch; ls->lq = CRSF::LinkStatistics.uplink_Link_quality; - ls->mspConfirm = MspReceiver.GetCurrentConfirm() ? 1 : 0; + ls->tlmConfirm = MspReceiver.GetCurrentConfirm() ? 1 : 0; #if defined(DEBUG_FREQ_CORRECTION) ls->SNR = FreqCorrection * 127 / FreqCorrectionMax; #else @@ -467,7 +467,6 @@ bool ICACHE_RAM_ATTR HandleSendTelemetryResponse() // ESP requires word aligned buffer WORD_ALIGNED_ATTR OTA_Packet_s otaPkt = {0}; alreadyTLMresp = true; - otaPkt.std.type = PACKET_TYPE_TLM; bool tlmQueued = false; if (firmwareOptions.is_airport) @@ -479,12 +478,13 @@ bool ICACHE_RAM_ATTR HandleSendTelemetryResponse() tlmQueued = TelemetrySender.IsActive(); } - if (NextTelemetryType == ELRS_TELEMETRY_TYPE_LINK || !tlmQueued) + if (NextTelemetryType == PACKET_TYPE_LINKSTATS || !tlmQueued) { + otaPkt.std.type = PACKET_TYPE_LINKSTATS; + OTA_LinkStats_s * ls; if (OtaIsFullRes) { - otaPkt.full.tlm_dl.containsLinkStats = 1; ls = &otaPkt.full.tlm_dl.ul_link_stats.stats; // Include some advanced telemetry in the extra space // Note the use of `ul_link_stats.payload` vs just `payload` @@ -494,17 +494,16 @@ bool ICACHE_RAM_ATTR HandleSendTelemetryResponse() } else { - otaPkt.std.tlm_dl.type = ELRS_TELEMETRY_TYPE_LINK; ls = &otaPkt.std.tlm_dl.ul_link_stats.stats; } LinkStatsToOta(ls); - NextTelemetryType = ELRS_TELEMETRY_TYPE_DATA; + NextTelemetryType = PACKET_TYPE_DATA; // Start the count at 1 because the next will be DATA and doing +1 before checking // against Max below is for some reason 10 bytes more code telemetryBurstCount = 1; } - else + else // if tlmQueued { if (telemetryBurstCount < telemetryBurstMax) { @@ -512,27 +511,38 @@ bool ICACHE_RAM_ATTR HandleSendTelemetryResponse() } else { - NextTelemetryType = ELRS_TELEMETRY_TYPE_LINK; + NextTelemetryType = PACKET_TYPE_LINKSTATS; } + otaPkt.std.type = PACKET_TYPE_DATA; + if (firmwareOptions.is_airport) { OtaPackAirportData(&otaPkt, &apInputBuffer); } - else if (TelemetrySender.IsActive()) + else { if (OtaIsFullRes) { - otaPkt.full.tlm_dl.packageIndex = TelemetrySender.GetCurrentPayload( - otaPkt.full.tlm_dl.payload, - sizeof(otaPkt.full.tlm_dl.payload)); + otaPkt.full.tlm_dl.tlmConfirm = MspReceiver.GetCurrentConfirm() ? 1 : 0; + + if (TelemetrySender.IsActive()) + { + otaPkt.full.tlm_dl.packageIndex = TelemetrySender.GetCurrentPayload( + otaPkt.full.tlm_dl.payload, + sizeof(otaPkt.full.tlm_dl.payload)); + } } else { - otaPkt.std.tlm_dl.type = ELRS_TELEMETRY_TYPE_DATA; - otaPkt.std.tlm_dl.packageIndex = TelemetrySender.GetCurrentPayload( - otaPkt.std.tlm_dl.payload, - sizeof(otaPkt.std.tlm_dl.payload)); + otaPkt.std.tlm_dl.tlmConfirm = MspReceiver.GetCurrentConfirm() ? 1 : 0; + + if (TelemetrySender.IsActive()) + { + otaPkt.std.tlm_dl.packageIndex = TelemetrySender.GetCurrentPayload( + otaPkt.std.tlm_dl.payload, + sizeof(otaPkt.std.tlm_dl.payload)); + } } } } @@ -965,7 +975,7 @@ static void ICACHE_RAM_ATTR ProcessRfPacket_MSP(OTA_Packet_s const * const otaPk dataLen = sizeof(otaPktPtr->full.msp_ul.payload); if (config.GetSerialProtocol() == PROTOCOL_MAVLINK) { - TelemetrySender.ConfirmCurrentPayload(otaPktPtr->full.msp_ul.tlmFlag); + TelemetrySender.ConfirmCurrentPayload(otaPktPtr->full.msp_ul.tlmConfirm); } else { @@ -979,7 +989,7 @@ static void ICACHE_RAM_ATTR ProcessRfPacket_MSP(OTA_Packet_s const * const otaPk dataLen = sizeof(otaPktPtr->std.msp_ul.payload); if (config.GetSerialProtocol() == PROTOCOL_MAVLINK) { - TelemetrySender.ConfirmCurrentPayload(otaPktPtr->std.msp_ul.tlmFlag); + TelemetrySender.ConfirmCurrentPayload(otaPktPtr->std.msp_ul.tlmConfirm); } else { @@ -997,14 +1007,9 @@ static void ICACHE_RAM_ATTR ProcessRfPacket_MSP(OTA_Packet_s const * const otaPk // Must be fully connected to process MSP, prevents processing MSP // during sync, where packets can be received before connection - if (connectionState != connected) - return; - - bool currentMspConfirmValue = MspReceiver.GetCurrentConfirm(); - MspReceiver.ReceiveData(packageIndex, payload, dataLen); - if (currentMspConfirmValue != MspReceiver.GetCurrentConfirm()) + if (connectionState == connected) { - NextTelemetryType = ELRS_TELEMETRY_TYPE_LINK; + MspReceiver.ReceiveData(packageIndex, payload, dataLen); } } @@ -1132,19 +1137,20 @@ bool ICACHE_RAM_ATTR ProcessRFPacket(SX12xxDriverCommon::rx_status const status) case PACKET_TYPE_RCDATA: //Standard RC Data Packet ProcessRfPacket_RC(otaPktPtr); break; - case PACKET_TYPE_MSPDATA: - ProcessRfPacket_MSP(otaPktPtr); - break; case PACKET_TYPE_SYNC: //sync packet from master doStartTimer = ProcessRfPacket_SYNC(now, OtaIsFullRes ? &otaPktPtr->full.sync.sync : &otaPktPtr->std.sync) && !InBindingMode; break; - case PACKET_TYPE_TLM: + case PACKET_TYPE_DATA: if (firmwareOptions.is_airport) { OtaUnpackAirportData(otaPktPtr, &apOutputBuffer); } + else + { + ProcessRfPacket_MSP(otaPktPtr); + } break; default: break; diff --git a/src/src/tx_main.cpp b/src/src/tx_main.cpp index 7563015a..724527b4 100644 --- a/src/src/tx_main.cpp +++ b/src/src/tx_main.cpp @@ -160,7 +160,7 @@ void ICACHE_RAM_ATTR LinkStatsFromOta(OTA_LinkStats_s * const ls) // -- uplink_TX_Power is updated when sending to the handset, so it updates when missing telemetry // -- rf_mode is updated when we change rates // -- downlink_Link_quality is updated before the LQ period is incremented - MspSender.ConfirmCurrentPayload(ls->mspConfirm); + MspSender.ConfirmCurrentPayload(ls->tlmConfirm); } bool ICACHE_RAM_ATTR ProcessTLMpacket(SX12xxDriverCommon::rx_status const status) @@ -178,12 +178,6 @@ bool ICACHE_RAM_ATTR ProcessTLMpacket(SX12xxDriverCommon::rx_status const status return false; } - if (otaPktPtr->std.type != PACKET_TYPE_TLM) - { - DBGLN("TLM type error %d", otaPktPtr->std.type); - return false; - } - LastTLMpacketRecvMillis = millis(); LQCalc.add(); @@ -198,43 +192,53 @@ bool ICACHE_RAM_ATTR ProcessTLMpacket(SX12xxDriverCommon::rx_status const status OTA_Packet8_s * const ota8 = (OTA_Packet8_s * const)otaPktPtr; uint8_t *telemPtr; uint8_t dataLen; - if (ota8->tlm_dl.containsLinkStats) - { - LinkStatsFromOta(&ota8->tlm_dl.ul_link_stats.stats); - telemPtr = ota8->tlm_dl.ul_link_stats.payload; - dataLen = sizeof(ota8->tlm_dl.ul_link_stats.payload); - } - else + + switch (otaPktPtr->std.type) { - if (firmwareOptions.is_airport) - { - OtaUnpackAirportData(otaPktPtr, &apOutputBuffer); - return true; - } - telemPtr = ota8->tlm_dl.payload; - dataLen = sizeof(ota8->tlm_dl.payload); + case PACKET_TYPE_LINKSTATS: + LinkStatsFromOta(&ota8->tlm_dl.ul_link_stats.stats); + telemPtr = ota8->tlm_dl.ul_link_stats.payload; + dataLen = sizeof(ota8->tlm_dl.ul_link_stats.payload); + break; + + case PACKET_TYPE_DATA: + if (firmwareOptions.is_airport) + { + OtaUnpackAirportData(otaPktPtr, &apOutputBuffer); + } + else + { + telemPtr = ota8->tlm_dl.payload; + dataLen = sizeof(ota8->tlm_dl.payload); + MspSender.ConfirmCurrentPayload(ota8->tlm_dl.tlmConfirm); + } + break; } + //DBGLN("pi=%u len=%u", ota8->tlm_dl.packageIndex, dataLen); TelemetryReceiver.ReceiveData(ota8->tlm_dl.packageIndex & ELRS8_TELEMETRY_MAX_PACKAGES, telemPtr, dataLen); } // Std res mode else { - switch (otaPktPtr->std.tlm_dl.type) + switch (otaPktPtr->std.type) { - case ELRS_TELEMETRY_TYPE_LINK: + case PACKET_TYPE_LINKSTATS: LinkStatsFromOta(&otaPktPtr->std.tlm_dl.ul_link_stats.stats); break; - case ELRS_TELEMETRY_TYPE_DATA: + case PACKET_TYPE_DATA: if (firmwareOptions.is_airport) { OtaUnpackAirportData(otaPktPtr, &apOutputBuffer); - return true; } - TelemetryReceiver.ReceiveData(otaPktPtr->std.tlm_dl.packageIndex & ELRS4_TELEMETRY_MAX_PACKAGES, - otaPktPtr->std.tlm_dl.payload, - sizeof(otaPktPtr->std.tlm_dl.payload)); + else + { + MspSender.ConfirmCurrentPayload(otaPktPtr->std.tlm_dl.tlmConfirm); + TelemetryReceiver.ReceiveData(otaPktPtr->std.tlm_dl.packageIndex & ELRS4_TELEMETRY_MAX_PACKAGES, + otaPktPtr->std.tlm_dl.payload, + sizeof(otaPktPtr->std.tlm_dl.payload)); + } break; } } @@ -529,14 +533,14 @@ void ICACHE_RAM_ATTR SendRCdataToRF() } else if ((NextPacketIsMspData && MspSender.IsActive()) || dontSendChannelData) { - otaPkt.std.type = PACKET_TYPE_MSPDATA; + otaPkt.std.type = PACKET_TYPE_DATA; if (OtaIsFullRes) { otaPkt.full.msp_ul.packageIndex = MspSender.GetCurrentPayload( otaPkt.full.msp_ul.payload, sizeof(otaPkt.full.msp_ul.payload)); if (config.GetLinkMode() == TX_MAVLINK_MODE) - otaPkt.full.msp_ul.tlmFlag = TelemetryReceiver.GetCurrentConfirm(); + otaPkt.full.msp_ul.tlmConfirm = TelemetryReceiver.GetCurrentConfirm(); } else { @@ -544,7 +548,7 @@ void ICACHE_RAM_ATTR SendRCdataToRF() otaPkt.std.msp_ul.payload, sizeof(otaPkt.std.msp_ul.payload)); if (config.GetLinkMode() == TX_MAVLINK_MODE) - otaPkt.std.msp_ul.tlmFlag = TelemetryReceiver.GetCurrentConfirm(); + otaPkt.std.msp_ul.tlmConfirm = TelemetryReceiver.GetCurrentConfirm(); } // send channel data next so the channel messages also get sent during msp transmissions -- 2.11.4.GIT