gomaomao/DIY-Multiprotocol-TX-Module
1
0
Fork 0
mirror of https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git synced 2025-02-04 22:58:10 +00:00
Code Issues Projects Releases Wiki Activity

DEVO: improve telemetry

Browse Source
This commit is contained in:
Pascal Langer 2021-03-02 16:14:14 +01:00
parent 3d4ed5ad11
commit 44d362aeb4
3 changed files with 58 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
Multiprotocol/Devo_cyrf6936.ino
View File

@ -183,8 +183,12 @@ static void __attribute__((unused)) DEVO_float_to_ints(uint8_t *ptr, uint16_t *v
seen_decimal = true; seen_decimal = true;
continue; continue;
} }
if(ptr[i] == 0) if(ptr[i] < '0' || ptr[i] > '9')
break; {
if(value!=0 || seen_decimal)
break;
continue;
}
if(seen_decimal) if(seen_decimal)
*decimal = *decimal * 10 + (ptr[i] - '0'); *decimal = *decimal * 10 + (ptr[i] - '0');
else else
@ -193,20 +197,29 @@ static void __attribute__((unused)) DEVO_float_to_ints(uint8_t *ptr, uint16_t *v
} }
static void __attribute__((unused)) DEVO_parse_telemetry_packet() static void __attribute__((unused)) DEVO_parse_telemetry_packet()
{ { // Telemetry packets every 2.4ms
DEVO_scramble_pkt(); //This will unscramble the packet DEVO_scramble_pkt(); //This will unscramble the packet
debugln("RX"); debugln("RX");
if ((((uint32_t)packet[15] << 16) | ((uint32_t)packet[14] << 8) | packet[13]) != (MProtocol_id & 0x00ffffff)) if ((((uint32_t)packet[15] << 16) | ((uint32_t)packet[14] << 8) | packet[13]) != (MProtocol_id & 0x00ffffff))
return; // ID does not match return; // ID does not match
if((telemetry_link & 3) != 0)
{
debugln("S%d",telemetry_link);
return; // Previous telemetry not sent yet...
}
//RSSI //RSSI
TX_RSSI = CYRF_ReadRegister(CYRF_13_RSSI) & 0x1F; TX_RSSI = CYRF_ReadRegister(CYRF_13_RSSI) & 0x1F;
TX_RSSI = (TX_RSSI << 1) + TX_RSSI; TX_RSSI = (TX_RSSI << 1) + TX_RSSI;
RX_RSSI = TX_RSSI; RX_RSSI = TX_RSSI;
telemetry_link = 1; telemetry_link |= 1;
debug("P[0]=%02X",packet[0]); //debug
//for(uint8_t i=0;i<12;i++)
// debug("%02X ",packet[i]);
//debugln("");
#if defined HUB_TELEMETRY #if defined HUB_TELEMETRY
//Telemetry https://github.com/DeviationTX/deviation/blob/5efb6a28bea697af9a61b5a0ed2528cc8d203f90/src/protocol/devo_cyrf6936.c#L232 //Telemetry https://github.com/DeviationTX/deviation/blob/5efb6a28bea697af9a61b5a0ed2528cc8d203f90/src/protocol/devo_cyrf6936.c#L232
@ -216,48 +229,67 @@ static void __attribute__((unused)) DEVO_parse_telemetry_packet()
case 0x30: // Volt and RPM packet case 0x30: // Volt and RPM packet
v_lipo1 = packet[1] << 1; v_lipo1 = packet[1] << 1;
v_lipo2 = packet[3] << 1; v_lipo2 = packet[3] << 1;
val = packet[5]/10; //packet[5] = 127; // 12.7V
frsky_send_user_frame(0x3A, val, 0x00); // volt3 if(packet[5] != 0)
frsky_send_user_frame(0x3B, packet[5] - val*10, 0x00); // volt3 {
val = packet[7] * 120; //In RPM val = (packet[5]*11)/21; // OpenTX strange transformation??
frsky_send_user_frame(0x03, val, val>>8); // RPM dec = val;
val /= 10;
dec -= val*10;
frsky_send_user_frame(0x3A, val, 0x00); // volt3
frsky_send_user_frame(0x3B, dec, 0x00); // volt3
}
if(packet[7] != 0)
{
val = packet[7] * 120; // change to RPM
frsky_send_user_frame(0x03, val, val>>8); // RPM
}
break; break;
case 0x31: // Temperature packet case 0x31: // Temperature packet
//memcpy(&packet[1],"\x15\x16\x17\x00\x00\x00\x00\x00\x00\x00\x00\x00",12); // guess 21°, 22°, 23°
if(packet[1]!=0xff) if(packet[1]!=0xff)
frsky_send_user_frame(0x02, packet[1], 0x00); // temp1 frsky_send_user_frame(0x02, packet[1], 0x00); // temp1
if(packet[2]!=0xff) if(packet[2]!=0xff)
frsky_send_user_frame(0x05, packet[2], 0x00); // temp2 frsky_send_user_frame(0x05, packet[2], 0x00); // temp2
break; break;
// GPS Data // GPS Data
case 0x32: // Longitude case 0x32: // Longitude
val = DEVO_text_to_int(&packet[1], 3)*100 + DEVO_text_to_int(&packet[4], 2); // dddmm //memcpy(&packet[1],"\x30\x33\x30\x32\x30\x2e\x38\x32\x37\x30\x45\xfb",12); // 030°20.8270E
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); frsky_send_user_frame(0x12 , val, val>>8);
val = DEVO_text_to_int(&packet[7], 4); // .mmmm val = DEVO_text_to_int(&packet[7], 4); // .mmmm
frsky_send_user_frame(0x12+8, val, val>>8); frsky_send_user_frame(0x12+8, val, val>>8);
frsky_send_user_frame(0x1A+8, packet[11], 0x00); // 'E'/'W' frsky_send_user_frame(0x1A+8, packet[11], 0x00); // 'E'/'W'
break; break;
case 0x33: // Latitude case 0x33: // Latitude
val = DEVO_text_to_int(&packet[1], 2)*100 + DEVO_text_to_int(&packet[3], 2); // ddmm //memcpy(&packet[1],"\x35\x39\x35\x34\x2e\x37\x37\x37\x36\x4e\x07\x00",12); // 59°54.776N
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); frsky_send_user_frame(0x13 , val, val>>8);
val = DEVO_text_to_int(&packet[6], 4); // .mmmm val = DEVO_text_to_int(&packet[6], 4); // .mmmm
frsky_send_user_frame(0x13+8, val, val>>8); frsky_send_user_frame(0x13+8, val, val>>8);
frsky_send_user_frame(0x1B+8, packet[10], 0x00); // 'N'/'S' frsky_send_user_frame(0x1B+8, packet[10], 0x00); // 'N'/'S'
break; break;
case 0x34: // Altitude case 0x34: // Altitude
//memcpy(&packet[1],"\x31\x32\x2e\x38\x00\x00\x00\x4d\x4d\x4e\x45\xfb",12); // 12.8 MMNE
DEVO_float_to_ints(&packet[1], &val, &dec); DEVO_float_to_ints(&packet[1], &val, &dec);
frsky_send_user_frame(0x10, val, val>>8); frsky_send_user_frame(0x10, val, val>>8);
frsky_send_user_frame(0x21, dec, dec>>8); frsky_send_user_frame(0x21, dec, dec>>8);
break; break;
case 0x35: // Speed case 0x35: // Speed
DEVO_float_to_ints(&packet[7], &val, &dec); //memcpy(&packet[1],"\x00\x00\x00\x00\x00\x00\x30\x2e\x30\x30\x00\x00",12); // 0.0
DEVO_float_to_ints(&packet[1], &val, &dec);
frsky_send_user_frame(0x11 , val, val>>8); frsky_send_user_frame(0x11 , val, val>>8);
frsky_send_user_frame(0x11+8, dec, dec>>8); frsky_send_user_frame(0x11+8, dec, dec>>8);
break; break;
case 0x36: // Time case 0x36: // Time
frsky_send_user_frame(0x15, DEVO_text_to_int(&packet[9], 2), DEVO_text_to_int(&packet[7], 2)); // month, day //memcpy(&packet[1],"\x31\x38\x32\x35\x35\x32\x31\x35\x31\x30\x31\x32",12); // 2012-10-15 18:25:52 (UTC)
frsky_send_user_frame(0x16, DEVO_text_to_int(&packet[11], 2), 0x00); // year if(packet[1]!=0)
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 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)+24, 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; break;
} }
#else #else

2
Multiprotocol/Multiprotocol.h
View File

@ -19,7 +19,7 @@
#define VERSION_MAJOR 1 #define VERSION_MAJOR 1
#define VERSION_MINOR 3 #define VERSION_MINOR 3
#define VERSION_REVISION 2 #define VERSION_REVISION 2
#define VERSION_PATCH_LEVEL 54 #define VERSION_PATCH_LEVEL 55
#define MODE_SERIAL 0 #define MODE_SERIAL 0

2
Multiprotocol/Telemetry.ino
View File

@ -637,7 +637,7 @@ static void __attribute__((unused)) frsky_send_user_frame(uint8_t ID, uint8_t lo
static void __attribute__((unused)) frsky_check_user_frame() static void __attribute__((unused)) frsky_check_user_frame()
{ {
if(telemetry_link&2 || FrSkyD_User_Frame_Start == FrSkyD_User_Frame_End) 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 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); 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++;