From d36af55b846baaaa1232de88832befdf1029a608 Mon Sep 17 00:00:00 2001 From: Pascal Langer Date: Mon, 1 Mar 2021 13:47:44 +0100 Subject: [PATCH] DEVO: full telemetry --- Multiprotocol/Devo_cyrf6936.ino | 80 ++++++++++++++++++++++++++++++--- Multiprotocol/Multi_Protos.ino | 3 ++ Multiprotocol/Multiprotocol.h | 7 ++- Multiprotocol/Multiprotocol.ino | 11 +++++ Multiprotocol/Telemetry.ino | 64 +++++++++++++++++++++++--- 5 files changed, 153 insertions(+), 12 deletions(-) diff --git a/Multiprotocol/Devo_cyrf6936.ino b/Multiprotocol/Devo_cyrf6936.ino index 369f687..0c7e062 100644 --- a/Multiprotocol/Devo_cyrf6936.ino +++ b/Multiprotocol/Devo_cyrf6936.ino @@ -163,6 +163,35 @@ static void __attribute__((unused)) DEVO_build_data_pkt() } #if defined DEVO_HUB_TELEMETRY +static uint32_t __attribute__((unused)) DEVO_text_to_int(uint8_t *ptr, uint8_t len) +{ + uint32_t value = 0; + for(uint8_t i = 0; i < len; i++) + value = value * 10 + (ptr[i] - '0'); + return value; +} + +static void __attribute__((unused)) DEVO_float_to_ints(uint8_t *ptr, uint16_t *value, uint16_t *decimal) +{ + bool seen_decimal = false; + *value = 0; + *decimal = 0; + for(uint8_t i = 0; i < 7; i++) + { + if(ptr[i] == '.') + { + seen_decimal = true; + continue; + } + if(ptr[i] == 0) + break; + if(seen_decimal) + *decimal = *decimal * 10 + (ptr[i] - '0'); + else + *value = *value * 10 + (ptr[i] - '0'); + } +} + static void __attribute__((unused)) DEVO_parse_telemetry_packet() { DEVO_scramble_pkt(); //This will unscramble the packet @@ -177,14 +206,55 @@ static void __attribute__((unused)) DEVO_parse_telemetry_packet() RX_RSSI = TX_RSSI; telemetry_link = 1; - //TODO: FW telemetry https://github.com/DeviationTX/deviation/blob/5efb6a28bea697af9a61b5a0ed2528cc8d203f90/src/protocol/devo_cyrf6936.c#L232 - debug("P[0]=%02X",packet[0]); - if (packet[0] == 0x30) // Volt packet + //Telemetry https://github.com/DeviationTX/deviation/blob/5efb6a28bea697af9a61b5a0ed2528cc8d203f90/src/protocol/devo_cyrf6936.c#L232 + uint16_t val, dec; + switch(packet[0]) { - v_lipo1 = packet[1] << 1; - v_lipo2 = packet[3] << 1; + case 0x30: // Volt and RPM packet + v_lipo1 = packet[1] << 1; + v_lipo2 = packet[3] << 1; + val = packet[7] * 120; //In RPM + frsky_send_user_frame(0x03, val, val>>8); // RPM + break; + case 0x31: // Temperature packet + if(packet[1]!=0xff) + frsky_send_user_frame(0x02, packet[1], 0x00); // temp1 + if(packet[2]!=0xff) + frsky_send_user_frame(0x05, packet[2], 0x00); // temp2 + break; + // GPS Data + case 0x32: // Longitude + val = DEVO_text_to_int(&packet[1], 3)*100 + DEVO_text_to_int(&packet[4], 2); // dddmm + frsky_send_user_frame(0x12 , val, val>>8); + val = DEVO_text_to_int(&packet[7], 4); // .mmmm + frsky_send_user_frame(0x12+8, val, val>>8); + frsky_send_user_frame(0x1A+8, packet[11], 0x00); // 'E'/'W' + break; + case 0x33: // Latitude + val = DEVO_text_to_int(&packet[1], 2)*100 + DEVO_text_to_int(&packet[3], 2); // ddmm + frsky_send_user_frame(0x13 , val, val>>8); + val = DEVO_text_to_int(&packet[6], 4); // .mmmm + frsky_send_user_frame(0x13+8, val, val>>8); + frsky_send_user_frame(0x1B+8, packet[10], 0x00); // 'N'/'S' + break; + case 0x34: // Altitude + DEVO_float_to_ints(&packet[1], &val, &dec); + frsky_send_user_frame(0x10, val, val>>8); + frsky_send_user_frame(0x21, dec, dec>>8); + break; + case 0x35: // Speed + DEVO_float_to_ints(&packet[7], &val, &dec); + frsky_send_user_frame(0x11 , val, val>>8); + frsky_send_user_frame(0x11+8, dec, dec>>8); + break; + case 0x36: // Time + frsky_send_user_frame(0x15, DEVO_text_to_int(&packet[9], 2), DEVO_text_to_int(&packet[7], 2)); // month, day + frsky_send_user_frame(0x16, DEVO_text_to_int(&packet[11], 2), 0x00); // year + frsky_send_user_frame(0x17, DEVO_text_to_int(&packet[1], 2), DEVO_text_to_int(&packet[3], 2)); // hour, min + frsky_send_user_frame(0x18, DEVO_text_to_int(&packet[5], 2), 0x00); // second + break; } } #endif diff --git a/Multiprotocol/Multi_Protos.ino b/Multiprotocol/Multi_Protos.ino index 5a70c5a..e50ebac 100644 --- a/Multiprotocol/Multi_Protos.ino +++ b/Multiprotocol/Multi_Protos.ino @@ -173,6 +173,9 @@ enum const mm_protocol_definition multi_protocols[] = { // Protocol number, Protocol String, Sub_protocol strings, Number of sub_protocols, Option type, Failsafe, ChMap, RF switch, Init, Callback + #if defined(MULTI_CONFIG_INO) + {PROTO_CONFIG, STR_CONFIG, NO_SUBTYPE, 0, OPTION_NONE, 0, 0, 0, CONFIG_init, CONFIG_callback }, + #endif #if defined(ASSAN_NRF24L01_INO) {PROTO_ASSAN, STR_ASSAN, NO_SUBTYPE, 0, OPTION_NONE, 0, 0, SW_NRF, ASSAN_init, ASSAN_callback }, #endif diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index 15a02f1..bc65c4a 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -19,14 +19,16 @@ #define VERSION_MAJOR 1 #define VERSION_MINOR 3 #define VERSION_REVISION 2 -#define VERSION_PATCH_LEVEL 51 +#define VERSION_PATCH_LEVEL 52 + +#define MODE_SERIAL 0 //****************** // Protocols //****************** enum PROTOCOLS { - MODE_SERIAL = 0, // Serial commands + PROTO_CONFIG = 0, // Module config PROTO_FLYSKY = 1, // =>A7105 PROTO_HUBSAN = 2, // =>A7105 PROTO_FRSKYD = 3, // =>CC2500 @@ -489,6 +491,7 @@ enum MultiPacketTypes MULTI_TELEMETRY_RX_CHANNELS = 13, MULTI_TELEMETRY_HOTT = 14, MULTI_TELEMETRY_MLINK = 15, + MULTI_TELEMETRY_CONFIG = 16, }; // Macros diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino index 46532ed..73addee 100644 --- a/Multiprotocol/Multiprotocol.ino +++ b/Multiprotocol/Multiprotocol.ino @@ -257,6 +257,10 @@ uint8_t packet_in[TELEMETRY_BUFFER_SIZE];//telemetry receiving packets uint8_t DSM_SerialRX_val[7]; bool DSM_SerialRX=false; #endif + #ifdef MULTI_CONFIG_INO + uint8_t CONFIG_SerialRX_val[7]; + bool CONFIG_SerialRX=false; + #endif #endif // TELEMETRY uint8_t multi_protocols_index=0xFF; @@ -1503,6 +1507,13 @@ void update_serial_data() DSM_SerialRX=true; } #endif + #ifdef MULTI_CONFIG_INO + if(protocol==PROTO_CONFIG && rx_len==27+7) + {//Protocol waiting for 7 bytes + memcpy(CONFIG_SerialRX_val, (const void *)&rx_ok_buff[27],7); + CONFIG_SerialRX=true; + } + #endif } RX_DONOTUPDATE_off; diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino index bfac28d..03519df 100644 --- a/Multiprotocol/Telemetry.ino +++ b/Multiprotocol/Telemetry.ino @@ -53,6 +53,14 @@ uint8_t RetrySequence ; #if defined HUB_TELEMETRY #define USER_MAX_BYTES 6 uint8_t prev_index; + + struct t_FrSkyD_User_Frame + { + uint8_t ID; + uint8_t low; + uint8_t high; + } FrSkyD_User_Frame[8]; + uint8_t FrSkyD_User_Frame_Start=0, FrSkyD_User_Frame_End=0; #endif // HUB_TELEMETRY #define START_STOP 0x7e @@ -223,6 +231,15 @@ static void multi_send_status() } } +#ifdef MULTI_CONFIG_INO + void CONFIG_frame() + { + multi_send_header(MULTI_TELEMETRY_CONFIG, packet_in[0]); + for (uint8_t i = 1; i <= packet_in[0]; i++) // config data + Serial_write(packet_in[i]); + } +#endif + #ifdef MLINK_FW_TELEMETRY void MLINK_frame() { @@ -498,6 +515,9 @@ void init_frskyd_link_telemetry() TX_RSSI=0; RX_LQI=0; TX_LQI=0; + #if defined HUB_TELEMETRY + FrSkyD_User_Frame_Start=FrSkyD_User_Frame_End=0; + #endif } void frsky_link_frame() @@ -575,7 +595,7 @@ packet_in[6]|(counter++)|00 01 02 03 04 05 06 07 08 09 0A 0F 5E 3A 06 00 5E 5E 3B 09 00 5E 05 10 5E 06 16 72 5E 5E 3A 06 00 5E */ -static void __attribute__((unused)) frsky_send_user_frame(uint8_t ID, uint8_t low, uint8_t high) +static void __attribute__((unused)) frsky_write_user_frame(uint8_t ID, uint8_t low, uint8_t high) { telemetry_in_buffer[6] = 0x04; // number of bytes in the payload telemetry_in_buffer[7] = 0x00; // unknown? @@ -596,7 +616,32 @@ static void __attribute__((unused)) frsky_send_user_frame(uint8_t ID, uint8_t lo telemetry_in_buffer[pos++] = value; value = high; } - telemetry_link |= 2; // request to send frame + telemetry_link |= 2; // request to send frame +} + +static void __attribute__((unused)) frsky_send_user_frame(uint8_t ID, uint8_t low, uint8_t high) +{ + if(telemetry_link&2) + { // add to buffer + uint8_t test = (FrSkyD_User_Frame_End + 1) & 0x07; + if(test == FrSkyD_User_Frame_Start) + return; // buffer full... + FrSkyD_User_Frame_End = test; + FrSkyD_User_Frame[FrSkyD_User_Frame_End].ID = ID; + FrSkyD_User_Frame[FrSkyD_User_Frame_End].low = low; + FrSkyD_User_Frame[FrSkyD_User_Frame_End].high = high; + } + else // send to TX direct + frsky_write_user_frame(ID, low, high); +} + +static void __attribute__((unused)) frsky_check_user_frame() +{ + if(telemetry_link&2 || FrSkyD_User_Frame_Start == FrSkyD_User_Frame_End) + return; // need to wait that the last frame is sent or buffer is empty + frsky_write_user_frame(FrSkyD_User_Frame[FrSkyD_User_Frame_Start].ID, FrSkyD_User_Frame[FrSkyD_User_Frame_Start].low, FrSkyD_User_Frame[FrSkyD_User_Frame_Start].high); + FrSkyD_User_Frame_Start++; + FrSkyD_User_Frame_Start &= 0x07; } #endif @@ -905,6 +950,14 @@ void TelemetryUpdate() #endif // SPORT_TELEMETRY #ifdef MULTI_TELEMETRY + #if defined MULTI_CONFIG_INO + if(telemetry_link && protocol == PROTO_CONFIG) + { + CONFIG_frame(); + telemetry_link=0; + return; + } + #endif #if defined MLINK_FW_TELEMETRY if(telemetry_link && protocol == PROTO_MLINK) { @@ -964,15 +1017,16 @@ void TelemetryUpdate() #endif //MULTI_TELEMETRY if( telemetry_link & 1 ) - { // FrSkyD + Hubsan + AFHDS2A + Bayang + Cabell + Hitec + Bugs + BugsMini + NCC1701 + PROPEL + RLINK + OMP + MLINK + { // FrSkyD + Hubsan + AFHDS2A + Bayang + Cabell + Hitec + Bugs + BugsMini + NCC1701 + PROPEL + RLINK + OMP + MLINK + DEVO // FrSkyX telemetry if in PPM frsky_link_frame(); return; } #if defined HUB_TELEMETRY - if((telemetry_link & 2) && ( protocol == PROTO_FRSKYD || protocol == PROTO_BAYANG || protocol == PROTO_MLINK ) ) - { // FrSkyD + Bayang + MLINK + if((telemetry_link & 2) && ( protocol == PROTO_FRSKYD || protocol == PROTO_BAYANG || protocol == PROTO_MLINK || protocol == PROTO_DEVO ) ) + { // FrSkyD + Bayang + MLINK + DEVO frsky_user_frame(); + frsky_check_user_frame(); return; } #endif