From 0b19fa0bdf1e5b4de5b16770e22a3cfa7996ea17 Mon Sep 17 00:00:00 2001 From: pascallanger Date: Tue, 8 Oct 2019 18:52:47 +0200 Subject: [PATCH] Disable channel mapping + telemetry inversion --- Multiprotocol/DSM_cyrf6936.ino | 4 +- Multiprotocol/FlySky_a7105.ino | 2 +- Multiprotocol/Hisky_nrf24l01.ino | 4 +- Multiprotocol/Multiprotocol.h | 11 +++-- Multiprotocol/Multiprotocol.ino | 69 ++++++++++++++++++++++++++++---- Multiprotocol/SFHSS_cc2500.ino | 4 +- Multiprotocol/SLT_nrf24l01.ino | 2 +- Multiprotocol/Telemetry.ino | 40 ++++++++++-------- 8 files changed, 101 insertions(+), 35 deletions(-) diff --git a/Multiprotocol/DSM_cyrf6936.ino b/Multiprotocol/DSM_cyrf6936.ino index fc18528..457d616 100644 --- a/Multiprotocol/DSM_cyrf6936.ino +++ b/Multiprotocol/DSM_cyrf6936.ino @@ -273,8 +273,8 @@ static void __attribute__((unused)) DSM_build_data_packet(uint8_t upper) /* Spektrum own remotes transmit normal values during bind and actually use this (e.g. Nano CP X) to select the transmitter mode (e.g. computer vs non-computer radio), so always send normal output */ #ifdef DSM_THROTTLE_KILL_CH - if(CH_TAER[idx]==THROTTLE && kill_ch<=604) - {//Activate throttle kill only if DSM_THROTTLE_KILL_CH below -50% + if(idx==CH1 && kill_ch<=604) + {//Activate throttle kill only if channel is throttle and DSM_THROTTLE_KILL_CH below -50% if(kill_ch>8)&0xFF; //high byte of servo timing(1000-2000us) diff --git a/Multiprotocol/Hisky_nrf24l01.ino b/Multiprotocol/Hisky_nrf24l01.ino index 2596c9a..519fbb2 100644 --- a/Multiprotocol/Hisky_nrf24l01.ino +++ b/Multiprotocol/Hisky_nrf24l01.ino @@ -123,8 +123,8 @@ static void __attribute__((unused)) build_ch_data() for (i = 0; i< 8; i++) { j=CH_AETR[i]; temp=convert_channel_16b_limit(j,0,1000); - if (j == THROTTLE) // It is clear that hisky's throttle stick is made reversely, so I adjust it here on purpose - temp = 1000 -temp; + if (j == CH3) // It is clear that hisky's throttle stick is made reversely, so I adjust it here on purpose + temp = 1000 - temp; if (j == CH7) temp = temp < 400 ? 0 : 3; // Gyro mode, 0 - 6 axis, 3 - 3 axis packet[i] = (uint8_t)(temp&0xFF); diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index 2cb00b8..26c4430 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -19,7 +19,7 @@ #define VERSION_MAJOR 1 #define VERSION_MINOR 3 #define VERSION_REVISION 0 -#define VERSION_PATCH_LEVEL 9 +#define VERSION_PATCH_LEVEL 10 //****************** // Protocols @@ -829,11 +829,13 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p -- 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=no pulse, 2047=hold. If failsafe is not set or RX then failsafe packets should not be sent. - Stream[26] = sub_protocol bits 6 & 7|RxNum bits 4 & 5|Disable_CH_Mapping 3|Disable_Telemetry 2|future use 1|future use 0; + Stream[26] = sub_protocol bits 6 & 7|RxNum bits 4 & 5|Disable_Telemetry 3|Disable_CH_Mapping 2|Future_Use 1|Telem_Invert 0; sub_protocol is 0..255 (bits 0..5 + bits 6..7) RxNum value is 0..63 (bits 0..3 + bits 4..5) - Disable_CH_Mapping => 0x08 0=enable, 1=disable - Disable_Telemetry => 0x04 0=enable, 1=disable + Disable_Telemetry => 0x08 0=enable, 1=disable + Disable_CH_Mapping => 0x04 0=enable, 1=disable + Future_Use => 0x02 0= , 1= + Telem_Invert => 0x01 0=normal, 1=invert Stream[27.. 36] = between 0 and 9 bytes for additional protocol data */ /* @@ -855,6 +857,7 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p -- 0x08 = Module is in binding mode 0x10 = Module waits a bind event to load the protocol 0x20 = Current protocol supports failsafe + 0x40 = Current protocol supports disable channel mapping 0x80 = Data buffer is almost full [3] major [4] minor diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino index 5bc264f..8df03c1 100644 --- a/Multiprotocol/Multiprotocol.ino +++ b/Multiprotocol/Multiprotocol.ino @@ -132,7 +132,7 @@ uint8_t num_ch; //Channel mapping for protocols uint8_t CH_AETR[]={AILERON, ELEVATOR, THROTTLE, RUDDER, CH5, CH6, CH7, CH8, CH9, CH10, CH11, CH12, CH13, CH14, CH15, CH16}; uint8_t CH_TAER[]={THROTTLE, AILERON, ELEVATOR, RUDDER, CH5, CH6, CH7, CH8, CH9, CH10, CH11, CH12, CH13, CH14, CH15, CH16}; -uint8_t CH_RETA[]={RUDDER, ELEVATOR, THROTTLE, AILERON, CH5, CH6, CH7, CH8, CH9, CH10, CH11, CH12, CH13, CH14, CH15, CH16}; +//uint8_t CH_RETA[]={RUDDER, ELEVATOR, THROTTLE, AILERON, CH5, CH6, CH7, CH8, CH9, CH10, CH11, CH12, CH13, CH14, CH15, CH16}; uint8_t CH_EATR[]={ELEVATOR, AILERON, THROTTLE, RUDDER, CH5, CH6, CH7, CH8, CH9, CH10, CH11, CH12, CH13, CH14, CH15, CH16}; // Mode_select variables @@ -1402,6 +1402,15 @@ static void protocol_init() void update_serial_data() { + static bool prev_ch_mapping=false; + #ifdef TELEMETRY + #ifdef INVERT_TELEMETRY + static bool prev_inv_telem=true; + #else + static bool prev_inv_telem=false; + #endif + #endif + RX_DONOTUPDATE_on; RX_FLAG_off; //data is being processed @@ -1473,21 +1482,67 @@ void update_serial_data() 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 + rx_ok_buff[0]&=0xFD; // Remove the failsafe flag + FAILSAFE_VALUES_on; // Failsafe data has been received debugln("Failsafe received"); } #endif - //DISABLE_CH_MAP_off; + DISABLE_CH_MAP_off; DISABLE_TELEM_off; if(rx_len>26) {//Additional flag received at the end rx_ok_buff[0]=(rx_ok_buff[26]&0xF0) | (rx_ok_buff[0]&0x0F); // Additional protocol numbers and RX_Num available -> store them in rx_ok_buff[0] - //if(rx_ok_buff[26]&0x08) - // DISABLE_CH_MAP_on; - if(rx_ok_buff[26]&0x04) + if(rx_ok_buff[26]&0x08) DISABLE_TELEM_on; + if(rx_ok_buff[26]&0x04) + DISABLE_CH_MAP_on; + #if defined TELEMETRY + if((rx_ok_buff[26]&0x01) ^ prev_inv_telem) + { //value changed + if(rx_ok_buff[26]&0x01) + { // Invert telemetry + debugln("Invert telem %d,%d",rx_ok_buff[26]&0x01,prev_inv_telem); + #ifdef ORANGE_TX + PORTC.PIN3CTRL |= 0x40 ; + #endif + #ifdef STM32_BOARD + TX_INV_on; + RX_INV_on; + #endif + } + else + { // Normal telemetry + debugln("Normal telem %d,%d",rx_ok_buff[26]&0x01,prev_inv_telem); + #ifdef ORANGE_TX + PORTC.PIN3CTRL &= 0xBF ; + #endif + #ifdef STM32_BOARD + TX_INV_off; + RX_INV_off; + #endif + } + prev_inv_telem=rx_ok_buff[26]&0x01; + } + #endif + } + + if(prev_ch_mapping!=IS_DISABLE_CH_MAP_on) + { + prev_ch_mapping=IS_DISABLE_CH_MAP_on; + if(IS_DISABLE_CH_MAP_on) + { + for(uint8_t i=0;i<4;i++) + CH_AETR[i]=CH_TAER[i]=CH_EATR[i]=i; + debugln("DISABLE_CH_MAP_on"); + } + else + { + CH_AETR[0]=AILERON;CH_AETR[1]=ELEVATOR;CH_AETR[2]=THROTTLE;CH_AETR[3]=RUDDER; + CH_TAER[0]=THROTTLE;CH_TAER[1]=AILERON;CH_TAER[2]=ELEVATOR;CH_TAER[3]=RUDDER; + CH_EATR[0]=ELEVATOR;CH_EATR[1]=AILERON;CH_EATR[2]=THROTTLE;CH_EATR[3]=RUDDER; + debugln("DISABLE_CH_MAP_off"); + } } 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) ) ) diff --git a/Multiprotocol/SFHSS_cc2500.ino b/Multiprotocol/SFHSS_cc2500.ino index 7f9dbd9..8588f00 100644 --- a/Multiprotocol/SFHSS_cc2500.ino +++ b/Multiprotocol/SFHSS_cc2500.ino @@ -173,8 +173,8 @@ static void __attribute__((unused)) SFHSS_build_data_packet() else { //Use channel value ch[i]=(ch[i]>>1)+2560; - if(CH_AETR[ch_offset+i]==THROTTLE && ch[i]<3072) // Throttle - ch[i]+=1024; + //if(IS_DISABLE_CH_MAP_off && ch_offset+i==CH3 && ch[i]<3072) // Throttle + // ch[i]+=1024; } } } diff --git a/Multiprotocol/SLT_nrf24l01.ino b/Multiprotocol/SLT_nrf24l01.ino index 3ce909f..11ee902 100644 --- a/Multiprotocol/SLT_nrf24l01.ino +++ b/Multiprotocol/SLT_nrf24l01.ino @@ -141,7 +141,7 @@ static void __attribute__((unused)) SLT_build_packet() for (uint8_t i = 0; i < 4; ++i) { uint16_t v = convert_channel_10b(CH_AETR[i]); - if(sub_protocol>SLT_V2 && (CH_AETR[i]==THROTTLE || CH_AETR[i]==ELEVATOR) ) + if(sub_protocol>SLT_V2 && (i==CH1 || i==CH3) ) v=1023-v; // reverse throttle and elevator channels for Q100/Q200/MR100 protocols packet[i] = v; e = (e >> 2) | (uint8_t) ((v >> 2) & 0xC0); diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino index 3b8d421..5dca7dd 100644 --- a/Multiprotocol/Telemetry.ino +++ b/Multiprotocol/Telemetry.ino @@ -94,23 +94,31 @@ static void multi_send_status() else if (IS_BIND_IN_PROGRESS) flags |= 0x08; - #ifdef FAILSAFE_ENABLE - //Is failsafe supported? - switch (protocol) - { - case PROTO_HISKY: - if(sub_protocol!=HK310) - break; - case PROTO_AFHDS2A: - case PROTO_DEVO: - case PROTO_SFHSS: - case PROTO_WK2x01: - case PROTO_FRSKYX: - flags |= 0x20; //Yes - default: + switch (protocol) + { + case PROTO_HISKY: + if(sub_protocol!=HK310) break; - } - #endif + case PROTO_AFHDS2A: + case PROTO_DEVO: + case PROTO_SFHSS: + case PROTO_WK2x01: + #ifdef FAILSAFE_ENABLE + flags |= 0x20; //Failsafe supported + #endif + case PROTO_DSM: + case PROTO_SLT: + case PROTO_FLYSKY: + case PROTO_ESKY: + case PROTO_J6PRO: + flags |= 0x40; //Disable_ch_mapping supported + break; + case PROTO_FRSKYX: + #ifdef FAILSAFE_ENABLE + flags |= 0x20; //Failsafe supported + #endif + break; + } if(IS_DATA_BUFFER_LOW_on) flags |= 0x80; }