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

S-FHSS: fix failsafe

Browse Source
This commit is contained in:
Pascal Langer 2017-11-22 14:51:45 +01:00
parent 76e9002995
commit bd9c772a52
2 changed files with 14 additions and 14 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 1 #define VERSION_MINOR 1
#define VERSION_REVISION 6 #define VERSION_REVISION 6
#define VERSION_PATCH_LEVEL 26 #define VERSION_PATCH_LEVEL 27
//****************** //******************
// Protocols // Protocols
//****************** //******************

20
Multiprotocol/SFHSS_cc2500.ino
View File

@ -148,20 +148,20 @@ static void __attribute__((unused)) SFHSS_build_data_packet()
command|=0x02; // Assuming packet[0] == 0x81 command|=0x02; // Assuming packet[0] == 0x81
counter&=0x3FF; // Reset failsafe counter counter&=0x3FF; // Reset failsafe counter
if(counter&1) command|=0x08; // Transmit lower and upper channels twice in a row if(counter&1) command|=0x08; // Transmit lower and upper channels twice in a row
if(command&0x04)
{//Failsafe data uint8_t ch_offset = ((command&0x08) >> 1) | ((command&0x04) << 1); // CH1..CH4 or CH5..CH8, if failsafe CH9..CH12 or CH13..CH16
ch1=0x400; // Centered
ch2=0x400; // Centered
ch3=(command&0x08)?0x400:0xC00; // Centered or zero if throttle channel
ch4=0x400; // Centered
}
else
{//Normal data
uint8_t ch_offset = (command&0x08) >> 1; // CH1..CH4 or CH5..CH8
ch1 = convert_channel_16b_nolim(CH_AETR[ch_offset+0],2020,1020); ch1 = convert_channel_16b_nolim(CH_AETR[ch_offset+0],2020,1020);
ch2 = convert_channel_16b_nolim(CH_AETR[ch_offset+1],2020,1020); ch2 = convert_channel_16b_nolim(CH_AETR[ch_offset+1],2020,1020);
ch3 = convert_channel_16b_nolim(CH_AETR[ch_offset+2],2020,1020); ch3 = convert_channel_16b_nolim(CH_AETR[ch_offset+2],2020,1020);
ch4 = convert_channel_16b_nolim(CH_AETR[ch_offset+3],2020,1020); ch4 = convert_channel_16b_nolim(CH_AETR[ch_offset+3],2020,1020);
if(command&0x04)
{//Failsafe data are coded for sbus: (ch_value-880)/0.625
ch1=((ch1-880)<<3)/5;
ch2=((ch2-880)<<3)/5;
ch3=((ch3-880)<<3)/5;
if((command&0x08)==0) ch3|=0x800; // Special flag for throttle which appears on dumps...
ch4=((ch4-880)<<3)/5;
} }
// XK [0]=0x81 [3]=0x00 [4]=0x00 // XK [0]=0x81 [3]=0x00 [4]=0x00