From 880e463b015d2219758d8c276bad83394b96c655 Mon Sep 17 00:00:00 2001 From: Pascal Langer Date: Sun, 10 Dec 2017 09:48:20 +0100 Subject: [PATCH] FrSkyX failsafe --- Multiprotocol/AFHDS2A_a7105.ino | 2 +- Multiprotocol/FrSkyX_cc2500.ino | 95 ++++++++++++++++++++++----------- Multiprotocol/Multiprotocol.h | 6 ++- Multiprotocol/Multiprotocol.ino | 12 ++--- Multiprotocol/_Config.h | 4 +- 5 files changed, 78 insertions(+), 41 deletions(-) diff --git a/Multiprotocol/AFHDS2A_a7105.ino b/Multiprotocol/AFHDS2A_a7105.ino index e49bf23..15fa0cf 100644 --- a/Multiprotocol/AFHDS2A_a7105.ino +++ b/Multiprotocol/AFHDS2A_a7105.ino @@ -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; diff --git a/Multiprotocol/FrSkyX_cc2500.ino b/Multiprotocol/FrSkyX_cc2500.ino index 7380e71..103ca23 100644 --- a/Multiprotocol/FrSkyX_cc2500.ino +++ b/Multiprotocol/FrSkyX_cc2500.ino @@ -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>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 } diff --git a/Multiprotocol/_Config.h b/Multiprotocol/_Config.h index ba7ef83..951d8ce 100644 --- a/Multiprotocol/_Config.h +++ b/Multiprotocol/_Config.h @@ -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 ***/