From ff829f1f7b912812bfd1e0362633915700299445 Mon Sep 17 00:00:00 2001 From: Pascal Langer Date: Wed, 10 Feb 2021 12:31:37 +0100 Subject: [PATCH] M-Link telemetry forwarded to OpenTX --- Multiprotocol/MLINK_cyrf6936.ino | 24 +++++++++++++++++++++--- Multiprotocol/Multiprotocol.h | 9 ++++++++- Multiprotocol/Telemetry.ino | 19 +++++++++++++++++++ Multiprotocol/Validate.h | 9 ++++++++- Multiprotocol/_Config.h | 3 ++- 5 files changed, 58 insertions(+), 6 deletions(-) diff --git a/Multiprotocol/MLINK_cyrf6936.ino b/Multiprotocol/MLINK_cyrf6936.ino index 709c8fc..cf833e2 100644 --- a/Multiprotocol/MLINK_cyrf6936.ino +++ b/Multiprotocol/MLINK_cyrf6936.ino @@ -321,6 +321,24 @@ static void __attribute__((unused)) MLINK_send_data_packet() } #endif +#ifdef MLINK_FW_TELEMETRY + static void __attribute__((unused)) MLINK_Send_Telemetry() + { + telemetry_counter += 2; // TX LQI counter + telemetry_link = 4; + + // Read TX RSSI + TX_RSSI = CYRF_ReadRegister(CYRF_13_RSSI)&0x1F; + + if(telemetry_lost) + { + telemetry_lost = 0; + packet_count = 50; + telemetry_counter = 100; + } + } +#endif + uint16_t MLINK_callback() { uint8_t status; @@ -408,7 +426,7 @@ uint16_t MLINK_callback() hopping_frequency_no = 0x00; CYRF_ConfigRFChannel(hopping_frequency[hopping_frequency_no]); CYRF_SetPower(0x38); - #ifdef MLINK_HUB_TELEMETRY + #if defined(MLINK_HUB_TELEMETRY) || defined(MLINK_FW_TELEMETRY) packet_count = 0; telemetry_lost = 1; #endif @@ -449,7 +467,7 @@ uint16_t MLINK_callback() phase=MLINK_SEND1; return 4470; case MLINK_RX: - #ifdef MLINK_HUB_TELEMETRY + #if defined(MLINK_HUB_TELEMETRY) || defined(MLINK_FW_TELEMETRY) //TX LQI calculation packet_count++; if(packet_count>=50) @@ -470,7 +488,7 @@ uint16_t MLINK_callback() if( len && len <= MLINK_PACKET_SIZE ) { CYRF_ReadDataPacketLen(packet_in, len*2); - #ifdef MLINK_HUB_TELEMETRY + #if defined(MLINK_HUB_TELEMETRY) || defined(MLINK_FW_TELEMETRY) if(len==MLINK_PACKET_SIZE) { for(uint8_t i=0;i<8;i++) diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index 5533a17..70944af 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -19,7 +19,7 @@ #define VERSION_MAJOR 1 #define VERSION_MINOR 3 #define VERSION_REVISION 2 -#define VERSION_PATCH_LEVEL 35 +#define VERSION_PATCH_LEVEL 36 //****************** // Protocols @@ -488,6 +488,7 @@ enum MultiPacketTypes MULTI_TELEMETRY_AFHDS2A_AC = 12, MULTI_TELEMETRY_RX_CHANNELS = 13, MULTI_TELEMETRY_HOTT = 14, + MULTI_TELEMETRY_MLINK = 15, }; // Macros @@ -1234,4 +1235,10 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p -- data[3] = page data[4-13] = data + Type 0x0F M-Link telemetry + length: 10 + data[0] = TX_RSSI + data[1] = TX_LQI + data[2] = telem_type + data[3-9] = data */ diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino index afc5d14..4ebe687 100644 --- a/Multiprotocol/Telemetry.ino +++ b/Multiprotocol/Telemetry.ino @@ -203,6 +203,17 @@ static void multi_send_status() } } +#ifdef MLINK_FW_TELEMETRY + void MLINK_frame() + { + multi_send_header(MULTI_TELEMETRY_MLINK, 10); + Serial_write(TX_RSSI); // RSSI + Serial_write(TX_LQI); // LQI + for (uint8_t i = 0; i < 8; i++) // followed by 8 bytes of telemetry data + Serial_write(packet_in[i]); + } +#endif + #ifdef DSM_TELEMETRY void DSM_frame() { @@ -874,6 +885,14 @@ void TelemetryUpdate() #endif // SPORT_TELEMETRY #ifdef MULTI_TELEMETRY + #if defined MLINK_FW_TELEMETRY + if(telemetry_link && protocol == PROTO_MLINK) + { + MLINK_frame(); + telemetry_link=0; + return; + } + #endif #if defined DSM_TELEMETRY if(telemetry_link && protocol == PROTO_DSM) { // DSM diff --git a/Multiprotocol/Validate.h b/Multiprotocol/Validate.h index ca6a6f5..537b225 100644 --- a/Multiprotocol/Validate.h +++ b/Multiprotocol/Validate.h @@ -359,6 +359,8 @@ #undef DSM_FWD_PGM #undef WFLY2_HUB_TELEMETRY #undef LOLI_HUB_TELEMETRY + #undef MLINK_HUB_TELEMETRY + #undef MLINK_FW_TELEMETRY #else #if not defined(SCANNER_CC2500_INO) || not defined(SCANNER_TELEMETRY) #undef SCANNER_TELEMETRY @@ -423,6 +425,7 @@ #endif #if not defined(MLINK_CYRF6936_INO) #undef MLINK_HUB_TELEMETRY + #undef MLINK_FW_TELEMETRY #endif #if not defined(HOTT_CC2500_INO) #undef HOTT_FW_TELEMETRY @@ -437,13 +440,17 @@ //protocols using FRSKYD user frames #undef HUB_TELEMETRY #endif - #if not defined(HOTT_FW_TELEMETRY) && not defined(DSM_TELEMETRY) && not defined(SPORT_TELEMETRY) && not defined(HUB_TELEMETRY) && not defined(HUBSAN_HUB_TELEMETRY) && not defined(BUGS_HUB_TELEMETRY) && not defined(NCC1701_HUB_TELEMETRY) && not defined(BAYANG_HUB_TELEMETRY) && not defined(CABELL_HUB_TELEMETRY) && not defined(RLINK_HUB_TELEMETRY) && not defined(AFHDS2A_HUB_TELEMETRY) && not defined(AFHDS2A_FW_TELEMETRY) && not defined(MULTI_TELEMETRY) && not defined(MULTI_STATUS) && not defined(HITEC_HUB_TELEMETRY) && not defined(HITEC_FW_TELEMETRY) && not defined(SCANNER_TELEMETRY) && not defined(FRSKY_RX_TELEMETRY) && not defined(AFHDS2A_RX_TELEMETRY) && not defined(BAYANG_RX_TELEMETRY) && not defined(DEVO_HUB_TELEMETRY) && not defined(PROPEL_HUB_TELEMETRY) && not defined(OMP_HUB_TELEMETRY) && not defined(WFLY2_HUB_TELEMETRY) && not defined(LOLI_HUB_TELEMETRY) && not defined(MLINK_HUB_TELEMETRY) + #if not defined(HOTT_FW_TELEMETRY) && not defined(DSM_TELEMETRY) && not defined(SPORT_TELEMETRY) && not defined(HUB_TELEMETRY) && not defined(HUBSAN_HUB_TELEMETRY) && not defined(BUGS_HUB_TELEMETRY) && not defined(NCC1701_HUB_TELEMETRY) && not defined(BAYANG_HUB_TELEMETRY) && not defined(CABELL_HUB_TELEMETRY) && not defined(RLINK_HUB_TELEMETRY) && not defined(AFHDS2A_HUB_TELEMETRY) && not defined(AFHDS2A_FW_TELEMETRY) && not defined(MULTI_TELEMETRY) && not defined(MULTI_STATUS) && not defined(HITEC_HUB_TELEMETRY) && not defined(HITEC_FW_TELEMETRY) && not defined(SCANNER_TELEMETRY) && not defined(FRSKY_RX_TELEMETRY) && not defined(AFHDS2A_RX_TELEMETRY) && not defined(BAYANG_RX_TELEMETRY) && not defined(DEVO_HUB_TELEMETRY) && not defined(PROPEL_HUB_TELEMETRY) && not defined(OMP_HUB_TELEMETRY) && not defined(WFLY2_HUB_TELEMETRY) && not defined(LOLI_HUB_TELEMETRY) && not defined(MLINK_HUB_TELEMETRY) && not defined(MLINK_FW_TELEMETRY) #undef TELEMETRY #undef INVERT_TELEMETRY #undef MULTI_TELEMETRY #endif #endif +#if defined(MLINK_HUB_TELEMETRY) && defined(MLINK_FW_TELEMETRY) + #error "You must select MLINK_HUB_TELEMETRY or MLINK_FW_TELEMETRY, not both." +#endif + #ifdef SPORT_TELEMETRY #define SPORT_SEND #endif diff --git a/Multiprotocol/_Config.h b/Multiprotocol/_Config.h index 2fdfa70..830d094 100644 --- a/Multiprotocol/_Config.h +++ b/Multiprotocol/_Config.h @@ -327,7 +327,8 @@ #define RLINK_HUB_TELEMETRY // Use FrSkyD Hub format to send telemetry to TX #define WFLY2_HUB_TELEMETRY // Use FrSkyD Hub format to send telemetry to TX #define LOLI_HUB_TELEMETRY // Use FrSkyD Hub format to send telemetry to TX -#define MLINK_HUB_TELEMETRY // Use FrSkyD Hub format to send telemetry to TX +//#define MLINK_HUB_TELEMETRY // Use FrSkyD Hub format to send telemetry to TX +#define MLINK_FW_TELEMETRY // Forward received telemetry packet directly to TX to be decoded by erskyTX and OpenTX //#define HITEC_HUB_TELEMETRY // Use FrSkyD Hub format to send basic telemetry to the radios which can decode it like er9x, erskyTX and OpenTX #define HITEC_FW_TELEMETRY // Forward received telemetry packets to be decoded by erskyTX and OpenTX #define SCANNER_TELEMETRY // Forward spectrum scanner data to TX