mirror of
https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git
synced 2025-02-04 16:38:12 +00:00
FrSkyX failsafe
This commit is contained in:
Pascal Langer
parent
6ae819e8d5
commit
880e463b01
@ -198,7 +198,7 @@ static void AFHDS2A_build_packet(uint8_t type)
|
||||
packet[0] = 0xaa;
|
||||
packet[9] = 0xfd;
|
||||
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
|
||||
packet[11]= val_hz;
|
||||
packet[12]= val_hz >> 8;
|
||||
|
||||
@ -74,14 +74,6 @@ static uint16_t __attribute__((unused)) frskyX_crc_x(uint8_t *data, uint8_t len)
|
||||
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()
|
||||
{
|
||||
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()
|
||||
{
|
||||
//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_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[2] = rx_tx_addr[2];
|
||||
packet[3] = 0x02;
|
||||
@ -129,35 +162,37 @@ static void __attribute__((unused)) frskyX_data_frame()
|
||||
//packet[7] = FLAGS 00 - standard packet
|
||||
//10, 12, 14, 16, 18, 1A, 1C, 1E - failsafe packet
|
||||
//20 - range check packet
|
||||
packet[7] = 0;
|
||||
packet[7] = FS_flag;
|
||||
packet[8] = 0;
|
||||
//
|
||||
if ( lpass & 1 )
|
||||
startChan += 8 ;
|
||||
|
||||
for(uint8_t i = 0; i <12 ; i+=3)
|
||||
{//12 bytes
|
||||
chan_0 = frskyX_scaleForPXX(startChan);
|
||||
if(lpass & 1 )
|
||||
chan_0+=2048;
|
||||
startChan+=1;
|
||||
uint8_t startChan = chan_offset; for(uint8_t i = 0; i <12 ; i+=3)
|
||||
{//12 bytes of channel data
|
||||
#ifdef FAILSAFE_ENABLE
|
||||
if( (FS_flag & 0x10) && ((failsafe_chan & 0x07) == (startChan & 0x07)) )
|
||||
chan_0 = frskyX_scaleForPXX_FS(failsafe_chan);
|
||||
else
|
||||
#endif
|
||||
chan_0 = frskyX_scaleForPXX(startChan);
|
||||
startChan++;
|
||||
//
|
||||
chan_1 = frskyX_scaleForPXX(startChan);
|
||||
if(lpass & 1 )
|
||||
chan_1+= 2048;
|
||||
startChan+=1;
|
||||
#ifdef FAILSAFE_ENABLE
|
||||
if( (FS_flag & 0x10) && ((failsafe_chan & 0x07) == (startChan & 0x07)) )
|
||||
chan_1 = frskyX_scaleForPXX_FS(failsafe_chan);
|
||||
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+2]=chan_1>>4;
|
||||
}
|
||||
|
||||
packet[21] = (FrX_receive_seq << 4) | FrX_send_seq ;//8 at start
|
||||
|
||||
if(sub_protocol & 1 )// in X8 mode send only 8ch every 9ms
|
||||
lpass = 0 ;
|
||||
if(sub_protocol & 0x01 ) // in X8 mode send only 8ch every 9ms
|
||||
chan_offset = 0 ;
|
||||
else
|
||||
lpass += 1 ;
|
||||
chan_offset^=0x08;
|
||||
|
||||
uint8_t limit = (sub_protocol & 2 ) ? 31 : 28 ;
|
||||
for (uint8_t i=22;i<limit;i++)
|
||||
|
||||
@ -19,7 +19,7 @@
|
||||
#define VERSION_MAJOR 1
|
||||
#define VERSION_MINOR 1
|
||||
#define VERSION_REVISION 6
|
||||
#define VERSION_PATCH_LEVEL 39
|
||||
#define VERSION_PATCH_LEVEL 40
|
||||
//******************
|
||||
// Protocols
|
||||
//******************
|
||||
@ -326,9 +326,11 @@ enum MultiPacketTypes {
|
||||
|
||||
//Debug messages
|
||||
#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
|
||||
#define debug(...) { }
|
||||
#define debugln(...) { }
|
||||
#undef SERIAL_DEBUG
|
||||
#endif
|
||||
|
||||
|
||||
@ -218,7 +218,7 @@ void setup()
|
||||
#ifdef SERIAL_DEBUG
|
||||
Serial.begin(115200,SERIAL_8N1);
|
||||
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
|
||||
|
||||
// General pinout
|
||||
@ -353,7 +353,7 @@ void setup()
|
||||
((MODE_DIAL3_ipr & _BV(MODE_DIAL3_pin)) ? 0 : 4) +
|
||||
((MODE_DIAL4_ipr & _BV(MODE_DIAL4_pin)) ? 0 : 8);
|
||||
#endif
|
||||
debug("Mode switch reads as %d", mode_select);
|
||||
debugln("Mode switch reads as %d", mode_select);
|
||||
|
||||
// Set default channels' value
|
||||
for(uint8_t i=0;i<NUM_CHN;i++)
|
||||
@ -382,7 +382,7 @@ void setup()
|
||||
// Read or create protocol id
|
||||
MProtocol_id_master=random_id(10,false);
|
||||
|
||||
debug("Module Id: %lx", MProtocol_id_master);
|
||||
debugln("Module Id: %lx", MProtocol_id_master);
|
||||
|
||||
#ifdef ENABLE_PPM
|
||||
//Protocol and interrupts initialization
|
||||
@ -438,7 +438,7 @@ void setup()
|
||||
#endif //ENABLE_SERIAL
|
||||
}
|
||||
servo_mid=servo_min_100+servo_max_100; //In fact 2* mid_value
|
||||
debug("Init complete");
|
||||
debugln("Init complete");
|
||||
}
|
||||
|
||||
// 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]
|
||||
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
|
||||
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
|
||||
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
|
||||
#ifdef FAILSAFE_ENABLE
|
||||
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
|
||||
}
|
||||
|
||||
|
||||
@ -43,7 +43,7 @@
|
||||
/*************************/
|
||||
/*** 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.
|
||||
|
||||
//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).
|
||||
//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!
|
||||
#define SPORT_POLLING
|
||||
//#define SPORT_POLLING
|
||||
|
||||
/****************************/
|
||||
/*** SERIAL MODE SETTINGS ***/
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user