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

FrSkyX failsafe

Browse Source
This commit is contained in:
Pascal Langer 2017-12-10 09:48:20 +01:00
parent 6ae819e8d5
commit 880e463b01
5 changed files with 78 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Multiprotocol/AFHDS2A_a7105.ino
View File

@ -198,7 +198,7 @@ static void AFHDS2A_build_packet(uint8_t type)
packet[0] = 0xaa; packet[0] = 0xaa;
packet[9] = 0xfd; packet[9] = 0xfd;
packet[10]= 0xff; packet[10]= 0xff;
uint16_t val_hz=5*(option & 0x7f)+50; // option value should be between 0 and 70 which gives a value between 50 and 400Hz uint16_t val_hz=5*(option & 0x7f)+50; // option value should be between 0 and 70 which gives a value between 50 and 400Hz
if(val_hz<50 || val_hz>400) val_hz=50; // default is 50Hz if(val_hz<50 || val_hz>400) val_hz=50; // default is 50Hz
packet[11]= val_hz; packet[11]= val_hz;
packet[12]= val_hz >> 8; packet[12]= val_hz >> 8;

95
Multiprotocol/FrSkyX_cc2500.ino
View File

@ -74,14 +74,6 @@ static uint16_t __attribute__((unused)) frskyX_crc_x(uint8_t *data, uint8_t len)
return crc; return crc;
} }
// 0-2047, 0 = 817, 1024 = 1500, 2047 = 2182
//64=860,1024=1500,1984=2140//Taranis 125%
static uint16_t __attribute__((unused)) frskyX_scaleForPXX( uint8_t i )
{ //mapped 860,2140(125%) range to 64,1984(PXX values);
return (uint16_t)(((Servo_data[i]-servo_min_125)*3)>>1)+64;
}
static void __attribute__((unused)) frskyX_build_bind_packet() static void __attribute__((unused)) frskyX_build_bind_packet()
{ {
packet[0] = (sub_protocol & 2 ) ? 0x20 : 0x1D ; // LBT or FCC packet[0] = (sub_protocol & 2 ) ? 0x20 : 0x1D ; // LBT or FCC
@ -109,16 +101,57 @@ static void __attribute__((unused)) frskyX_build_bind_packet()
// //
} }
// 0-2047, 0 = 817, 1024 = 1500, 2047 = 2182
//64=860,1024=1500,1984=2140//Taranis 125%
static uint16_t __attribute__((unused)) frskyX_scaleForPXX( uint8_t i )
{ //mapped 860,2140(125%) range to 64,1984(PXX values);
uint16_t chan_val=(((Servo_data[i]-servo_min_125)*3)>>1)+64;
if(i>7) chan_val|=2048; // upper channels offset
return chan_val;
}
#ifdef FAILSAFE_ENABLE
static uint16_t __attribute__((unused)) frskyX_scaleForPXX_FS( uint8_t i )
{ //mapped 1,2046(125%) range to 64,1984(PXX values);
uint16_t chan_val=((Failsafe_data[i]*15)>>4)+64;
if(Failsafe_data[i]==FAILSAFE_CHANNEL_NOPULSES)
chan_val=FAILSAFE_CHANNEL_NOPULSES;
else if(Failsafe_data[i]==FAILSAFE_CHANNEL_HOLD)
chan_val=FAILSAFE_CHANNEL_HOLD;
if(i>7) chan_val|=2048; // upper channels offset
return chan_val;
}
#endif
#define FRX_FAILSAFE_TIME 1032
static void __attribute__((unused)) frskyX_data_frame() static void __attribute__((unused)) frskyX_data_frame()
{ {
//0x1D 0xB3 0xFD 0x02 0x56 0x07 0x15 0x00 0x00 0x00 0x04 0x40 0x00 0x04 0x40 0x00 0x04 0x40 0x00 0x04 0x40 0x08 0x00 0x00 0x00 0x00 0x00 0x00 0x96 0x12 //0x1D 0xB3 0xFD 0x02 0x56 0x07 0x15 0x00 0x00 0x00 0x04 0x40 0x00 0x04 0x40 0x00 0x04 0x40 0x00 0x04 0x40 0x08 0x00 0x00 0x00 0x00 0x00 0x00 0x96 0x12
// //
static uint8_t lpass; static uint8_t chan_offset=0;
uint16_t chan_0 ; uint16_t chan_0 ;
uint16_t chan_1 ; uint16_t chan_1 ;
uint8_t startChan = 0;
// //
packet[0] = (sub_protocol & 2 ) ? 0x20 : 0x1D ; // LBT or FCC // data frames sent every 9ms; failsafe every 9 seconds
#ifdef FAILSAFE_ENABLE
static uint16_t failsafe_count=0;
static uint8_t FS_flag=0,failsafe_chan=0;
if (FS_flag == 0 && failsafe_count > FRX_FAILSAFE_TIME && chan_offset == 0 && IS_FAILSAFE_VALUES_on)
{
FS_flag = 0x10;
failsafe_chan = 0;
} else if (FS_flag & 0x10 && failsafe_chan < (sub_protocol & 0x01 ? 8-1:16-1))
{
FS_flag = 0x10 | ((FS_flag + 2) & 0x0F); //10, 12, 14, 16, 18, 1A, 1C, 1E - failsafe packet
failsafe_chan ++;
} else if (FS_flag & 0x10)
{
FS_flag = 0;
failsafe_count = 0;
}
failsafe_count++;
#endif
packet[0] = (sub_protocol & 0x02 ) ? 0x20 : 0x1D ; // LBT or FCC
packet[1] = rx_tx_addr[3]; packet[1] = rx_tx_addr[3];
packet[2] = rx_tx_addr[2]; packet[2] = rx_tx_addr[2];
packet[3] = 0x02; packet[3] = 0x02;
@ -129,35 +162,37 @@ static void __attribute__((unused)) frskyX_data_frame()
//packet[7] = FLAGS 00 - standard packet //packet[7] = FLAGS 00 - standard packet
//10, 12, 14, 16, 18, 1A, 1C, 1E - failsafe packet //10, 12, 14, 16, 18, 1A, 1C, 1E - failsafe packet
//20 - range check packet //20 - range check packet
packet[7] = 0; packet[7] = FS_flag;
packet[8] = 0; packet[8] = 0;
// //
if ( lpass & 1 ) uint8_t startChan = chan_offset; for(uint8_t i = 0; i <12 ; i+=3)
startChan += 8 ; {//12 bytes of channel data
#ifdef FAILSAFE_ENABLE
for(uint8_t i = 0; i <12 ; i+=3) if( (FS_flag & 0x10) && ((failsafe_chan & 0x07) == (startChan & 0x07)) )
{//12 bytes chan_0 = frskyX_scaleForPXX_FS(failsafe_chan);
chan_0 = frskyX_scaleForPXX(startChan); else
if(lpass & 1 ) #endif
chan_0+=2048; chan_0 = frskyX_scaleForPXX(startChan);
startChan+=1; startChan++;
// //
chan_1 = frskyX_scaleForPXX(startChan); #ifdef FAILSAFE_ENABLE
if(lpass & 1 ) if( (FS_flag & 0x10) && ((failsafe_chan & 0x07) == (startChan & 0x07)) )
chan_1+= 2048; chan_1 = frskyX_scaleForPXX_FS(failsafe_chan);
startChan+=1; else
#endif
chan_1 = frskyX_scaleForPXX(startChan);
startChan++;
// //
packet[9+i] = lowByte(chan_0);//3 bytes*4 packet[9+i] = lowByte(chan_0); //3 bytes*4
packet[9+i+1]=(((chan_0>>8) & 0x0F)|(chan_1 << 4)); packet[9+i+1]=(((chan_0>>8) & 0x0F)|(chan_1 << 4));
packet[9+i+2]=chan_1>>4; packet[9+i+2]=chan_1>>4;
} }
packet[21] = (FrX_receive_seq << 4) | FrX_send_seq ;//8 at start packet[21] = (FrX_receive_seq << 4) | FrX_send_seq ;//8 at start
if(sub_protocol & 1 )// in X8 mode send only 8ch every 9ms if(sub_protocol & 0x01 ) // in X8 mode send only 8ch every 9ms
lpass = 0 ; chan_offset = 0 ;
else else
lpass += 1 ; chan_offset^=0x08;
uint8_t limit = (sub_protocol & 2 ) ? 31 : 28 ; uint8_t limit = (sub_protocol & 2 ) ? 31 : 28 ;
for (uint8_t i=22;i<limit;i++) for (uint8_t i=22;i<limit;i++)

6
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 39 #define VERSION_PATCH_LEVEL 40
//****************** //******************
// Protocols // Protocols
//****************** //******************
@ -326,9 +326,11 @@ enum MultiPacketTypes {
//Debug messages //Debug messages
#if defined(STM32_BOARD) && defined (SERIAL_DEBUG) #if defined(STM32_BOARD) && defined (SERIAL_DEBUG)
#define debug(msg, ...) {char buf[64]; sprintf(buf, msg "\r\n", ##__VA_ARGS__); Serial.write(buf);} #define debug(msg, ...) {char buf[64]; sprintf(buf, msg, ##__VA_ARGS__); Serial.write(buf);}
#define debugln(msg, ...) {char buf[64]; sprintf(buf, msg "\r\n", ##__VA_ARGS__); Serial.write(buf);}
#else #else
#define debug(...) { } #define debug(...) { }
#define debugln(...) { }
#undef SERIAL_DEBUG #undef SERIAL_DEBUG
#endif #endif

12
Multiprotocol/Multiprotocol.ino
View File

@ -218,7 +218,7 @@ void setup()
#ifdef SERIAL_DEBUG #ifdef SERIAL_DEBUG
Serial.begin(115200,SERIAL_8N1); Serial.begin(115200,SERIAL_8N1);
while (!Serial); // Wait for ever for the serial port to connect... while (!Serial); // Wait for ever for the serial port to connect...
debug("Multiprotocol version: %d.%d.%d.%d", VERSION_MAJOR, VERSION_MINOR, VERSION_REVISION, VERSION_PATCH_LEVEL); debugln("Multiprotocol version: %d.%d.%d.%d", VERSION_MAJOR, VERSION_MINOR, VERSION_REVISION, VERSION_PATCH_LEVEL);
#endif #endif
// General pinout // General pinout
@ -353,7 +353,7 @@ void setup()
((MODE_DIAL3_ipr & _BV(MODE_DIAL3_pin)) ? 0 : 4) + ((MODE_DIAL3_ipr & _BV(MODE_DIAL3_pin)) ? 0 : 4) +
((MODE_DIAL4_ipr & _BV(MODE_DIAL4_pin)) ? 0 : 8); ((MODE_DIAL4_ipr & _BV(MODE_DIAL4_pin)) ? 0 : 8);
#endif #endif
debug("Mode switch reads as %d", mode_select); debugln("Mode switch reads as %d", mode_select);
// Set default channels' value // Set default channels' value
for(uint8_t i=0;i<NUM_CHN;i++) for(uint8_t i=0;i<NUM_CHN;i++)
@ -382,7 +382,7 @@ void setup()
// Read or create protocol id // Read or create protocol id
MProtocol_id_master=random_id(10,false); MProtocol_id_master=random_id(10,false);
debug("Module Id: %lx", MProtocol_id_master); debugln("Module Id: %lx", MProtocol_id_master);
#ifdef ENABLE_PPM #ifdef ENABLE_PPM
//Protocol and interrupts initialization //Protocol and interrupts initialization
@ -438,7 +438,7 @@ void setup()
#endif //ENABLE_SERIAL #endif //ENABLE_SERIAL
} }
servo_mid=servo_min_100+servo_max_100; //In fact 2* mid_value servo_mid=servo_min_100+servo_max_100; //In fact 2* mid_value
debug("Init complete"); debugln("Init complete");
} }
// Main // Main
@ -1102,7 +1102,7 @@ void update_serial_data()
protocol=(rx_ok_buff[0]==0x55?0:32) + (rx_ok_buff[1]&0x1F); //protocol no (0-63) bits 4-6 of buff[1] and bit 0 of buf[0] protocol=(rx_ok_buff[0]==0x55?0:32) + (rx_ok_buff[1]&0x1F); //protocol no (0-63) bits 4-6 of buff[1] and bit 0 of buf[0]
sub_protocol=(rx_ok_buff[2]>>4)& 0x07; //subprotocol no (0-7) bits 4-6 sub_protocol=(rx_ok_buff[2]>>4)& 0x07; //subprotocol no (0-7) bits 4-6
RX_num=rx_ok_buff[2]& 0x0F; // rx_num bits 0---3 RX_num=rx_ok_buff[2]& 0x0F; // rx_num bits 0---3
debug("New protocol selected: %d, sub proto %d, rxnum %d", protocol, sub_protocol, RX_num); debugln("New protocol selected: %d, sub proto %d, rxnum %d", protocol, sub_protocol, RX_num);
} }
else else
if( ((rx_ok_buff[1]&0x80)!=0) && ((cur_protocol[1]&0x80)==0) ) // Bind flag has been set if( ((rx_ok_buff[1]&0x80)!=0) && ((cur_protocol[1]&0x80)==0) ) // Bind flag has been set
@ -1161,7 +1161,7 @@ void update_serial_data()
#endif #endif
#ifdef FAILSAFE_ENABLE #ifdef FAILSAFE_ENABLE
if(failsafe) if(failsafe)
debug("RX_FS:%d,%d,%d,%d",Failsafe_data[0],Failsafe_data[1],Failsafe_data[2],Failsafe_data[3]); debugln("RX_FS:%d,%d,%d,%d",Failsafe_data[0],Failsafe_data[1],Failsafe_data[2],Failsafe_data[3]);
#endif #endif
} }

4
Multiprotocol/_Config.h
View File

@ -43,7 +43,7 @@
/*************************/ /*************************/
/*** BIND FROM CHANNEL ***/ /*** BIND FROM CHANNEL ***/
/*************************/ /*************************/
//Bind from channel enables you to bind when a specified channel is giong from low to high. This feature is only active //Bind from channel enables you to bind when a specified channel is going from low to high. This feature is only active
// if you specify AUTOBIND in PPM mode or set AutoBind to YES for serial mode. It also requires that the throttle channel is low. // if you specify AUTOBIND in PPM mode or set AutoBind to YES for serial mode. It also requires that the throttle channel is low.
//Comment to globaly disable the bind feature from a channel. //Comment to globaly disable the bind feature from a channel.
@ -214,7 +214,7 @@
//This is useful for passing sport control frames from TX to RX(ex: changing Betaflight PID or VTX channels on the fly using LUA scripts with OpentX). //This is useful for passing sport control frames from TX to RX(ex: changing Betaflight PID or VTX channels on the fly using LUA scripts with OpentX).
//Using this feature on turnigy 9XR_PRO requires uncomment INVERT_TELEMETRY as this TX output on telemetry pin only inverted signal. //Using this feature on turnigy 9XR_PRO requires uncomment INVERT_TELEMETRY as this TX output on telemetry pin only inverted signal.
//!This is a work in progress! //!This is a work in progress!
#define SPORT_POLLING //#define SPORT_POLLING
/****************************/ /****************************/
/*** SERIAL MODE SETTINGS ***/ /*** SERIAL MODE SETTINGS ***/