From ac71b4d98122a3b219764044cf392f119fd01bc5 Mon Sep 17 00:00:00 2001 From: Pascal Langer Date: Mon, 1 Mar 2021 14:02:39 +0100 Subject: [PATCH] Update Devo_cyrf6936.ino --- Multiprotocol/Devo_cyrf6936.ino | 100 +++++++++++++++++--------------- 1 file changed, 53 insertions(+), 47 deletions(-) diff --git a/Multiprotocol/Devo_cyrf6936.ino b/Multiprotocol/Devo_cyrf6936.ino index 7ab125f..a1fea87 100644 --- a/Multiprotocol/Devo_cyrf6936.ino +++ b/Multiprotocol/Devo_cyrf6936.ino @@ -192,8 +192,6 @@ static void __attribute__((unused)) DEVO_float_to_ints(uint8_t *ptr, uint16_t *v } } -extern void frsky_send_user_frame(uint8_t, uint8_t, uint8_t); - static void __attribute__((unused)) DEVO_parse_telemetry_packet() { DEVO_scramble_pkt(); //This will unscramble the packet @@ -211,53 +209,61 @@ static void __attribute__((unused)) DEVO_parse_telemetry_packet() debug("P[0]=%02X",packet[0]); //Telemetry https://github.com/DeviationTX/deviation/blob/5efb6a28bea697af9a61b5a0ed2528cc8d203f90/src/protocol/devo_cyrf6936.c#L232 - uint16_t val, dec; - switch(packet[0]) - { - case 0x30: // Volt and RPM packet + #if defined HUB_TELEMETRY + uint16_t val, dec; + switch(packet[0]) + { + 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; + } + #else + if(packet[0] == 0x30) + { 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 } #endif