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:48:12 +00:00
Code Issues Projects Releases Wiki Activity

FrSkyX: fix telem

Browse Source
This commit is contained in:
Pascal Langer 2020-07-04 15:15:55 +02:00
parent e56f737b34
commit 7112f58dae
4 changed files with 14 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Multiprotocol/FrSkyD_cc2500.ino
View File

@ -181,7 +181,7 @@ uint16_t ReadFrSky_2way()
if(packet_in[len-1] & 0x80) if(packet_in[len-1] & 0x80)
{//with valid crc {//with valid crc
packet_count=0; packet_count=0;
frsky_check_telemetry(packet_in,len); //check if valid telemetry packets and buffer them. frsky_process_telemetry(packet_in,len); //check if valid telemetry packets and buffer them.
} }
#endif #endif
} }

2
Multiprotocol/FrSkyR9_sx1276.ino
View File

@ -192,7 +192,7 @@ uint16_t FrSkyR9_callback()
TX_RSSI=SX1276_ReadReg(SX1276_1A_PACKETRSSI)-157; TX_RSSI=SX1276_ReadReg(SX1276_1A_PACKETRSSI)-157;
for(uint8_t i=0;i<9;i++) for(uint8_t i=0;i<9;i++)
packet[4+i]=packet_in[i]; //Adjust buffer to match FrSkyX packet[4+i]=packet_in[i]; //Adjust buffer to match FrSkyX
frsky_check_telemetry(packet,len); //Check and parse telemetry packets frsky_process_telemetry(packet,len); //Check and parse telemetry packets
} }
} }
} }

11
Multiprotocol/FrSkyX_cc2500.ino
View File

@ -280,15 +280,15 @@ uint16_t ReadFrSkyX()
if (len && len <= 17) //Telemetry frame is 17 bytes if (len && len <= 17) //Telemetry frame is 17 bytes
{ {
//debug("Telem:"); //debug("Telem:");
packet_count=0;
CC2500_ReadData(packet_in, len); //Read what has been received so far CC2500_ReadData(packet_in, len); //Read what has been received so far
if(len<17) if(len<17)
{//not all bytes were received {//not all bytes were received
uint8_t last_len=CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; uint8_t last_len=CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
if(last_len==17) //All bytes received if(last_len==17) //All bytes received
{
CC2500_ReadData(packet_in+len, last_len-len); //Finish to read CC2500_ReadData(packet_in+len, last_len-len); //Finish to read
else len=17;
len=0; //Discard frame }
} }
#if defined TELEMETRY #if defined TELEMETRY
if(len==17 && (protocol==PROTO_FRSKYX || (protocol==PROTO_FRSKYX2 && (packet_in[len-1] & 0x80))) ) if(len==17 && (protocol==PROTO_FRSKYX || (protocol==PROTO_FRSKYX2 && (packet_in[len-1] & 0x80))) )
@ -296,7 +296,10 @@ uint16_t ReadFrSkyX()
//Debug //Debug
//for(uint8_t i=0;i<len;i++) //for(uint8_t i=0;i<len;i++)
// debug(" %02X",packet_in[i]); // debug(" %02X",packet_in[i]);
frsky_check_telemetry(packet_in,len); //Check and parse telemetry packets if(frsky_process_telemetry(packet_in,len)) //Check and process telemetry packets
packet_count=0; // good
else
len=0; // bad
} }
#endif #endif
//debugln(""); //debugln("");

9
Multiprotocol/Telemetry.ino
View File

@ -347,12 +347,12 @@ void frskySendStuffed()
Serial_write(START_STOP); Serial_write(START_STOP);
} }
void frsky_check_telemetry(uint8_t *buffer,uint8_t len) bool frsky_process_telemetry(uint8_t *buffer,uint8_t len)
{ {
if(protocol!=PROTO_FRSKY_R9) if(protocol!=PROTO_FRSKY_R9)
{ {
if(buffer[1] != rx_tx_addr[3] || buffer[2] != rx_tx_addr[2] || len != buffer[0] + 3 ) if(buffer[1] != rx_tx_addr[3] || buffer[2] != rx_tx_addr[2] || len != buffer[0] + 3 )
return; // Bad address or length... return false; // Bad address or length...
// RSSI and LQI are the 2 last bytes // RSSI and LQI are the 2 last bytes
TX_RSSI = buffer[len-2]; TX_RSSI = buffer[len-2];
if(TX_RSSI >=128) if(TX_RSSI >=128)
@ -417,7 +417,7 @@ void frsky_check_telemetry(uint8_t *buffer,uint8_t len)
//len=17 -> len-7=10 -> 3..12 //len=17 -> len-7=10 -> 3..12
uint16_t lcrc = FrSkyX_crc(&buffer[3], len-7 ) ; uint16_t lcrc = FrSkyX_crc(&buffer[3], len-7 ) ;
if ( ( (lcrc >> 8) != buffer[len-4]) || ( (lcrc & 0x00FF ) != buffer[len-3]) ) if ( ( (lcrc >> 8) != buffer[len-4]) || ( (lcrc & 0x00FF ) != buffer[len-3]) )
return; // Bad CRC return false; // Bad CRC
if(buffer[4] & 0x80) if(buffer[4] & 0x80)
RX_RSSI=buffer[4] & 0x7F ; RX_RSSI=buffer[4] & 0x7F ;
@ -425,7 +425,7 @@ void frsky_check_telemetry(uint8_t *buffer,uint8_t len)
v_lipo1 = (buffer[4]<<1) + 1 ; v_lipo1 = (buffer[4]<<1) + 1 ;
#if defined(TELEMETRY_FRSKYX_TO_FRSKYD) && defined(ENABLE_PPM) #if defined(TELEMETRY_FRSKYX_TO_FRSKYD) && defined(ENABLE_PPM)
if(mode_select != MODE_SERIAL) if(mode_select != MODE_SERIAL)
return; return true;
#endif #endif
} }
if (protocol==PROTO_FRSKYX||protocol==PROTO_FRSKYX2||protocol==PROTO_FRSKY_R9) if (protocol==PROTO_FRSKYX||protocol==PROTO_FRSKYX2||protocol==PROTO_FRSKY_R9)
@ -503,6 +503,7 @@ void frsky_check_telemetry(uint8_t *buffer,uint8_t len)
} }
} }
#endif #endif
return true;
} }
void init_frskyd_link_telemetry() void init_frskyd_link_telemetry()