diff --git a/Multiprotocol/FrSkyD_cc2500.ino b/Multiprotocol/FrSkyD_cc2500.ino index 8368977..771b6be 100644 --- a/Multiprotocol/FrSkyD_cc2500.ino +++ b/Multiprotocol/FrSkyD_cc2500.ino @@ -96,6 +96,7 @@ static void __attribute__((unused)) frsky2way_data_frame() uint16_t initFrSky_2way() { Frsky_init_hop(); + packet_count=0; if(IS_AUTOBIND_FLAG_on) { frsky2way_init(1); @@ -151,14 +152,29 @@ uint16_t ReadFrSky_2way() if (state == FRSKY_DATA1) { len = CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; - if (len && len<=MAX_PKT)//27 bytes - { - CC2500_ReadData(pkt, len); //received telemetry packets + if (len && len<=(0x11+3))// 20bytes + { + CC2500_ReadData(pkt, len); //received telemetry packets #if defined(TELEMETRY) - //parse telemetry packet here - frsky_check_telemetry(pkt,len); //check if valid telemetry packets and buffer them. - #endif - } + if(pkt[len-1] & 0x80) + {//with valid crc + frsky_check_telemetry(pkt,len); //check if valid telemetry packets and buffer them. + } + #endif + } + else + { + packet_count++; + // restart sequence on missed packet - might need count or timeout instead of one missed + if(packet_count>100) + {//~1sec + packet_count=0; + #if defined TELEMETRY + telemetry_link=0;//no link frames + pkt[6]=0;//no user frames. + #endif + } + } CC2500_SetTxRxMode(TX_EN); CC2500_SetPower(); // Set tx_power }