diff --git a/Multiprotocol/AFHDS2A_a7105.ino b/Multiprotocol/AFHDS2A_a7105.ino index 5fc8963..709f326 100644 --- a/Multiprotocol/AFHDS2A_a7105.ino +++ b/Multiprotocol/AFHDS2A_a7105.ino @@ -1,4 +1,4 @@ -/* + /* This project is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index 7c8fe3a..741f0c6 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -183,6 +183,18 @@ struct PPM_Parameters uint8_t option; }; +// Telemetry + +enum MultiPacketTypes { + MULTI_TELEMETRY_STATUS = 1, + MULTI_TELEMETRY_SPORT = 2, + MULTI_TELEMETRY_HUB = 3, + MULTI_TELEMETRY_DSM = 4, + MULTI_TELEMETRY_DSMBIND = 5, + MULTI_TELEMETRY_AFHDS2A = 6, +}; + + // Macros #define NOP() __asm__ __volatile__("nop") @@ -523,3 +535,63 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p -- 2047 +125% Channels bits are concatenated to fit in 22 bytes like in SBUS protocol */ +/* + Multiprotocol telemetry definition + + Serial: 100000 Baud 8e2 (same as input) + + TLV Protocol (type, length, value), allows a TX to ignore unknown messages + + Format: header (4 byte) + data (variable) + + [0] = 'M' (0x4d) + [1] = 'P' (0x50) + + + The first byte is deliberatly chosen to be different from other telemetry protocols + (e.g. 0xAA for DSM/Multi, 0xAA for FlySky and 0x7e for Frsky) to allow a TX to detect + the telemetry format of older versions + + [2] Type (see below) + [3] Length (excluding the 4 header bytes) + + [4-xx] data + + +Type = 0x01 Multimodule Status: + [4] Flags + 0x01 = Input signal detected + 0x02 = Serial mode enabled + 0x04 = protocol is valid + 0x08 = module is in binding mode + [5] major + [6] mior + [7-8] patchlevel + version of multi code, should be displayed as major.minor.patchlevel + + + more information can be added by specifing a longer length of the type, the TX will just ignore these bytes + + +Type 0x02 Frksy S.port telemetry +Type 0x03 Frsky Hub telemetry + + *No* usual frsky byte stuffing and without start/stop byte (0x7e) + + +Type 0x04 Spektrum telemetry data + data[0] RSSI + data[1-15] telemetry data + +Type 0x05 DSM bind data + data[0-16] DSM bind data + + technically DSM bind data is only 10 bytes but multi send 16 + like with telemtry, check length field) + +Type 0x06 Flysky AFHDS2 telemetry data + length: 29 + data[0] = RSSI value + data[1-28] telemetry data + +*/ diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino index 4f9798a..75c435d 100644 --- a/Multiprotocol/Multiprotocol.ino +++ b/Multiprotocol/Multiprotocol.ino @@ -490,7 +490,9 @@ void Update_All() #endif //ENABLE_PPM update_channels_aux(); #if defined(TELEMETRY) + #if !defined(MULTI_TELEMETRY) if((protocol==MODE_FRSKYD) || (protocol==MODE_BAYANG) || (protocol==MODE_HUBSAN) || (protocol==MODE_AFHDS2A) || (protocol==MODE_FRSKYX) || (protocol==MODE_DSM) ) + #endif TelemetryUpdate(); #endif update_led_status(); diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino index ac986c6..2abf2a7 100644 --- a/Multiprotocol/Telemetry.ino +++ b/Multiprotocol/Telemetry.ino @@ -17,8 +17,13 @@ //************************** #if defined TELEMETRY +#if defined MULTI_TELEMETRY + #define MULTI_TIME 250 //in ms + uint32_t lastMulti = 0; +#endif + #if defined SPORT_TELEMETRY - #define SPORT_TIME 12000 + #define SPORT_TIME 12000 //12ms #define FRSKY_SPORT_PACKET_SIZE 8 uint32_t last = 0; uint8_t sport_counter=0; @@ -52,7 +57,62 @@ uint8_t frame[18]; } SerialControl ; #endif +#ifdef MULTI_TELEMETRY +static void multi_send_header(uint8_t type, uint8_t len) +{ + Serial_write('M'); + Serial_write('P'); + Serial_write(type); + Serial_write(len); +} + +static void multi_send_frskyhub() +{ + multi_send_header(MULTI_TELEMETRY_HUB, 9); + for (uint8_t i = 0; i < 9; i++) + Serial_write(frame[i]); +} + +static void multi_send_status() +{ + multi_send_header(MULTI_TELEMETRY_STATUS, 5); + + // Build flags + uint8_t flags; + if (millis()-last_signal<70) + flags |= 0x01; + if (mode_select==MODE_SERIAL) + flags |= 0x02; + if (remote_callback != 0) + flags |= 0x04; + if (!IS_BIND_DONE_on) + flags |= 0x08; + Serial_write(flags); + + // Version number: 1.15.0 + Serial_write(1); + Serial_write(15); + Serial_write(0); + Serial_write(0); +} +#endif + #ifdef DSM_TELEMETRY +#if defined MULTI_TELEMETRY +void DSM_frame() +{ + if (pkt[0] == 0x80) { + multi_send_header(MULTI_TELEMETRY_DSMBIND, 10); + for (uint8_t i = 1; i < 11; i++) // 10 byte of DSM bind response + Serial_write(pkt[i]); + + } else { + multi_send_header(MULTI_TELEMETRY_DSM, 17); + for (uint8_t i = 0; i < 29; i++) // RSSI value followed by 4*7 bytes of telemetry data + Serial_write(pkt[i]); + } +} +#else void DSM_frame() { Serial_write(0xAA); // Telemetry packet @@ -60,11 +120,16 @@ void DSM_frame() Serial_write(pkt[i]); } #endif +#endif #ifdef AFHDS2A_FW_TELEMETRY void AFHDSA_short_frame() { - Serial_write(0xAA); // Telemetry packet +#if defined MULTI_TELEMETRY + multi_send_header(MULTI_TELEMETRY_AFHDS2A, 29); +#else + Serial_write(0xAA); // Telemetry packet +#endif for (uint8_t i = 0; i < 29; i++) // RSSI value followed by 4*7 bytes of telemetry data Serial_write(pkt[i]); } @@ -157,7 +222,11 @@ void frsky_link_frame() frame[4] = TX_RSSI; } frame[5] = frame[6] = frame[7] = frame[8] = 0; +#if defined MULTI_TELEMETRY + multi_send_frskyhub(); +#else frskySendStuffed(); +#endif } #if defined HUB_TELEMETRY @@ -223,7 +292,11 @@ void frsky_user_frame() if(!indx) return; frame[1] = indx; +#if defined MULTI_TELEMETRY + multi_send_frskyhub(); +#else frskySendStuffed(); +#endif } else pass=0; @@ -294,6 +367,28 @@ pkt[6]|(counter++)|00 01 02 03 04 05 06 07 08 09 0x34 0x0A 0xC3 0x56 0xF3 */ +#ifdef MULTI_TELEMETRY +void sportSend(uint8_t *p) +{ + multi_send_header(MULTI_TELEMETRY_SPORT, 9); + uint16_t crc_s = 0; + Serial_write(p[0]) ; + for (uint8_t i = 1; i < 9; i++) + { + if (i == 8) + p[i] = 0xff - crc_s; + Serial_write(p[i]); + + if (i>0) + { + crc_s += p[i]; //0-1FF + crc_s += crc_s >> 8; //0-100 + crc_s &= 0x00ff; + } + } +} + +#else void sportSend(uint8_t *p) { uint16_t crc_s = 0; @@ -320,10 +415,13 @@ void sportSend(uint8_t *p) } } } +#endif void sportIdle() { +#if !defined MULTI_TELEMETRY Serial_write(START_STOP); +#endif } void sportSendFrame() @@ -468,6 +566,16 @@ void TelemetryUpdate() return ; } #endif + #if defined MULTI_TELEMETRY + { + uint32_t now = millis(); + if ((now - lastMulti) > MULTI_TIME) { + multi_send_status(); + lastMulti = now; + return; + } + } + #endif #if defined SPORT_TELEMETRY if (protocol==MODE_FRSKYX) diff --git a/Multiprotocol/_Config.h b/Multiprotocol/_Config.h index 2f19578..6a55657 100644 --- a/Multiprotocol/_Config.h +++ b/Multiprotocol/_Config.h @@ -119,7 +119,7 @@ #define AFHDS2A_HUB_TELEMETRY // Use FrSkyD Hub format to send telemetry to TX #define BAYANG_HUB_TELEMETRY // Use FrSkyD Hub format to send telemetry to TX #define HUBSAN_HUB_TELEMETRY // Use FrSkyD Hub format to send telemetry to TX - +#define MULTI_TELEMETRY // Send also Multi status and wrap other telemetry to allow TX to autodetect the format /****************************/ /*** SERIAL MODE SETTINGS ***/