From 3e9359b6be35637480e118a0621ee42a00960486 Mon Sep 17 00:00:00 2001 From: Arne Schwabe Date: Thu, 4 May 2017 05:37:41 +0200 Subject: [PATCH] Allow setting config parameters from TX and implement TX commands --- Multiprotocol/AFHDS2A_a7105.ino | 4 +- Multiprotocol/Devo_cyrf6936.ino | 1 + Multiprotocol/Multiprotocol.h | 40 ++++++++-- Multiprotocol/Multiprotocol.ino | 137 ++++++++++++++++++++++++-------- Multiprotocol/Telemetry.ino | 6 +- Multiprotocol/_Config.h | 7 +- 6 files changed, 151 insertions(+), 44 deletions(-) diff --git a/Multiprotocol/AFHDS2A_a7105.ino b/Multiprotocol/AFHDS2A_a7105.ino index 4cb1675..f9d8dd0 100644 --- a/Multiprotocol/AFHDS2A_a7105.ino +++ b/Multiprotocol/AFHDS2A_a7105.ino @@ -245,7 +245,7 @@ uint16_t ReadAFHDS2A() A7105_ReadData(AFHDS2A_RXPACKET_SIZE); if(packet[0] == 0xbc && packet[9] == 0x01) { - uint8_t temp=50+RX_num*4; + uint8_t temp=AFHDS2A_EEPROM_OFFSET+RX_num*4; uint8_t i; for(i=0; i<4; i++) { @@ -361,7 +361,7 @@ uint16_t initAFHDS2A() { phase = AFHDS2A_DATA; //Read RX ID from EEPROM based on RX_num, RX_num must be uniq for each RX - uint8_t temp=50+RX_num*4; + uint8_t temp=AFHDS2A_EEPROM_OFFSET+RX_num*4; for(uint8_t i=0;i<4;i++) rx_id[i]=eeprom_read_byte((EE_ADDR)(temp+i)); } diff --git a/Multiprotocol/Devo_cyrf6936.ino b/Multiprotocol/Devo_cyrf6936.ino index d848a00..467e123 100644 --- a/Multiprotocol/Devo_cyrf6936.ino +++ b/Multiprotocol/Devo_cyrf6936.ino @@ -63,6 +63,7 @@ static void __attribute__((unused)) DEVO_add_pkt_suffix() BIND_SET_PULLUP; // set pullup if(IS_BIND_BUTTON_on) { + eeprom_write_byte((EE_ADDR)(MODELMODE_EEPROM_OFFSET+mode_select),0x01); // Set fixed id mode for the current model option=1; } diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index b45870c..4e600d6 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -243,7 +243,6 @@ struct PPM_Parameters }; // Telemetry - enum MultiPacketTypes { MULTI_TELEMETRY_STATUS = 1, @@ -254,7 +253,7 @@ enum MultiPacketTypes MULTI_TELEMETRY_AFHDS2A = 6, MULTI_TELEMETRY_CONFIG = 7, MULTI_TELEMETRY_SYNC = 8, - MULTI_TELEMETRY_SPORT_POLLING = 9, + MULTI_COMMAND_CONFIG = 0x80, }; // Macros @@ -331,6 +330,12 @@ enum MultiPacketTypes #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 FAILSAFE_CHANNEL_HOLD 2047 #define FAILSAFE_CHANNEL_NOPULSES 0 @@ -489,9 +494,9 @@ enum { #define SPEED_125K 3 /** EEPROM Layout */ -#define EEPROM_ID_OFFSET 10 // Module ID (4 bytes) -#define EEPROM_ID_VALID_OFFSET 20 // 1 byte flag that ID is valid -#define MODELMODE_EEPROM_OFFSET 30 // Autobind mode, 1 byte per model, end is 46 +#define EEPROM_ID_OFFSET 10 // Module ID (4 bytes) +#define EEPROM_ID_VALID_OFFSET 20 // 1 byte flag that ID is valid +#define MODELMODE_EEPROM_OFFSET 30 // Autobind mode, 1 byte per model, end is 46 #define AFHDS2A_EEPROM_OFFSET 50 // RX ID, 4 byte per model id, end is 114 #define CONFIG_EEPROM_OFFSET 120 // Current configuration of the multimodule @@ -769,4 +774,29 @@ 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 + */ diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino index 9d97a4a..fc7bb2d 100644 --- a/Multiprotocol/Multiprotocol.ino +++ b/Multiprotocol/Multiprotocol.ino @@ -134,6 +134,14 @@ const uint8_t CH_EATR[]={ELEVATOR, AILERON, THROTTLE, RUDDER, CH5, CH6, CH7, CH8 uint8_t mode_select; uint8_t protocol_flags=0,protocol_flags2=0; +#if defined(ALLOW_CONFIGURATION) +uint8_t multi_config; +#elif defined(INVERT_TELEMETRY) +const uint8_t multi_config = 0x1; +#else +const uint8_t multi_config = 0x0; +#endif + #ifdef ENABLE_PPM // PPM variable volatile uint16_t PPM_data[NUM_CHN]; @@ -221,6 +229,9 @@ void setup() while (!Serial); // Wait for ever for the serial port to connect... debugln("Multiprotocol version: %d.%d.%d.%d", VERSION_MAJOR, VERSION_MINOR, VERSION_REVISION, VERSION_PATCH_LEVEL); #endif + #if defined(ALLOW_CONFIGURATION) + read_multimodule_config(); + #endif // General pinout #ifdef ORANGE_TX @@ -253,15 +264,6 @@ void setup() pinMode(PE2_pin,OUTPUT); pinMode(TX_INV_pin,OUTPUT); pinMode(RX_INV_pin,OUTPUT); - #if defined TELEMETRY - #if defined INVERT_SERIAL - TX_INV_on; //activate inverter for both serial TX and RX signals - RX_INV_on; - #else - TX_INV_off; - RX_INV_off; - #endif - #endif pinMode(BIND_pin,INPUT_PULLUP); pinMode(PPM_pin,INPUT); pinMode(S1_pin,INPUT_PULLUP);//dial switch @@ -760,11 +762,11 @@ static void protocol_init() telemetry_lost=1; #ifdef BASH_SERIAL TIMSK0 = 0 ; // Stop all timer 0 interrupts - #ifdef INVERT_SERIAL + if (IS_TELEMTRY_INVERSION_ON) SERIAL_TX_off; - #else + else SERIAL_TX_on; - #endif + SerialControl.tail=0; SerialControl.head=0; SerialControl.busy=0; @@ -1090,7 +1092,7 @@ static void protocol_init() BIND_BUTTON_FLAG_off; // do not bind/reset id anymore even if protocol change } -void update_serial_data() +void parse_multi_frame() { RX_DONOTUPDATE_on; RX_FLAG_off; //data is being processed @@ -1143,6 +1145,9 @@ void update_serial_data() rx_ok_buff[0]&=0xFD; //remove the failsafe flag FAILSAFE_VALUES_on; //failsafe data has been received } + #else + if(rx_ok_buff[0]&0x02) // Failsafe data, ignore + return; #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 @@ -1192,13 +1197,30 @@ void update_serial_data() p++; uint16_t temp=((*((uint32_t *)p))>>dec)&0x7FF; #ifdef FAILSAFE_ENABLE - if(failsafe) - Failsafe_data[i]=temp; //value range 0..2047, 0=no pulses, 2047=hold - else + if(failsafe) { + Failsafe_data[i]=temp; //value range 0..2047, 0=no pulses, 2047=hold + debugln("RX_FS:%d,%d,%d,%d",Failsafe_data[0],Failsafe_data[1],Failsafe_data[2],Failsafe_data[3]); + } else #endif Channel_data[i]=temp; //value range 0..2047, 0=-125%, 2047=+125% } - RX_DONOTUPDATE_off; +} + +void update_serial_data() +{ + RX_DONOTUPDTAE_on; + RX_FLAG_off; //data is being processed + if (rx_ok_buff[0] == 'M') +#ifdef ALLOW_CONFIGURATION + parse_serial_multi_command(); +#else + ; +#endif + else + parse_multi_frame(); + + RX_DONOTUPDTAE_off; +>>>>>>> Allow setting config parameters from TX and implement TX commands #ifdef ORANGE_TX cli(); #else @@ -1208,18 +1230,64 @@ void update_serial_data() { memcpy((void*)rx_ok_buff,(const void*)rx_buff,RXBUFFER_SIZE);// Duplicate the buffer RX_FLAG_on; // data to be processed next time... RX_MISSED_BUFF_off; - } + } #ifdef ORANGE_TX sei(); #else UCSR0B |= _BV(RXCIE0) ; // RX interrupt enable #endif - #ifdef FAILSAFE_ENABLE - if(failsafe) - debugln("RX_FS:%d,%d,%d,%d",Failsafe_data[0],Failsafe_data[1],Failsafe_data[2],Failsafe_data[3]); - #endif } +#if defined(ALLOW_CONFIGURATION) +void read_multimodule_config() { +// Check if we have valid eeprom contents + if (eeprom_read_byte((EE_ADDR)(EEPROM_ID_VALID_OFFSET)) == 0xf0) + { + multi_config = eeprom_read_byte((EE_ADDR)(CONFIG_EEPROM_OFFSET)); + } + else + { + // Set default values that should fit er9x/erksy9x + // OpenTX will override the values anyway + multi_config = 0x00; + eeprom_write_byte(CONFIG_EEPROM_OFFSET, multi_config); + } +} + +void parse_serial_multi_command() +{ + // Header 'M', 'P, Type, Len + if (rx_buff[1] != 'P') + return; + uint8_t len = rx_buff[3]; + switch (rx_buff[2]) { +#ifdef ALLOW_CONFIGURATION + case MULTI_COMMAND_CONFIG: { + if (len == 0) + return; + + uint8_t newconfig = rx_buff[4]; + + // Echo back config + multi_send_header (MULTI_COMMAND_CONFIG, 2); + Serial_write (multi_config); + Serial_write (newconfig); + if (newconfig != multi_config) + { + multi_config = newconfig; + eeprom_write_byte(CONFIG_EEPROM_OFFSET, newconfig); + // Reinit serial port to enable/disable inversion + Mprotocol_serial_init(); + } + } + break; +#endif + default: + ; + } +} +#endif + void modules_reset() { #ifdef CC2500_INSTALLED @@ -1257,10 +1325,16 @@ void modules_reset() USARTC0.CTRLA = (USARTC0.CTRLA & 0xCC) | 0x11 ; USARTC0.CTRLC = 0x2B ; UDR0 ; - #ifdef INVERT_SERIAL + if (IS_TELEMTRY_INVERSION_ON) PORTC.PIN3CTRL |= 0x40 ; - #endif #ifdef CHECK_FOR_BOOTLOADER + if (IS_TELEMTRY_INVERSION_ON) { + TX_INV_on;//activated inverter for both serial TX and RX signals + RX_INV_on; + } else { + TX_INV_off; + RX_INV_off; + } if ( boot ) { USARTC0.BAUDCTRLB = 0 ; @@ -1566,12 +1640,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; - #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 + + if((rx_buff[0]&0xFC)==0x54 || rx_buff[0]=='M') // If 1st byte is 0x54, 0x55, 0x56 or 0x57 or 'M' it looks ok { + // Note that we also flag failsafe and multi command okay as first byte and throw it away (after 3,25ms) + // so we don't sync to the wrong byte if the features are not enabled TX_RX_PAUSE_on; tx_pause(); #if defined STM32_BOARD @@ -1589,9 +1662,9 @@ static uint32_t random_id(uint16_t address, uint8_t create_new) else { rx_buff[idx++]=UDR0; // Store received byte - if(idx>=RXBUFFER_SIZE) - { // A full frame has been received - if(!IS_RX_DONOTUPDATE_on) + if(idx>=RXBUFFER_SIZE || (rx_buff[0] == 'M' && idx >= 4 && rx_buff[3] == idx-4)) // A full frame has been received or a long enough multicommand message + { + if(!IS_RX_DONOTUPDTAE_on) { //Good frame received and main is not working on the buffer memcpy((void*)rx_ok_buff,(const void*)rx_buff,RXBUFFER_SIZE);// Duplicate the buffer RX_FLAG_on; // flag for main to process servo data diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino index 58bae07..f5b3b26 100644 --- a/Multiprotocol/Telemetry.ino +++ b/Multiprotocol/Telemetry.ino @@ -79,12 +79,10 @@ uint8_t frame[18]; static void multi_send_header(uint8_t type, uint8_t len) { Serial_write('M'); - #ifdef MULTI_TELEMETRY + if (IS_MULTI_TELEMETRY_ON) { Serial_write('P'); Serial_write(type); - #else - (void)type; - #endif + } Serial_write(len); } diff --git a/Multiprotocol/_Config.h b/Multiprotocol/_Config.h index 6563ff9..09bc283 100644 --- a/Multiprotocol/_Config.h +++ b/Multiprotocol/_Config.h @@ -217,6 +217,11 @@ /**************************/ //In this section you can configure the telemetry. +// Allow settings of configuration from TX side (serial inversion (not on AVR!), and telemetry type) +// Allows the same firmware to be used for ersky9x/9x and OpenTX, defaults to er9x/ersky9x when no configuration from +// TX is sent +#define ALLOW_CONFIGURATION + //If you do not plan using the telemetry comment this global setting using "//" and skip to the next section. #define TELEMETRY @@ -234,7 +239,7 @@ //Uncomment to send Multi status and allow OpenTX to autodetect the telemetry format //Supported by OpenTX version 2.2 RC9 and newer. NOT supported by er9x/ersky9x use MULTI_STATUS instead. -//#define MULTI_TELEMETRY +#define MULTI_TELEMETRY //Comment a line to disable a specific protocol telemetry #define DSM_TELEMETRY // Forward received telemetry packet directly to TX to be decoded