mirror of
https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git
synced 2025-02-04 16:48:10 +00:00
Failsafe: default for PPM and set on radio for Serial
This commit is contained in:
Pascal Langer
parent
ebd44d9628
commit
4418cab3a5
@ -179,30 +179,19 @@ static void AFHDS2A_build_packet(uint8_t type)
|
||||
packet[0] = 0x56;
|
||||
for(uint8_t ch=0; ch<14; ch++)
|
||||
{
|
||||
#ifdef AFHDS2A_FAILSAFE
|
||||
int8_t failsafe = AFHDS2AFailsafe[ch];
|
||||
//
|
||||
if(failsafe != -1)
|
||||
{
|
||||
//
|
||||
if (failsafe > AFHDS2AFailsafeMAX)
|
||||
failsafe = AFHDS2AFailsafeMAX;
|
||||
//
|
||||
if (failsafe < AFHDS2AFailsafeMIN)
|
||||
failsafe = AFHDS2AFailsafeMIN;
|
||||
//
|
||||
double scale = (float)failsafe/(float)100;
|
||||
int16_t failsafeMicros = 1500 + ((float)512 * scale);
|
||||
//
|
||||
packet[9 + ch*2] = failsafeMicros & 0xff;
|
||||
packet[10+ ch*2] = ( failsafeMicros >> 8) & 0xff;
|
||||
}
|
||||
else
|
||||
#endif
|
||||
{
|
||||
packet[9 + ch*2] = 0xff;
|
||||
packet[10+ ch*2] = 0xff;
|
||||
}
|
||||
#ifdef FAILSAFE_ENABLE
|
||||
uint16_t failsafeMicros = (Failsafe_data[CH_AETR[ch]]*5)/8+860;
|
||||
if( failsafeMicros!=FAILSAFE_CHANNEL_HOLD+860)
|
||||
{ // Failsafe values
|
||||
packet[9 + ch*2] = failsafeMicros & 0xff;
|
||||
packet[10+ ch*2] = ( failsafeMicros >> 8) & 0xff;
|
||||
}
|
||||
else
|
||||
#endif
|
||||
{ // no values
|
||||
packet[9 + ch*2] = 0xff;
|
||||
packet[10+ ch*2] = 0xff;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case AFHDS2A_PACKET_SETTINGS:
|
||||
@ -307,10 +296,15 @@ uint16_t ReadAFHDS2A()
|
||||
hopping_frequency_no = 0;
|
||||
if(!(packet_counter % 1313))
|
||||
packet_type = AFHDS2A_PACKET_SETTINGS;
|
||||
else if(!(packet_counter % 1569))
|
||||
packet_type = AFHDS2A_PACKET_FAILSAFE;
|
||||
else
|
||||
packet_type = AFHDS2A_PACKET_STICKS; // todo : check for settings changes
|
||||
{
|
||||
#ifdef FAILSAFE_ENABLE
|
||||
if(!(packet_counter % 1569) && IS_FAILSAFE_VALUES_on)
|
||||
packet_type = AFHDS2A_PACKET_FAILSAFE;
|
||||
else
|
||||
#endif
|
||||
packet_type = AFHDS2A_PACKET_STICKS; // todo : check for settings changes
|
||||
}
|
||||
if(!(A7105_ReadReg(A7105_00_MODE) & (1<<5 | 1<<6)) && data_rx==1)
|
||||
{ // RX+FECF+CRCF Ok
|
||||
A7105_ReadData(AFHDS2A_RXPACKET_SIZE);
|
||||
|
||||
@ -12,6 +12,21 @@
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifdef FAILSAFE_ENABLE
|
||||
void InitFailsafe()
|
||||
{
|
||||
for(uint8_t i=0;i<NUM_CHN;i++)
|
||||
Failsafe_data[i]=1024;
|
||||
Failsafe_data[THROTTLE]=FAILSAFE_THROTTLE_LOW; //0=-125%, 204=-100%
|
||||
FAILSAFE_VALUES_on;
|
||||
#ifdef FAILSAFE_SERIAL_ONLY
|
||||
if(mode_select == MODE_SERIAL)
|
||||
FAILSAFE_VALUES_off;
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
/************************/
|
||||
/** Convert routines **/
|
||||
/************************/
|
||||
@ -54,6 +69,15 @@ void convert_channel_HK310(uint8_t num, uint8_t *low, uint8_t *high)
|
||||
*low=(uint8_t)(temp&0xFF);
|
||||
*high=(uint8_t)(temp>>8);
|
||||
}
|
||||
// Failsafe value is converted for HK310
|
||||
#ifdef FAILSAFE_ENABLE
|
||||
void convert_failsafe_HK310(uint8_t num, uint8_t *low, uint8_t *high)
|
||||
{
|
||||
uint16_t temp=0xFFFF-(3440+((Failsafe_data[num]*5)>>1))/3;
|
||||
*low=(uint8_t)(temp&0xFF);
|
||||
*high=(uint8_t)(temp>>8);
|
||||
}
|
||||
#endif
|
||||
|
||||
// Channel value is converted to 16bit values
|
||||
uint16_t convert_channel_16b(uint8_t num, int16_t out_min, int16_t out_max)
|
||||
|
||||
@ -94,15 +94,29 @@ static void __attribute__((unused)) DEVO_add_pkt_suffix()
|
||||
static void __attribute__((unused)) DEVO_build_beacon_pkt(uint8_t upper)
|
||||
{
|
||||
packet[0] = (DEVO_NUM_CHANNELS << 4) | 0x07;
|
||||
uint8_t max = 8;
|
||||
uint8_t max = 8, offset = 0, enable = 0;
|
||||
if (upper)
|
||||
{
|
||||
packet[0] += 1;
|
||||
max = 4;
|
||||
offset = 8;
|
||||
}
|
||||
for(uint8_t i = 0; i < max; i++)
|
||||
packet[i+1] = 0;
|
||||
packet[9] = 0;
|
||||
{
|
||||
#ifdef FAILSAFE_ENABLE
|
||||
uint16_t failsafe=Failsafe_data[CH_EATR[i+offset]];
|
||||
if(i + offset < DEVO_NUM_CHANNELS && failsafe!=FAILSAFE_CHANNEL_HOLD && IS_FAILSAFE_VALUES_on)
|
||||
{
|
||||
enable |= 0x80 >> i;
|
||||
packet[i+1] = ((failsafe*25)>>8)-100;
|
||||
}
|
||||
else
|
||||
#else
|
||||
(void)offset;
|
||||
#endif
|
||||
packet[i+1] = 0;
|
||||
}
|
||||
packet[9] = enable;
|
||||
DEVO_add_pkt_suffix();
|
||||
}
|
||||
|
||||
|
||||
@ -19,14 +19,13 @@
|
||||
|
||||
#include "iface_cc2500.h"
|
||||
|
||||
uint8_t chanskip;
|
||||
//uint8_t seq_last_sent;
|
||||
//uint8_t seq_last_rcvd;
|
||||
|
||||
uint8_t FrX_chanskip;
|
||||
uint8_t FrX_send_seq ;
|
||||
uint8_t FrX_receive_seq ;
|
||||
|
||||
static void __attribute__((unused)) set_start(uint8_t ch )
|
||||
#define FRX_FAILSAFE_TIMEOUT 1032
|
||||
|
||||
static void __attribute__((unused)) frskyX_set_start(uint8_t ch )
|
||||
{
|
||||
CC2500_Strobe(CC2500_SIDLE);
|
||||
CC2500_WriteReg(CC2500_25_FSCAL1, calData[ch]);
|
||||
@ -48,7 +47,7 @@ static void __attribute__((unused)) frskyX_init()
|
||||
//#######END INIT########
|
||||
}
|
||||
|
||||
static void __attribute__((unused)) initialize_data(uint8_t adr)
|
||||
static void __attribute__((unused)) frskyX_initialize_data(uint8_t adr)
|
||||
{
|
||||
CC2500_WriteReg(CC2500_0C_FSCTRL0,option); // Frequency offset hack
|
||||
CC2500_WriteReg(CC2500_18_MCSM0, 0x8);
|
||||
@ -57,28 +56,28 @@ static void __attribute__((unused)) initialize_data(uint8_t adr)
|
||||
}
|
||||
|
||||
//**CRC**
|
||||
const uint16_t PROGMEM CRC_Short[]={
|
||||
const uint16_t PROGMEM frskyX_CRC_Short[]={
|
||||
0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF,
|
||||
0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7 };
|
||||
static uint16_t CRCTable(uint8_t val)
|
||||
static uint16_t __attribute__((unused)) frskyX_CRCTable(uint8_t val)
|
||||
{
|
||||
uint16_t word ;
|
||||
word = pgm_read_word(&CRC_Short[val&0x0F]) ;
|
||||
word = pgm_read_word(&frskyX_CRC_Short[val&0x0F]) ;
|
||||
val /= 16 ;
|
||||
return word ^ (0x1081 * val) ;
|
||||
}
|
||||
static uint16_t __attribute__((unused)) crc_x(uint8_t *data, uint8_t len)
|
||||
static uint16_t __attribute__((unused)) frskyX_crc_x(uint8_t *data, uint8_t len)
|
||||
{
|
||||
uint16_t crc = 0;
|
||||
for(uint8_t i=0; i < len; i++)
|
||||
crc = (crc<<8) ^ CRCTable((uint8_t)(crc>>8) ^ *data++);
|
||||
crc = (crc<<8) ^ frskyX_CRCTable((uint8_t)(crc>>8) ^ *data++);
|
||||
return crc;
|
||||
}
|
||||
|
||||
// 0-2047, 0 = 817, 1024 = 1500, 2047 = 2182
|
||||
//64=860,1024=1500,1984=2140//Taranis 125%
|
||||
|
||||
static uint16_t __attribute__((unused)) scaleForPXX( uint8_t i )
|
||||
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;
|
||||
}
|
||||
@ -103,7 +102,7 @@ static void __attribute__((unused)) frskyX_build_bind_packet()
|
||||
//
|
||||
uint8_t limit = (sub_protocol & 2 ) ? 31 : 28 ;
|
||||
memset(&packet[13], 0, limit - 13);
|
||||
uint16_t lcrc = crc_x(&packet[3], limit-3);
|
||||
uint16_t lcrc = frskyX_crc_x(&packet[3], limit-3);
|
||||
//
|
||||
packet[limit++] = lcrc >> 8;
|
||||
packet[limit] = lcrc;
|
||||
@ -124,8 +123,8 @@ static void __attribute__((unused)) frskyX_data_frame()
|
||||
packet[2] = rx_tx_addr[2];
|
||||
packet[3] = 0x02;
|
||||
//
|
||||
packet[4] = (chanskip<<6)|hopping_frequency_no;
|
||||
packet[5] = chanskip>>2;
|
||||
packet[4] = (FrX_chanskip<<6)|hopping_frequency_no;
|
||||
packet[5] = FrX_chanskip>>2;
|
||||
packet[6] = RX_num;
|
||||
//packet[7] = FLAGS 00 - standard packet
|
||||
//10, 12, 14, 16, 18, 1A, 1C, 1E - failsafe packet
|
||||
@ -138,12 +137,12 @@ static void __attribute__((unused)) frskyX_data_frame()
|
||||
|
||||
for(uint8_t i = 0; i <12 ; i+=3)
|
||||
{//12 bytes
|
||||
chan_0 = scaleForPXX(startChan);
|
||||
chan_0 = frskyX_scaleForPXX(startChan);
|
||||
if(lpass & 1 )
|
||||
chan_0+=2048;
|
||||
startChan+=1;
|
||||
//
|
||||
chan_1 = scaleForPXX(startChan);
|
||||
chan_1 = frskyX_scaleForPXX(startChan);
|
||||
if(lpass & 1 )
|
||||
chan_1+= 2048;
|
||||
startChan+=1;
|
||||
@ -187,8 +186,8 @@ static void __attribute__((unused)) frskyX_data_frame()
|
||||
Serial.println(" ");
|
||||
#endif
|
||||
#endif // SPORT_POLLING
|
||||
uint16_t lcrc = crc_x(&packet[3], limit-3);
|
||||
|
||||
|
||||
uint16_t lcrc = frskyX_crc_x(&packet[3], limit-3);
|
||||
packet[limit++]=lcrc>>8;//high byte
|
||||
packet[limit]=lcrc;//low byte
|
||||
}
|
||||
@ -198,7 +197,7 @@ uint16_t ReadFrSkyX()
|
||||
switch(state)
|
||||
{
|
||||
default:
|
||||
set_start(47);
|
||||
frskyX_set_start(47);
|
||||
CC2500_SetPower();
|
||||
CC2500_Strobe(CC2500_SFRX);
|
||||
//
|
||||
@ -211,7 +210,7 @@ uint16_t ReadFrSkyX()
|
||||
state++;
|
||||
return 9000;
|
||||
case FRSKY_BIND_DONE:
|
||||
initialize_data(0);
|
||||
frskyX_initialize_data(0);
|
||||
hopping_frequency_no=0;
|
||||
BIND_DONE;
|
||||
state++;
|
||||
@ -223,10 +222,10 @@ uint16_t ReadFrSkyX()
|
||||
prev_option = option ;
|
||||
}
|
||||
CC2500_SetTxRxMode(TX_EN);
|
||||
set_start(hopping_frequency_no);
|
||||
frskyX_set_start(hopping_frequency_no);
|
||||
CC2500_SetPower();
|
||||
CC2500_Strobe(CC2500_SFRX);
|
||||
hopping_frequency_no = (hopping_frequency_no+chanskip)%47;
|
||||
hopping_frequency_no = (hopping_frequency_no+FrX_chanskip)%47;
|
||||
CC2500_Strobe(CC2500_SIDLE);
|
||||
CC2500_WriteData(packet, packet[0]+1);
|
||||
//
|
||||
@ -287,8 +286,8 @@ uint16_t initFrSkyX()
|
||||
set_rx_tx_addr(MProtocol_id_master);
|
||||
Frsky_init_hop();
|
||||
packet_count=0;
|
||||
while(!chanskip)
|
||||
chanskip=random(0xfefefefe)%47;
|
||||
while(!FrX_chanskip)
|
||||
FrX_chanskip=random(0xfefefefe)%47;
|
||||
|
||||
//for test***************
|
||||
//rx_tx_addr[3]=0xB3;
|
||||
@ -299,12 +298,12 @@ uint16_t initFrSkyX()
|
||||
if(IS_AUTOBIND_FLAG_on)
|
||||
{
|
||||
state = FRSKY_BIND;
|
||||
initialize_data(1);
|
||||
frskyX_initialize_data(1);
|
||||
}
|
||||
else
|
||||
{
|
||||
state = FRSKY_DATA1;
|
||||
initialize_data(0);
|
||||
frskyX_initialize_data(0);
|
||||
}
|
||||
// seq_last_sent = 0;
|
||||
// seq_last_rcvd = 8;
|
||||
|
||||
@ -150,12 +150,25 @@ uint16_t hisky_cb()
|
||||
case 4:
|
||||
phase=6;
|
||||
break;
|
||||
case 7: // build packet with failsafe every 100ms
|
||||
convert_channel_HK310(hopping_frequency_no!=0?RUDDER:AUX2,&packet[0],&packet[1]);
|
||||
convert_channel_HK310(hopping_frequency_no!=0?THROTTLE:AUX3,&packet[2],&packet[3]);
|
||||
convert_channel_HK310(hopping_frequency_no!=0?AUX1:AUX4,&packet[4],&packet[5]);
|
||||
packet[7]=hopping_frequency_no!=0?0x55:0xAA;
|
||||
packet[8]=hopping_frequency_no!=0?0x67:0x5A;
|
||||
case 7: // build packet
|
||||
#ifdef FAILSAFE_ENABLE
|
||||
if(IS_FAILSAFE_VALUES_on && hopping_frequency_no==0)
|
||||
{ // send failsafe every 100ms
|
||||
convert_failsafe_HK310(RUDDER, &packet[0],&packet[1]);
|
||||
convert_failsafe_HK310(THROTTLE,&packet[2],&packet[3]);
|
||||
convert_failsafe_HK310(AUX1, &packet[4],&packet[5]);
|
||||
packet[7]=0xAA;
|
||||
packet[8]=0x5A;
|
||||
}
|
||||
else
|
||||
#endif
|
||||
{
|
||||
convert_channel_HK310(RUDDER, &packet[0],&packet[1]);
|
||||
convert_channel_HK310(THROTTLE,&packet[2],&packet[3]);
|
||||
convert_channel_HK310(AUX1, &packet[4],&packet[5]);
|
||||
packet[7]=0x55;
|
||||
packet[8]=0x67;
|
||||
}
|
||||
phase=8;
|
||||
break;
|
||||
}
|
||||
|
||||
@ -245,24 +245,8 @@ enum MultiPacketTypes {
|
||||
MULTI_TELEMETRY_DSM = 4,
|
||||
MULTI_TELEMETRY_DSMBIND = 5,
|
||||
MULTI_TELEMETRY_AFHDS2A = 6,
|
||||
MULTI_TELEMETRY_INPUTSYNC=8,
|
||||
MULTI_COMMAND_CONFIG = 0x80,
|
||||
MULTI_COMMAND_FAILSAFE =0x81,
|
||||
};
|
||||
|
||||
enum FailSafeMode {
|
||||
FAILSAFE_NOTSET = 0,
|
||||
FAILSAFE_HOLD = 1,
|
||||
FAILSAFE_CUSTOM = 2,
|
||||
FAILSAFE_NOPULSES = 3,
|
||||
FAILSAFE_RECEIVER = 4,
|
||||
// Use during update so we can get away with only one copy of Failsafe channels
|
||||
FAILSEFASE_INVALID = 0xfe
|
||||
};
|
||||
|
||||
#define FAILSAFE_CHANNEL_HOLD 0
|
||||
#define FAILSAFE_CHANNEL_NOPULSES 2047
|
||||
|
||||
// Macros
|
||||
#define NOP() __asm__ __volatile__("nop")
|
||||
|
||||
@ -301,9 +285,9 @@ enum FailSafeMode {
|
||||
#define BIND_DONE protocol_flags |= _BV(7)
|
||||
#define IS_BIND_DONE_on ( ( protocol_flags & _BV(7) ) !=0 )
|
||||
//
|
||||
#define BAD_PROTO_off protocol_flags2 &= ~_BV(0)
|
||||
#define BAD_PROTO_on protocol_flags2 |= _BV(0)
|
||||
#define IS_BAD_PROTO_on ( ( protocol_flags2 & _BV(0) ) !=0 )
|
||||
#define FAILSAFE_VALUES_off protocol_flags2 &= ~_BV(0)
|
||||
#define FAILSAFE_VALUES_on protocol_flags2 |= _BV(0)
|
||||
#define IS_FAILSAFE_VALUES_on ( ( protocol_flags2 & _BV(0) ) !=0 )
|
||||
//
|
||||
#define RX_DONOTUPDTAE_off protocol_flags2 &= ~_BV(1)
|
||||
#define RX_DONOTUPDTAE_on protocol_flags2 |= _BV(1)
|
||||
@ -336,21 +320,15 @@ enum FailSafeMode {
|
||||
#define IS_WAIT_BIND_on ( ( protocol_flags2 & _BV(7) ) !=0 )
|
||||
#define IS_WAIT_BIND_off ( ( protocol_flags2 & _BV(7) ) ==0 )
|
||||
|
||||
//Configuration
|
||||
#define IS_TELEMTRY_INVERSION_ON (multi_config & 0x01)
|
||||
#define IS_MULTI_TELEMETRY_ON (multi_config & 0x02)
|
||||
#define IS_EXTRA_TELEMETRY_ON (multi_config & 0x04)
|
||||
|
||||
// Failsafe
|
||||
#define failsafeToPPM(i) (Failsafe_data[i]* 5/8+860)
|
||||
#define isNormalFailsafeChanel(i) (Failsafe_data[i] != FAILSAFE_CHANNEL_HOLD && Failsafe_data[i] != FAILSAFE_CHANNEL_NOPULSES)
|
||||
#define FAILSAFE_CHANNEL_HOLD 0
|
||||
#define FAILSAFE_CHANNEL_NOPULSES 2047
|
||||
|
||||
|
||||
//Status messages
|
||||
//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);}
|
||||
#else
|
||||
#define debug(...)
|
||||
#define debug(...) { }
|
||||
#undef SERIAL_DEBUG
|
||||
#endif
|
||||
|
||||
@ -365,7 +343,6 @@ enum FailSafeMode {
|
||||
#define BLINK_WAIT_BIND_TIME_HIGH 1000
|
||||
#define BLINK_WAIT_BIND_TIME_LOW 100
|
||||
|
||||
|
||||
//*******************
|
||||
//*** AUX flags ***
|
||||
//*******************
|
||||
@ -509,8 +486,10 @@ enum {
|
||||
**************************
|
||||
Serial: 100000 Baud 8e2 _ xxxx xxxx p --
|
||||
Total of 26 bytes
|
||||
Stream[0] = 0x55 sub_protocol values are 0..31
|
||||
Stream[0] = 0x54 sub_protocol values are 32..63
|
||||
Stream[0] = 0x55 sub_protocol values are 0..31 Stream contains channels
|
||||
Stream[0] = 0x54 sub_protocol values are 32..63 Stream contains channels
|
||||
Stream[0] = 0x57 sub_protocol values are 0..31 Stream contains failsafe
|
||||
Stream[0] = 0x56 sub_protocol values are 32..63 Stream contains failsafe
|
||||
header
|
||||
Stream[1] = sub_protocol|BindBit|RangeCheckBit|AutoBindBit;
|
||||
sub_protocol is 0..31 (bits 0..4), value should be added with 32 if Stream[0] = 0x54
|
||||
@ -671,14 +650,16 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p --
|
||||
Power value => 0x80 0=High/1=Low
|
||||
Stream[3] = option_protocol;
|
||||
option_protocol value is -128..127
|
||||
Stream[4] to [25] = Channels
|
||||
Stream[4] to [25] = Channels or failsafe depending on Steam[0]
|
||||
16 Channels on 11 bits (0..2047)
|
||||
0 -125%
|
||||
204 -100%
|
||||
1024 0%
|
||||
1843 +100%
|
||||
2047 +125%
|
||||
Channels bits are concatenated to fit in 22 bytes like in SBUS protocol
|
||||
Values are concatenated to fit in 22 bytes like in SBUS protocol.
|
||||
Failsafe values have exactly the same range/values than normal channels except the extremes where
|
||||
0=hold, 2047=no pulse. If failsafe is not set or RX then failsafe packets should not be sent.
|
||||
*/
|
||||
/*
|
||||
Multimodule Status
|
||||
@ -764,45 +745,4 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p --
|
||||
data[0] = RSSI value
|
||||
data[1-28] telemetry data
|
||||
|
||||
|
||||
Type 0x08 Input synchronisation
|
||||
Informs the TX about desired rate and current delay
|
||||
length: 4
|
||||
data[0-1] Desired refresh rate in µs
|
||||
data[2-3] Time (µs) between last serial servo input received and servo input needed (lateness), TX should adjust its
|
||||
sending time to minimise this value.
|
||||
data[4] Interval of this message in ms
|
||||
data[5] Input delay target in 10µs
|
||||
|
||||
Note that there are protocols (AFHDS2A) that have a refresh rate that is smaller than the maximum achievable
|
||||
refresh rate via the serial protocol, in this case, the TX should double the rate and also subract this
|
||||
refresh rate from the input lag if the input lag is more than the desired refresh rate.
|
||||
|
||||
The remote should try to get to zero of (inputdelay+target*10).
|
||||
|
||||
Commands from TX to module use values > 127 for command type
|
||||
|
||||
Type 0x80 Module Configuration
|
||||
This sent from the TX to Multi to configure inversion and multi telemetry type
|
||||
length: 1
|
||||
data[0] flags
|
||||
0x01 Telemetry inversion (1 = inverted)
|
||||
0x02 Use Multi telemetry protocol (if 0 use multi status)
|
||||
0x04 Send extra telemetry (type 0x08) to allow input synchronisation
|
||||
|
||||
|
||||
Type 0x81 Failsafe data
|
||||
length: 23
|
||||
data[0] Failsafe mode:
|
||||
0 - Failsafe not set
|
||||
1 - Failsafe hold, keep last received values
|
||||
2 - Failsafe custom, use the values from the channels
|
||||
3 - Failsafe nopulses, stop sending pulses from the receiver
|
||||
4 - Failsafe receiver, use receiver stored values
|
||||
|
||||
Many of these many modes don't work with all protocols, fallback to best
|
||||
available method
|
||||
data[1-22] Failsafe data, encoded like normal channel data, with the expection
|
||||
that 0 means hold for that channel and 2047 means no pulses
|
||||
|
||||
*/
|
||||
|
||||
@ -79,6 +79,9 @@ uint16_t Servo_data[NUM_CHN];
|
||||
uint8_t Servo_AUX;
|
||||
uint16_t servo_max_100,servo_min_100,servo_max_125,servo_min_125;
|
||||
uint16_t servo_mid;
|
||||
#ifdef FAILSAFE_ENABLE
|
||||
uint16_t Failsafe_data[NUM_CHN];
|
||||
#endif
|
||||
|
||||
// Protocol variables
|
||||
uint8_t cyrfmfg_id[6];//for dsm2 and devo
|
||||
@ -101,6 +104,7 @@ uint8_t flags;
|
||||
uint16_t crc;
|
||||
uint8_t crc8;
|
||||
uint16_t seed;
|
||||
uint16_t failsafe_count;
|
||||
//
|
||||
uint16_t state;
|
||||
uint8_t len;
|
||||
@ -329,14 +333,6 @@ void setup()
|
||||
SCLK_off;
|
||||
#endif
|
||||
|
||||
// Set servos positions
|
||||
for(uint8_t i=0;i<NUM_CHN;i++)
|
||||
Servo_data[i]=1500;
|
||||
Servo_data[THROTTLE]=servo_min_100;
|
||||
#ifdef ENABLE_PPM
|
||||
memcpy((void *)PPM_data,Servo_data, sizeof(Servo_data));
|
||||
#endif
|
||||
|
||||
//Wait for every component to start
|
||||
delayMilliseconds(100);
|
||||
|
||||
@ -359,6 +355,14 @@ void setup()
|
||||
#endif
|
||||
debug("Mode switch reads as %d", mode_select);
|
||||
|
||||
// Set default channels' value
|
||||
for(uint8_t i=0;i<NUM_CHN;i++)
|
||||
Servo_data[i]=1500;
|
||||
Servo_data[THROTTLE]=servo_min_100;
|
||||
#ifdef ENABLE_PPM
|
||||
memcpy((void *)PPM_data,Servo_data, sizeof(Servo_data));
|
||||
#endif
|
||||
|
||||
// Update LED
|
||||
LED_off;
|
||||
LED_output;
|
||||
@ -754,6 +758,10 @@ static void protocol_init()
|
||||
MProtocol_id = RX_num + MProtocol_id_master;
|
||||
set_rx_tx_addr(MProtocol_id);
|
||||
|
||||
#ifdef FAILSAFE_ENABLE
|
||||
InitFailsafe();
|
||||
#endif
|
||||
|
||||
blink=millis();
|
||||
|
||||
if(IS_BIND_BUTTON_FLAG_on)
|
||||
@ -1077,7 +1085,16 @@ void update_serial_data()
|
||||
POWER_FLAG_on; //power high
|
||||
|
||||
option=rx_ok_buff[3];
|
||||
|
||||
|
||||
#ifdef FAILSAFE_ENABLE
|
||||
bool failsafe=false;
|
||||
if(rx_ok_buff[0]&0x02)
|
||||
{ //packet contains failsafe instead of channels
|
||||
failsafe=true;
|
||||
rx_ok_buff[0]&=0xFD; //remove the failsafe flag
|
||||
FAILSAFE_VALUES_on; //failsafe data has been received
|
||||
}
|
||||
#endif
|
||||
if( (rx_ok_buff[0] != cur_protocol[0]) || ((rx_ok_buff[1]&0x5F) != (cur_protocol[1]&0x5F)) || ( (rx_ok_buff[2]&0x7F) != (cur_protocol[2]&0x7F) ) )
|
||||
{ // New model has been selected
|
||||
CHANGE_PROTOCOL_FLAG_on; //change protocol
|
||||
@ -1106,7 +1123,7 @@ void update_serial_data()
|
||||
for(uint8_t i=0;i<3;i++)
|
||||
cur_protocol[i] = rx_ok_buff[i];
|
||||
|
||||
// decode channel values
|
||||
// decode channel/failsafe values
|
||||
volatile uint8_t *p=rx_ok_buff+3;
|
||||
uint8_t dec=-3;
|
||||
for(uint8_t i=0;i<NUM_CHN;i++)
|
||||
@ -1118,7 +1135,13 @@ void update_serial_data()
|
||||
p++;
|
||||
}
|
||||
p++;
|
||||
Servo_data[i]=((((*((uint32_t *)p))>>dec)&0x7FF)*5)/8+860; //value range 860<->2140 -125%<->+125%
|
||||
uint16_t temp=((*((uint32_t *)p))>>dec)&0x7FF;
|
||||
#ifdef FAILSAFE_ENABLE
|
||||
if(failsafe)
|
||||
Failsafe_data[i]=temp; //value range 0..2047, 0=hold, 2047=no pulses
|
||||
else
|
||||
#endif
|
||||
Servo_data[i]=(temp*5)/8+860; //value range 860<->2140 -125%<->+125%
|
||||
}
|
||||
RX_DONOTUPDTAE_off;
|
||||
#ifdef ORANGE_TX
|
||||
@ -1136,6 +1159,10 @@ void update_serial_data()
|
||||
#else
|
||||
UCSR0B |= _BV(RXCIE0) ; // RX interrupt enable
|
||||
#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]);
|
||||
#endif
|
||||
}
|
||||
|
||||
void modules_reset()
|
||||
@ -1501,7 +1528,11 @@ static uint32_t random_id(uint16_t address, uint8_t create_new)
|
||||
idx=0;discard_frame=0;
|
||||
RX_MISSED_BUFF_off; // If rx_buff was good it's not anymore...
|
||||
rx_buff[0]=UDR0;
|
||||
if((rx_buff[0]&0xFE)==0x54) // If 1st byte is 0x54 or 0x55 it looks ok
|
||||
#ifdef FAILSAFE_ENABLE
|
||||
if((rx_buff[0]&0xFC)==0x54) // If 1st byte is 0x54, 0x55, 0x56 or 0x57 it looks ok
|
||||
#else
|
||||
if((rx_buff[0]&0xFE)==0x54) // If 1st byte is 0x54 or 0x55 it looks ok
|
||||
#endif
|
||||
{
|
||||
TX_RX_PAUSE_on;
|
||||
tx_pause();
|
||||
|
||||
@ -127,7 +127,7 @@ static void __attribute__((unused)) SFHSS_calc_next_chan()
|
||||
// Values grow down and to the right.
|
||||
static void __attribute__((unused)) SFHSS_build_data_packet()
|
||||
{
|
||||
uint16_t ch1,ch2,ch3,ch4;
|
||||
uint16_t ch[4];
|
||||
// command.bit0 is the packet number indicator: =0 -> SFHSS_DATA1, =1 -> SFHSS_DATA2
|
||||
// command.bit1 is unknown but seems to be linked to the payload[0].bit0 but more dumps are needed: payload[0]=0x82 -> =0, payload[0]=0x81 -> =1
|
||||
// command.bit2 is the failsafe transmission indicator: =0 -> normal data, =1->failsafe data
|
||||
@ -139,44 +139,53 @@ static void __attribute__((unused)) SFHSS_build_data_packet()
|
||||
|
||||
uint8_t command= (phase == SFHSS_DATA1) ? 0 : 1; // Building packet for Data1 or Data2
|
||||
counter+=command;
|
||||
if( (counter&0x3FC) == 0x3FC )
|
||||
{ // Transmit failsafe data twice every 7s
|
||||
if( ((counter&1)^(command&1)) == 0 )
|
||||
command|=0x04; // Failsafe
|
||||
}
|
||||
else
|
||||
command|=0x02; // Assuming packet[0] == 0x81
|
||||
#ifdef FAILSAFE_ENABLE
|
||||
if( (counter&0x3FC) == 0x3FC && IS_FAILSAFE_VALUES_on)
|
||||
{ // Transmit failsafe data twice every 7s
|
||||
if( ((counter&1)^(command&1)) == 0 )
|
||||
command|=0x04; // Failsafe
|
||||
}
|
||||
else
|
||||
#endif
|
||||
command|=0x02; // Assuming packet[0] == 0x81
|
||||
counter&=0x3FF; // Reset failsafe counter
|
||||
if(counter&1) command|=0x08; // Transmit lower and upper channels twice in a row
|
||||
|
||||
uint8_t ch_offset = ((command&0x08) >> 1) | ((command&0x04) << 1); // CH1..CH4 or CH5..CH8, if failsafe CH9..CH12 or CH13..CH16
|
||||
ch1 = convert_channel_16b_nolim(CH_AETR[ch_offset+0],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);
|
||||
ch4 = convert_channel_16b_nolim(CH_AETR[ch_offset+3],2020,1020);
|
||||
uint8_t ch_offset = (command&0x08) >> 1; // CH1..CH4 or CH5..CH8
|
||||
|
||||
if(command&0x04)
|
||||
{ //Failsafe data are:
|
||||
// 0 to 1023 -> no output on channel
|
||||
// 1024-2047 -> hold output on channel
|
||||
// 2048-4095 -> channel_output=(data&0x3FF)*5/4+880 in µs
|
||||
// Notes:
|
||||
// 2048-2559 -> does not look valid since it only covers the range from 1520µs to 2160µs
|
||||
// 2560-3583 -> valid for any channel values from 880µs to 2160µs
|
||||
// 3584-4095 -> looks to be used for the throttle channel with values ranging from 880µs to 1520µs
|
||||
#ifdef SFHSS_FAILSAFE_CH9_16
|
||||
ch1=((5360-ch1)<<2)/5; //((1520*2-ch1)<<2)/5+1856;
|
||||
ch2=((5360-ch2)<<2)/5;
|
||||
ch3=((5360-ch3)<<2)/5;
|
||||
if((command&0x08)==0 && ch3<3072) // Throttle
|
||||
ch3+=1024;
|
||||
ch4=((5360-ch4)<<2)/5;
|
||||
#else
|
||||
ch1=1024;ch2=1024;ch4=1024; // All channels hold their positions
|
||||
ch3=((command&0x08)==0)?3664:1024; // except throttle value set to 980µs
|
||||
#endif
|
||||
}
|
||||
#ifdef FAILSAFE_ENABLE
|
||||
if(command&0x04)
|
||||
{ //Failsafe data are:
|
||||
// 0 to 1023 -> no output on channel
|
||||
// 1024-2047 -> hold output on channel
|
||||
// 2048-4095 -> channel_output=(data&0x3FF)*5/4+880 in µs
|
||||
// Notes:
|
||||
// 2048-2559 -> does not look valid since it only covers the range from 1520µs to 2160µs
|
||||
// 2560-3583 -> valid for any channel values from 880µs to 2160µs
|
||||
// 3584-4095 -> looks to be used for the throttle channel with values ranging from 880µs to 1520µs
|
||||
for(uint8_t i=0;i<4;i++)
|
||||
{
|
||||
ch[i]=Failsafe_data[CH_AETR[ch_offset+i]];
|
||||
if(ch[i]==FAILSAFE_CHANNEL_HOLD)
|
||||
ch[i]=1024;
|
||||
else if(ch[i]==FAILSAFE_CHANNEL_NOPULSES)
|
||||
ch[i]=0;
|
||||
else
|
||||
{ //Use channel value
|
||||
ch[i]=(ch[i]>>1)+2560;
|
||||
if(CH_AETR[ch_offset+i]==THROTTLE && ch[i]<3072) // Throttle
|
||||
ch[i]+=1024;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
#endif
|
||||
{ //Normal data
|
||||
for(uint8_t i=0;i<4;i++)
|
||||
ch[i] = convert_channel_16b_nolim(CH_AETR[ch_offset+i],2020,1020);
|
||||
}
|
||||
|
||||
|
||||
// XK [0]=0x81 [3]=0x00 [4]=0x00
|
||||
// T8J [0]=0x81 [3]=0x42 [4]=0x07
|
||||
// T10J [0]=0x81 [3]=0x0F [4]=0x09
|
||||
@ -186,13 +195,13 @@ static void __attribute__((unused)) SFHSS_build_data_packet()
|
||||
packet[2] = rx_tx_addr[1];
|
||||
packet[3] = 0x00; // unknown but prevents some receivers to bind if not 0
|
||||
packet[4] = 0x00; // unknown but prevents some receivers to bind if not 0
|
||||
packet[5] = (rf_ch_num << 3) | ((ch1 >> 9) & 0x07);
|
||||
packet[6] = (ch1 >> 1);
|
||||
packet[7] = (ch1 << 7) | ((ch2 >> 5) & 0x7F );
|
||||
packet[8] = (ch2 << 3) | ((ch3 >> 9) & 0x07 );
|
||||
packet[9] = (ch3 >> 1);
|
||||
packet[10] = (ch3 << 7) | ((ch4 >> 5) & 0x7F );
|
||||
packet[11] = (ch4 << 3) | ((fhss_code >> 2) & 0x07 );
|
||||
packet[5] = (rf_ch_num << 3) | ((ch[0] >> 9) & 0x07);
|
||||
packet[6] = (ch[0] >> 1);
|
||||
packet[7] = (ch[0] << 7) | ((ch[1] >> 5) & 0x7F );
|
||||
packet[8] = (ch[1] << 3) | ((ch[2] >> 9) & 0x07 );
|
||||
packet[9] = (ch[2] >> 1);
|
||||
packet[10] = (ch[2] << 7) | ((ch[3] >> 5) & 0x7F );
|
||||
packet[11] = (ch[3] << 3) | ((fhss_code >> 2) & 0x07 );
|
||||
packet[12] = (fhss_code << 6) | command;
|
||||
}
|
||||
|
||||
@ -285,7 +294,6 @@ uint16_t initSFHSS()
|
||||
|
||||
SFHSS_rf_init();
|
||||
phase = SFHSS_START;
|
||||
|
||||
return 10000;
|
||||
}
|
||||
|
||||
|
||||
@ -228,7 +228,7 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len)
|
||||
telemetry_lost=0;
|
||||
if (protocol==MODE_FRSKYX)
|
||||
{
|
||||
uint16_t lcrc = crc_x(&pkt[3], len-7 ) ;
|
||||
uint16_t lcrc = frskyX_crc_x(&pkt[3], len-7 ) ;
|
||||
|
||||
if ( ( (lcrc >> 8) == pkt[len-4]) && ( (lcrc & 0x00FF ) == pkt[len-3]) )
|
||||
{
|
||||
|
||||
@ -317,8 +317,17 @@ static void __attribute__((unused)) WK_build_beacon_pkt_2801()
|
||||
bind_state = 0x99;
|
||||
|
||||
for (uint8_t i = 0; i < 4; i++)
|
||||
{ // failsafe info: WARNING All channels are set to 0 instead of midstick and 0 for throttle
|
||||
packet[i+1] = 0;
|
||||
{
|
||||
#ifdef FAILSAFE_ENABLE
|
||||
uint16_t failsafe=Failsafe_data[CH_AETR[i + WK_last_beacon * 4]];
|
||||
if(failsafe!=FAILSAFE_CHANNEL_HOLD && IS_FAILSAFE_VALUES_on)
|
||||
{
|
||||
packet[i+1] = failsafe>>3; //0..255
|
||||
en |= 1 << i;
|
||||
}
|
||||
else
|
||||
#endif
|
||||
packet[i+1] = 0;
|
||||
}
|
||||
packet[0] = en;
|
||||
packet[5] = packet[4];
|
||||
@ -453,7 +462,8 @@ uint16_t WK_cb()
|
||||
return 1200;
|
||||
}
|
||||
|
||||
uint16_t WK_setup() {
|
||||
uint16_t WK_setup()
|
||||
{
|
||||
wk2x01_cyrf_init();
|
||||
CYRF_SetTxRxMode(TX_EN);
|
||||
|
||||
|
||||
@ -158,42 +158,23 @@
|
||||
/**************************/
|
||||
/*** FAILSAFE SETTINGS ***/
|
||||
/**************************/
|
||||
//SHFSS failsafe is by default set to all channels hold their positions except throttle forced to low (980µs)
|
||||
//You can uncomment the setting below to use channels 9(1) to 16(8) as failsafe instead
|
||||
//#define SFHSS_FAILSAFE_CH9_16
|
||||
//The module is using the same default failsafe values for all protocols which currently supports it:
|
||||
// Devo, SFHSS and AFHDS2A
|
||||
//All channels are centered except throttle which is forced low.
|
||||
//If you want to diasble failsafe globally comment the line below using "//".
|
||||
#define FAILSAFE_ENABLE
|
||||
|
||||
#define AFHDS2A_FAILSAFE
|
||||
#ifdef AFHDS2A_FAILSAFE
|
||||
/*
|
||||
Failsafe Min/Max values 962 <-> 2038
|
||||
*/
|
||||
const int8_t AFHDS2AFailsafeMIN = -105;
|
||||
const int8_t AFHDS2AFailsafeMAX = 105;
|
||||
//
|
||||
const int8_t AFHDS2AFailsafe[14]= {
|
||||
/*
|
||||
Failsafe examples
|
||||
988 <-> 2012µs -100% = 988 = 1500 + (2012-988)/2 * (-100/100) = 1500 - 512 = 988
|
||||
988 <-> 2012µs 0% = 1500 = 1500 + (2012-988)/2 * ( 0/100) = 1500 + 0 = 1500
|
||||
988 <-> 2012µs 100% = 2012 = 1500 + (2012-988)/2 * ( 100/100) = 1500 + 512 = 2012
|
||||
988 <-> 2012µs -105% = 962 = 1500 + (2012-988)/2 * (-105/100) = 1500 - 538 = 962
|
||||
*/
|
||||
/* ch 1 */ -1,
|
||||
/* ch 2 */ -1,
|
||||
/* ch 3 */ -105,
|
||||
/* ch 4 */ -1,
|
||||
/* ch 5 */ -1,
|
||||
/* ch 6 */ -1,
|
||||
/* ch 7 */ -1,
|
||||
/* ch 8 */ -1,
|
||||
/* ch 9 */ -1,
|
||||
/* ch 10 */ -1,
|
||||
/* ch 11 */ -1,
|
||||
/* ch 12 */ -1,
|
||||
/* ch 13 */ -1,
|
||||
/* ch 14 */ -1
|
||||
};
|
||||
#endif
|
||||
//Failsafe throttle low value.
|
||||
//1=-125%, 204=-100%, 1024=0%, 1843=100%, 2046=+125%
|
||||
#define FAILSAFE_THROTTLE_LOW 204
|
||||
|
||||
//The radio using serial protocol can set failsafe data (ersky9x only for now).
|
||||
// Two options are available:
|
||||
// a. replace the default failsafe data with serial failsafe data when they are received.
|
||||
// b. wait for the radio to provide failsafe before sending it. Enable advanced settings like "FAILSAFE NOT SET" or "FAILSAFE RX".
|
||||
// Option a. is the default since you have a protection even if no failsafe has been set on the radio.
|
||||
// You can force option b. by uncommenting the line below (remove the "//").
|
||||
//#define FAILSAFE_SERIAL_ONLY
|
||||
|
||||
/**************************/
|
||||
/*** TELEMETRY SETTINGS ***/
|
||||
|
||||
@ -86,7 +86,7 @@ CH5|CH6|CH7
|
||||
---|---|---
|
||||
|
||||
## FLYSKY AFHDS2A - *28*
|
||||
Extended limits supported
|
||||
Extended limits and failsafe supported
|
||||
|
||||
Telemetry enabled for battery voltage and RX&TX RSSI using FrSky Hub protocol
|
||||
|
||||
@ -213,7 +213,7 @@ CH1|CH2|CH3|CH4|CH5|CH6|CH7|CH8
|
||||
## SFHSS - *21*
|
||||
Models: Futaba RXs and XK models.
|
||||
|
||||
Extended limits supported
|
||||
Extended limits and failsafe supported
|
||||
|
||||
Option for this protocol is fine frequency tuning. This value is different for each Module. To determine this value:
|
||||
- find a value where the RX accepts to bind. A good start is to use one of these values -40, 0 and 40.
|
||||
@ -231,7 +231,7 @@ Channels 9 to 16 are used as failsafe values for the channels 1 to 8.
|
||||
# CYRF6936 RF Module
|
||||
|
||||
## DEVO - *7*
|
||||
Extended limits supported
|
||||
Extended limits and failsafe supported
|
||||
|
||||
CH1|CH2|CH3|CH4|CH5|CH6|CH7|CH8
|
||||
---|---|---|---|---|---|---|---
|
||||
@ -267,6 +267,8 @@ Autobind protocol
|
||||
Note: RX ouput will always be AETR independently of the input AETR, RETA...
|
||||
|
||||
### Sub_protocol WK2801 - *0*
|
||||
Failsafe supported.
|
||||
|
||||
This roughly corresponds to the number of channels supported, but many of the newer 6-channel receivers actually support the WK2801 protocol. It is recommended to try the WK2801 protocol 1st when working with older Walkera models before attempting the WK2601 or WK2401 mode, as the WK2801 is a superior protocol. The WK2801 protocol supports up to 8 channels.
|
||||
|
||||
CH1|CH2|CH3|CH4|CH5|CH6|CH7|CH8
|
||||
@ -583,9 +585,11 @@ GYRO: -100%=6G, +100%=3G
|
||||
### Sub_protocol HK310 - *1*
|
||||
Models: RX HK-3000, HK3100 and XY3000 (TX are HK-300, HK-310 and TL-3C)
|
||||
|
||||
CH1|CH2|CH3|CH4|CH5|CH6|CH7|CH8
|
||||
---|---|---|---|---|---|---|---
|
||||
| | |T|R|AUX|T_FSAFE|R_FSAFE|AUX_FSAFE
|
||||
Failsafe supported
|
||||
|
||||
CH1|CH2|CH3|CH4|CH5
|
||||
---|---|---|---|---
|
||||
| | |T|R|AUX
|
||||
|
||||
## KN - *9*
|
||||
CH1|CH2|CH3|CH4|CH5|CH6|CH7|CH8|CH9|CH10|CH11
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user