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

FrSkyX: improve SPort to RX code

Browse Source
This commit is contained in:
Pascal Langer 2020-04-07 01:43:05 +02:00
parent 272d2be3ae
commit 3f652fa06c
2 changed files with 33 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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 0 #define VERSION_REVISION 0
#define VERSION_PATCH_LEVEL 82 #define VERSION_PATCH_LEVEL 83
//****************** //******************
// Protocols // Protocols

56
Multiprotocol/Multiprotocol.ino
View File

@ -1792,33 +1792,41 @@ void update_serial_data()
#define BYTE_STUFF 0x7D #define BYTE_STUFF 0x7D
#define STUFF_MASK 0x20 #define STUFF_MASK 0x20
//debug("SPort_in: "); //debug("SPort_in: ");
SportData[SportTail]=0x7E; boolean sport_valid=false;
SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
SportData[SportTail]=rx_ok_buff[27]&0x1F;
SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
for(uint8_t i=28;i<28+7;i++) for(uint8_t i=28;i<28+7;i++)
if(rx_ok_buff[i]!=0) sport_valid=true; //Check that the payload is not full of 0
if(rx_ok_buff[27]&0x1F > 0x1B) //Check 1st byte validity
sport_valid=false;
if(sport_valid)
{ {
if(rx_ok_buff[i]==BYTE_STUFF) SportData[SportTail]=0x7E;
{//stuff
SportData[SportTail]=BYTE_STUFF;
SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
SportData[SportTail]=rx_ok_buff[i]^STUFF_MASK;
}
else
SportData[SportTail]=rx_ok_buff[i];
//debug("%02X ",SportData[SportTail]);
SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1); SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
} SportData[SportTail]=rx_ok_buff[27]&0x1F;
uint8_t used = SportTail; SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
if ( SportHead > SportTail ) for(uint8_t i=28;i<28+7;i++)
used += MAX_SPORT_BUFFER - SportHead ; {
else if( (rx_ok_buff[i]==BYTE_STUFF) || (rx_ok_buff[i]==0x7E) )
used -= SportHead ; {//stuff
if ( used >= MAX_SPORT_BUFFER-(MAX_SPORT_BUFFER>>2) ) SportData[SportTail]=BYTE_STUFF;
{ SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
DATA_BUFFER_LOW_on; SportData[SportTail]=rx_ok_buff[i]^STUFF_MASK;
SEND_MULTI_STATUS_on; //Send Multi Status ASAP to inform the TX }
debugln("Low buf=%d,h=%d,t=%d",used,SportHead,SportTail); else
SportData[SportTail]=rx_ok_buff[i];
//debug("%02X ",SportData[SportTail]);
SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
}
uint8_t used = SportTail;
if ( SportHead > SportTail )
used += MAX_SPORT_BUFFER - SportHead ;
else
used -= SportHead ;
if ( used >= MAX_SPORT_BUFFER-(MAX_SPORT_BUFFER>>2) )
{
DATA_BUFFER_LOW_on;
SEND_MULTI_STATUS_on; //Send Multi Status ASAP to inform the TX
debugln("Low buf=%d,h=%d,t=%d",used,SportHead,SportTail);
}
} }
} }
#endif //SPORT_SEND #endif //SPORT_SEND