gomaomao/DIY-Multiprotocol-TX-Module
1
0
Fork 0
mirror of https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git synced 2025-02-04 19:48:11 +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)
{//with valid crc
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
}

2
Multiprotocol/FrSkyR9_sx1276.ino
View File

@ -192,7 +192,7 @@ uint16_t FrSkyR9_callback()
TX_RSSI=SX1276_ReadReg(SX1276_1A_PACKETRSSI)-157;
for(uint8_t i=0;i<9;i++)
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
{
//debug("Telem:");
packet_count=0;
CC2500_ReadData(packet_in, len); //Read what has been received so far
if(len<17)
{//not all bytes were received
uint8_t last_len=CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
if(last_len==17) //All bytes received
{
CC2500_ReadData(packet_in+len, last_len-len); //Finish to read
else
len=0; //Discard frame
len=17;
}
}
#if defined TELEMETRY
if(len==17 && (protocol==PROTO_FRSKYX || (protocol==PROTO_FRSKYX2 && (packet_in[len-1] & 0x80))) )
@ -296,7 +296,10 @@ uint16_t ReadFrSkyX()
//Debug
//for(uint8_t i=0;i<len;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
//debugln("");

9
Multiprotocol/Telemetry.ino
View File

@ -347,12 +347,12 @@ void frskySendStuffed()
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(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
TX_RSSI = buffer[len-2];
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
uint16_t lcrc = FrSkyX_crc(&buffer[3], len-7 ) ;
if ( ( (lcrc >> 8) != buffer[len-4]) || ( (lcrc & 0x00FF ) != buffer[len-3]) )
return; // Bad CRC
return false; // Bad CRC
if(buffer[4] & 0x80)
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 ;
#if defined(TELEMETRY_FRSKYX_TO_FRSKYD) && defined(ENABLE_PPM)
if(mode_select != MODE_SERIAL)
return;
return true;
#endif
}
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
return true;
}
void init_frskyd_link_telemetry()