From 7d41017850fc387173a0f19bbc67f1cc01d9f49c Mon Sep 17 00:00:00 2001 From: Pascal Langer Date: Fri, 24 Nov 2017 23:01:47 +0100 Subject: [PATCH] Updates Implemented debug output on uart1 for stm32 Replaced hardcoded eeprom offsets with documented constats Fixed a bug affecting telemetry on Atmega328p using the invert_telemetry flag. --- Multiprotocol/Devo_cyrf6936.ino | 2 +- Multiprotocol/Multiprotocol.h | 103 +++++++++- Multiprotocol/Multiprotocol.ino | 76 +++++-- Multiprotocol/Pins.h | 52 ++--- Multiprotocol/Symax_nrf24l01.ino | 4 +- Multiprotocol/Telemetry.ino | 321 +++++++++++------------------- Multiprotocol/WK2x01_cyrf6936.ino | 2 +- 7 files changed, 294 insertions(+), 266 deletions(-) diff --git a/Multiprotocol/Devo_cyrf6936.ino b/Multiprotocol/Devo_cyrf6936.ino index e37e551..8f536af 100644 --- a/Multiprotocol/Devo_cyrf6936.ino +++ b/Multiprotocol/Devo_cyrf6936.ino @@ -63,7 +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)(30+mode_select),0x01); // Set fixed id mode for the current model + eeprom_write_byte((EE_ADDR)(MODELMODE_EEPROM_OFFSET+mode_select),0x01); // Set fixed id mode for the current model option=1; } BIND_SET_OUTPUT; diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index 835eb56..3387e8e 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -19,7 +19,7 @@ #define VERSION_MAJOR 1 #define VERSION_MINOR 1 #define VERSION_REVISION 6 -#define VERSION_PATCH_LEVEL 29 +#define VERSION_PATCH_LEVEL 30 //****************** // Protocols //****************** @@ -224,14 +224,29 @@ struct PPM_Parameters // Telemetry enum MultiPacketTypes { - MULTI_TELEMETRY_STATUS = 1, - MULTI_TELEMETRY_SPORT = 2, - MULTI_TELEMETRY_HUB = 3, - MULTI_TELEMETRY_DSM = 4, - MULTI_TELEMETRY_DSMBIND = 5, - MULTI_TELEMETRY_AFHDS2A = 6, + MULTI_TELEMETRY_STATUS = 1, + MULTI_TELEMETRY_SPORT = 2, + MULTI_TELEMETRY_HUB = 3, + 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") @@ -306,6 +321,23 @@ 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 failsafeToPPM(i) (Failsafe_data[i]* 5/8+860) +#define isNormalFailsafeChanel(i) (Failsafe_data[i] != FAILSAFE_CHANNEL_HOLD && Failsafe_data[i] != FAILSAFE_CHANNEL_NOPULSES) + + +//Status messages +#if defined(STM32_BOARD) && defined (SERIAL_DEBUG) + #define debug(msg, ...) {char buf[64]; sprintf(buf, msg "\r\n", ##__VA_ARGS__); for(int i=0;buf[i] !=0; i++) StatusSerial_write(buf[i]);} +#else + #define debug(...) + #undef SERIAL_DEBUG +#endif //******************** //*** Blink timing *** @@ -443,6 +475,14 @@ enum { #define SPEED_57600 2 #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 AFHDS2A_EEPROM_OFFSET 50 // RX ID, 4 byte per model id, end is 114 +#define CONFIG_EEPROM_OFFSET 120 // Current configuration of the multimodule + + //**************************************** //*** MULTI protocol serial definition *** //**************************************** @@ -637,7 +677,7 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p -- version of multi code, should be displayed as major.minor.revision.patchlevel */ /* - Multiprotocol telemetry definition for OpenTX + Multiprotocol telemetry/command definition for OpenTX Based on #define MULTI_TELEMETRY enables OpenTX to get the multimodule status and select the correct telemetry type automatically. Serial: 100000 Baud 8e2 (same as input) @@ -657,6 +697,8 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p -- [4-xx] data + Commands from TX to multi cannot be longer than 22 bytes (RXLen -4byte header) + Type = 0x01 Multimodule Status: [4] Flags 0x01 = Input signal detected @@ -686,12 +728,53 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p -- Type 0x05 DSM bind data data[0-16] DSM bind data - technically DSM bind data is only 10 bytes but multi send 16 - like with telemtry, check length field) + technically DSM bind data is only 10 bytes but multi sends 16 + like with telemtery, check length field) Type 0x06 Flysky AFHDS2 telemetry data length: 29 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 + */ diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino index e634ef3..0703c71 100644 --- a/Multiprotocol/Multiprotocol.ino +++ b/Multiprotocol/Multiprotocol.ino @@ -21,20 +21,23 @@ along with Multiprotocol. If not, see . */ #include + //#define DEBUG_TX +//#define SERIAL_DEBUG // Only for STM32_BOARD on usart1 + #define USE_MY_CONFIG -#ifdef ARDUINO_AVR_XMEGA32D4 -#include "MultiOrange.h" + +#ifdef __arm__// Let's automatically select the board if arm is selected + #define STM32_BOARD #endif +#ifdef ARDUINO_AVR_XMEGA32D4 + #include "MultiOrange.h" +#endif + #include "Multiprotocol.h" //Multiprotocol module configuration file #include "_Config.h" -// Let's automatically select the board -// if arm is selected -#ifdef __arm__ - #define STM32_BOARD -#endif //Personal config file #if defined USE_MY_CONFIG @@ -58,6 +61,9 @@ void ISR_COMPB(); extern "C" { + #ifdef SERIAL_DEBUG + void __irq_usart1(void); + #endif void __irq_usart2(void); void __irq_usart3(void); } @@ -184,6 +190,11 @@ uint8_t pkt[MAX_PKT];//telemetry receiving packets volatile uint8_t tx_head=0; volatile uint8_t tx_tail=0; #endif // BASH_SERIAL + #ifdef SERIAL_DEBUG + volatile uint8_t tx_debug_buff[TXBUFFER_SIZE]; + volatile uint8_t tx_debug_head=0; + volatile uint8_t tx_debug_tail=0; + #endif // SERIAL_DEBUG uint8_t v_lipo1; uint8_t v_lipo2; uint8_t RX_RSSI; @@ -193,7 +204,7 @@ uint8_t pkt[MAX_PKT];//telemetry receiving packets uint8_t telemetry_link=0; uint8_t telemetry_counter=0; uint8_t telemetry_lost; -#endif +#endif // TELEMETRY // Callback typedef uint16_t (*void_function_t) (void);//pointer to a function with no parameters which return an uint16_t integer @@ -202,6 +213,13 @@ void_function_t remote_callback = 0; // Init void setup() { + // Setup diagnostic uart before anything else + #ifdef SERIAL_DEBUG + usart1_begin(115200,SERIAL_8N1); + tx_debug_resume(); + debug("Multiprotocol version: %d.%d.%d.%d", VERSION_MAJOR, VERSION_MINOR, VERSION_REVISION, VERSION_PATCH_LEVEL); + #endif + // General pinout #ifdef ORANGE_TX //XMEGA @@ -227,14 +245,15 @@ void setup() pinMode(CC25_CSN_pin,OUTPUT); pinMode(NRF_CSN_pin,OUTPUT); pinMode(CYRF_CSN_pin,OUTPUT); + pinMode(SPI_CSN_pin,OUTPUT); pinMode(CYRF_RST_pin,OUTPUT); pinMode(PE1_pin,OUTPUT); pinMode(PE2_pin,OUTPUT); + pinMode(TX_INV_pin,OUTPUT); + pinMode(RX_INV_pin,OUTPUT); #if defined TELEMETRY - pinMode(TX_INV_pin,OUTPUT); - pinMode(RX_INV_pin,OUTPUT); #if defined INVERT_SERIAL - TX_INV_on;//activated inverter for both serial TX and RX signals + TX_INV_on; //activate inverter for both serial TX and RX signals RX_INV_on; #else TX_INV_off; @@ -287,7 +306,7 @@ void setup() // Timer1 config TCCR1A = 0; TCCR1B = (1 << CS11); //prescaler8, set timer1 to increment every 0.5us(16Mhz) and start timer - + // Random random_init(); #endif @@ -341,6 +360,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); // Update LED LED_off; @@ -360,6 +380,8 @@ void setup() // Read or create protocol id MProtocol_id_master=random_id(10,false); + + debug("Module Id: %lx", MProtocol_id_master); #ifdef ENABLE_PPM //Protocol and interrupts initialization @@ -415,6 +437,7 @@ void setup() #endif //ENABLE_SERIAL } servo_mid=servo_min_100+servo_max_100; //In fact 2* mid_value + debug("init complete"); } // Main @@ -678,6 +701,18 @@ inline void tx_resume() #endif } +#ifdef SERIAL_DEBUG + inline void tx_debug_resume() + { + USART1_BASE->CR1 |= USART_CR1_TXEIE; + } + + inline void tx_debug_pause() + { + USART1_BASE->CR1 &= ~ USART_CR1_TXEIE; + } +#endif // SERIAL_DEBUG + #ifdef STM32_BOARD void start_timer2() { @@ -764,7 +799,7 @@ static void protocol_init() #if defined(HUBSAN_A7105_INO) case MODE_HUBSAN: PE1_off; //antenna RF1 - if(IS_BIND_BUTTON_FLAG_on) random_id(10,true); // Generate new ID if bind button is pressed. + if(IS_BIND_BUTTON_FLAG_on) random_id(EEPROM_ID_OFFSET,true); // Generate new ID if bind button is pressed. next_callback = initHubsan(); remote_callback = ReadHubsan; break; @@ -820,12 +855,12 @@ static void protocol_init() { if(IS_BIND_BUTTON_FLAG_on) { - eeprom_write_byte((EE_ADDR)(30+mode_select),0x00); // reset to autobind mode for the current model + eeprom_write_byte((EE_ADDR)(MODELMODE_EEPROM_OFFSET+mode_select),0x00); // reset to autobind mode for the current model option=0; } else { - option=eeprom_read_byte((EE_ADDR)(30+mode_select)); // load previous mode: autobind or fixed id + option=eeprom_read_byte((EE_ADDR)(MODELMODE_EEPROM_OFFSET+mode_select)); // load previous mode: autobind or fixed id if(option!=1) option=0; // if not fixed id mode then it should be autobind } } @@ -842,12 +877,12 @@ static void protocol_init() { if(IS_BIND_BUTTON_FLAG_on) { - eeprom_write_byte((EE_ADDR)(30+mode_select),0x00); // reset to autobind mode for the current model + eeprom_write_byte((EE_ADDR)(MODELMODE_EEPROM_OFFSET+mode_select),0x00); // reset to autobind mode for the current model option=0; } else { - option=eeprom_read_byte((EE_ADDR)(30+mode_select)); // load previous mode: autobind or fixed id + option=eeprom_read_byte((EE_ADDR)(MODELMODE_EEPROM_OFFSET+mode_select)); // load previous mode: autobind or fixed id if(option!=1) option=0; // if not fixed id mode then it should be autobind } } @@ -1051,6 +1086,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); } else if( ((rx_ok_buff[1]&0x80)!=0) && ((cur_protocol[1]&0x80)==0) ) // Bind flag has been set @@ -1070,7 +1106,7 @@ void update_serial_data() //store current protocol values for(uint8_t i=0;i<3;i++) cur_protocol[i] = rx_ok_buff[i]; - + // decode channel values volatile uint8_t *p=rx_ok_buff+3; uint8_t dec=-3; @@ -1363,7 +1399,7 @@ static uint32_t random_id(uint16_t address, uint8_t create_new) { id<<=8; id|=eeprom_read_byte((EE_ADDR)address+i-1); - } + } if(id!=0x2AD141A7) //ID with seed=0 return id; } @@ -1379,7 +1415,7 @@ static uint32_t random_id(uint16_t address, uint8_t create_new) { eeprom_write_byte((EE_ADDR)address+i,id); id>>=8; - } + } eeprom_write_byte((EE_ADDR)(address+10),0xf0);//write bind flag in eeprom. return id; #else diff --git a/Multiprotocol/Pins.h b/Multiprotocol/Pins.h index 9850055..e455326 100644 --- a/Multiprotocol/Pins.h +++ b/Multiprotocol/Pins.h @@ -209,14 +209,14 @@ #endif #else //STM32_BOARD #define BIND_pin PA0 - #define LED_pin PA1 + #define LED_pin PA1 // - #define PPM_pin PA8 //PPM 5V tolerant + #define PPM_pin PA8 //PPM 5V tolerant // - #define S1_pin PA4 //Dial switch pins - #define S2_pin PA5 + #define S1_pin PA4 //Dial switch pins + #define S2_pin PA5 #define S3_pin PA6 - #define S4_pin PA7 + #define S4_pin PA7 // #define PE1_pin PB4 //PE1 #define PE2_pin PB5 //PE2 @@ -226,10 +226,11 @@ #define CYRF_RST_pin PB8 //CYRF RESET #define A7105_CSN_pin PB9 //A7105 #define CYRF_CSN_pin PB12 //CYRF CSN + #define SPI_CSN_pin PA15 //SPI pins #define SCK_pin PB13 //SCK #define SDO_pin PB14 //MISO - #define SDI_pin PB15 //MOSI + #define SDI_pin PB15 //MOSI // #define TX_INV_pin PB3 #define RX_INV_pin PB1 @@ -240,40 +241,43 @@ #define PE2_on digitalWrite(PE2_pin,HIGH) #define PE2_off digitalWrite(PE2_pin,LOW) - #define A7105_CSN_on digitalWrite(A7105_CSN_pin,HIGH) - #define A7105_CSN_off digitalWrite(A7105_CSN_pin,LOW) + #define A7105_CSN_on digitalWrite(A7105_CSN_pin,HIGH) + #define A7105_CSN_off digitalWrite(A7105_CSN_pin,LOW) #define NRF_CE_on #define NRF_CE_off - #define SCK_on digitalWrite(SCK_pin,HIGH) - #define SCK_off digitalWrite(SCK_pin,LOW) + #define SCK_on digitalWrite(SCK_pin,HIGH) + #define SCK_off digitalWrite(SCK_pin,LOW) - #define SDI_on digitalWrite(SDI_pin,HIGH) - #define SDI_off digitalWrite(SDI_pin,LOW) + #define SDI_on digitalWrite(SDI_pin,HIGH) + #define SDI_off digitalWrite(SDI_pin,LOW) - #define SDI_1 (digitalRead(SDI_pin)==HIGH) - #define SDI_0 (digitalRead(SDI_pin)==LOW) + #define SDI_1 (digitalRead(SDI_pin)==HIGH) + #define SDI_0 (digitalRead(SDI_pin)==LOW) - #define CC25_CSN_on digitalWrite(CC25_CSN_pin,HIGH) - #define CC25_CSN_off digitalWrite(CC25_CSN_pin,LOW) + #define CC25_CSN_on digitalWrite(CC25_CSN_pin,HIGH) + #define CC25_CSN_off digitalWrite(CC25_CSN_pin,LOW) - #define NRF_CSN_on digitalWrite(NRF_CSN_pin,HIGH) - #define NRF_CSN_off digitalWrite(NRF_CSN_pin,LOW) + #define NRF_CSN_on digitalWrite(NRF_CSN_pin,HIGH) + #define NRF_CSN_off digitalWrite(NRF_CSN_pin,LOW) - #define CYRF_CSN_on digitalWrite(CYRF_CSN_pin,HIGH) + #define CYRF_CSN_on digitalWrite(CYRF_CSN_pin,HIGH) #define CYRF_CSN_off digitalWrite(CYRF_CSN_pin,LOW) - #define CYRF_RST_HI digitalWrite(CYRF_RST_pin,HIGH) //reset cyrf + #define SPI_CSN_on digitalWrite(SPI_CSN_pin,HIGH) + #define SPI_CSN_off digitalWrite(SPI_CSN_pin,LOW) + + #define CYRF_RST_HI digitalWrite(CYRF_RST_pin,HIGH) //reset cyrf #define CYRF_RST_LO digitalWrite(CYRF_RST_pin,LOW) // - #define SDO_1 (digitalRead(SDO_pin)==HIGH) - #define SDO_0 (digitalRead(SDO_pin)==LOW) + #define SDO_1 (digitalRead(SDO_pin)==HIGH) + #define SDO_0 (digitalRead(SDO_pin)==LOW) - #define TX_INV_on digitalWrite(TX_INV_pin,HIGH) + #define TX_INV_on digitalWrite(TX_INV_pin,HIGH) #define TX_INV_off digitalWrite(TX_INV_pin,LOW) - #define RX_INV_on digitalWrite(RX_INV_pin,HIGH) + #define RX_INV_on digitalWrite(RX_INV_pin,HIGH) #define RX_INV_off digitalWrite(RX_INV_pin,LOW) #define LED_on digitalWrite(LED_pin,HIGH) diff --git a/Multiprotocol/Symax_nrf24l01.ino b/Multiprotocol/Symax_nrf24l01.ino index b7fbb58..4df3f56 100644 --- a/Multiprotocol/Symax_nrf24l01.ino +++ b/Multiprotocol/Symax_nrf24l01.ino @@ -81,7 +81,7 @@ static void __attribute__((unused)) SYMAX_read_controls() if (Servo_AUX5) { flags |= SYMAX_FLAG_HEADLESS; - flags &= ~SYMAX_XTRM_RATES; // Extended rates & headless incompatible + flags &= ~SYMAX_XTRM_RATES; // Extended rates & headless incompatible } } @@ -158,7 +158,7 @@ static void __attribute__((unused)) SYMAX_build_packet(uint8_t bind) packet[6] = flags & SYMAX_FLAG_FLIP ? 0x40 : 0x00; packet[7] = flags & SYMAX_FLAG_HEADLESS ? 0x80 : 0x00; if (flags & SYMAX_XTRM_RATES) - { // use trims to extend controls + { // use trims to extend controls packet[5] |= elevator >> 2; packet[6] |= rudder >> 2; packet[7] |= aileron >> 2; diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino index 8997161..90cb6f6 100644 --- a/Multiprotocol/Telemetry.ino +++ b/Multiprotocol/Telemetry.ino @@ -20,54 +20,47 @@ uint8_t RetrySequence ; #if ( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) ) - #define MULTI_TIME 500 //in ms + #define MULTI_TIME 500 //in ms + #define INPUT_SYNC_TIME 100 //in ms + #define INPUT_ADDITIONAL_DELAY 100 // in 10µs, 100 => 1000 µs uint32_t lastMulti = 0; -#endif +#endif // MULTI_TELEMETRY/MULTI_STATUS #if defined SPORT_TELEMETRY - #define SPORT_TIME 12000 //12ms + #define SPORT_TIME 12000 //12ms #define FRSKY_SPORT_PACKET_SIZE 8 + #define FX_BUFFERS 4 uint32_t last = 0; uint8_t sport_counter=0; uint8_t RxBt = 0; uint8_t sport = 0; -#define MAX_PKTX 10 -#define FX_BUFFERS 4 -uint8_t pktx[MAX_PKTX]; -uint8_t pktx1[FRSKY_SPORT_PACKET_SIZE*FX_BUFFERS]; -uint8_t indx; -//struct t_fx_rx_packet -//{ -// uint8_t validSequence ; -// uint8_t count ; -// uint8_t payload[6] ; -//} ; + uint8_t pktx1[FRSKY_SPORT_PACKET_SIZE*FX_BUFFERS]; -// Store for out of sequence packet -//struct t_fx_rx_packet FrskyxRxTelemetry ; + // Store for out of sequence packet + uint8_t FrskyxRxTelemetryValidSequence ; + struct t_fx_rx_frame + { + uint8_t valid ; + uint8_t count ; + uint8_t payload[6] ; + } ; -uint8_t FrskyxRxTelemetryValidSequence ; + // Store for FrskyX telemetry + struct t_fx_rx_frame FrskyxRxFrames[4] ; + uint8_t NextFxFrameToForward ; +#endif // SPORT_TELEMETRY -struct t_fx_rx_frame -{ - uint8_t valid ; - uint8_t count ; - uint8_t payload[6] ; -} ; - -// Store for FrskyX telemetry -struct t_fx_rx_frame FrskyxRxFrames[4] ; -uint8_t NextFxFrameToForward ; - -#endif #if defined HUB_TELEMETRY #define USER_MAX_BYTES 6 uint8_t prev_index; -#endif +#endif // HUB_TELEMETRY -#define START_STOP 0x7e -#define BYTESTUFF 0x7d -#define STUFF_MASK 0x20 +#define START_STOP 0x7e +#define BYTESTUFF 0x7d +#define STUFF_MASK 0x20 +#define MAX_PKTX 10 +uint8_t pktx[MAX_PKTX]; +uint8_t indx; uint8_t frame[18]; #if ( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) ) @@ -85,30 +78,30 @@ static void multi_send_header(uint8_t type, uint8_t len) static void multi_send_status() { - multi_send_header(MULTI_TELEMETRY_STATUS, 5); + multi_send_header(MULTI_TELEMETRY_STATUS, 5); - // Build flags - uint8_t flags=0; - if (IS_INPUT_SIGNAL_on) - flags |= 0x01; - if (mode_select==MODE_SERIAL) - flags |= 0x02; - if (remote_callback != 0) - { - flags |= 0x04; + // Build flags + uint8_t flags=0; + if (IS_INPUT_SIGNAL_on) + flags |= 0x01; + if (mode_select==MODE_SERIAL) + flags |= 0x02; + if (remote_callback != 0) + { + flags |= 0x04; if (IS_WAIT_BIND_on) flags |= 0x10; else if (!IS_BIND_DONE_on) flags |= 0x08; } - Serial_write(flags); + Serial_write(flags); - // Version number example: 1.1.6.1 - Serial_write(VERSION_MAJOR); - Serial_write(VERSION_MINOR); - Serial_write(VERSION_REVISION); - Serial_write(VERSION_PATCH_LEVEL); + // Version number example: 1.1.6.1 + Serial_write(VERSION_MAJOR); + Serial_write(VERSION_MINOR); + Serial_write(VERSION_REVISION); + Serial_write(VERSION_PATCH_LEVEL); } #endif @@ -156,9 +149,9 @@ static void multi_send_status() #ifdef MULTI_TELEMETRY static void multi_send_frskyhub() { - multi_send_header(MULTI_TELEMETRY_HUB, 9); - for (uint8_t i = 0; i < 9; i++) - Serial_write(frame[i]); + multi_send_header(MULTI_TELEMETRY_HUB, 9); + for (uint8_t i = 0; i < 9; i++) + Serial_write(frame[i]); } #endif @@ -169,8 +162,8 @@ void frskySendStuffed() { if ((frame[i] == START_STOP) || (frame[i] == BYTESTUFF)) { - Serial_write(BYTESTUFF); - frame[i] ^= STUFF_MASK; + Serial_write(BYTESTUFF); + frame[i] ^= STUFF_MASK; } Serial_write(frame[i]); } @@ -181,12 +174,8 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len) { uint8_t clen = pkt[0] + 3 ; if(pkt[1] == rx_tx_addr[3] && pkt[2] == rx_tx_addr[2] && len == clen ) - { + { telemetry_link|=1; // Telemetry data is available - /*previous version - RSSI_dBm = (((uint16_t)(pktt[len-2])*18)>>4); - if(pktt[len-2] >=128) RSSI_dBm -= 164; - else RSSI_dBm += 130;*/ TX_RSSI = pkt[len-2]; if(TX_RSSI >=128) TX_RSSI -= 128; @@ -204,12 +193,8 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len) { uint8_t topBit = 0 ; if ( telemetry_counter & 0x80 ) - { if ( ( telemetry_counter & 0x1F ) != RetrySequence ) - { topBit = 0x80 ; - } - } telemetry_counter = ( (telemetry_counter+1)%32 ) | topBit ; // Request next telemetry frame } else @@ -222,28 +207,14 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len) } } else - { pktt[6]=0; // Discard packet - } // #if defined SPORT_TELEMETRY && defined FRSKYX_CC2500_INO telemetry_lost=0; if (protocol==MODE_FRSKYX) { uint16_t lcrc = crc_x(&pkt[3], len-7 ) ; -// if ( ( sub_protocol & 2 ) == 0 ) -// { -// if ( ( (lcrc >> 8) == pkt[len-4]) && ( (lcrc & 0x00FF ) == pkt[len-3]) ) -// { -// lcrc = 0 ; -// } -// else -// { -// lcrc = 1 ; -// } -// } -// if ( lcrc == 0 ) - + if ( ( (lcrc >> 8) == pkt[len-4]) && ( (lcrc & 0x00FF ) == pkt[len-3]) ) { // Check if in sequence @@ -267,18 +238,12 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len) { p->count = count ; for ( uint8_t i = 0 ; i < count ; i += 1 ) - { p->payload[i] = pkt[i+7] ; - } } else - { p->count = 0 ; - } p->valid = 1 ; - - - + FrX_receive_seq = ( FrX_receive_seq + 1 ) & 0x03 ; if ( FrskyxRxTelemetryValidSequence & 0x80 ) @@ -287,24 +252,16 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len) FrskyxRxTelemetryValidSequence &= 0x7F ; } -// if ( FrskyxRxTelemetry.validSequence & 0x80 ) -// { -// FrX_receive_seq = ( FrskyxRxTelemetry.validSequence + 1 ) & 3 ; - -// FrskyxRxTelemetry.validSequence &= 0x7F ; - -// } } else { // Save and request correct packet -// struct t_fx_rx_packet *p ; struct t_fx_rx_frame *q ; uint8_t count ; - // pkt[4] RSSI - // pkt[5] sequence control - // pkt[6] payload count - // pkt[7-12] payload + // pkt[4] RSSI + // pkt[5] sequence control + // pkt[6] payload count + // pkt[7-12] payload pktt[6] = 0 ; // Don't process if ( (pkt[5] & 0x03) == ( ( FrX_receive_seq +1 ) & 3 ) ) { @@ -319,55 +276,18 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len) } } else - { q->count = 0 ; - } q->valid = 1 ; FrskyxRxTelemetryValidSequence = 0x80 | ( pkt[5] & 0x03 ) ; } - -// p = &FrskyxRxTelemetry ; -// count = pkt[6] ; -// if ( count <= 6 ) -// { -// p->count = count ; -// for ( uint8_t i = 0 ; i < count ; i += 1 ) -// { -// p->payload[i] = pkt[i+7] ; -// } -// p->validSequence = 0x80 | ( pkt[5] & 0x03 ) ; -// } - FrX_receive_seq = ( FrX_receive_seq & 0x03 ) | 0x04 ; // Request re-transmission + FrX_receive_seq = ( FrX_receive_seq & 0x03 ) | 0x04 ; // Request re-transmission } if (((pktt[5] >> 4) & 0x0f) == 0x08) - { FrX_send_seq = 0 ; -// FrX_receive_seq = 0x08 ; - } } - -// packet[21] = (FrX_receive_seq << 4) | FrX_send_seq ;//8 at start -// if ( FrX_send_seq != 0x08 ) -// { -// FrX_send_seq = ( FrX_send_seq + 1 ) & 0x03 ; -// } - -// if ((pktt[5] >> 4 & 0x0f) == 0x08) -// { -// seq_last_sent = 8; -// seq_last_rcvd = 0; -// pass=0; -// } -// else -// { -// if ((pktt[5] >> 4 & 0x03) == (seq_last_rcvd + 1) % 4) -// seq_last_rcvd = (seq_last_rcvd + 1) % 4; -// else -// pass=0;//reset if sequence wrong -// } } #endif } @@ -446,7 +366,7 @@ void frsky_user_frame() } else telemetry_link=0; -} +} /* HuB RX packets. pkt[6]|(counter++)|00 01 02 03 04 05 06 07 08 09 @@ -514,23 +434,19 @@ pkt[6]|(counter++)|00 01 02 03 04 05 06 07 08 09 */ -const uint8_t PROGMEM Indices[] = { 0x00, 0xA1, 0x22, 0x83, 0xE4, 0x45, - 0xC6, 0x67, 0x48, 0xE9, 0x6A, 0xCB, - 0xAC, 0x0D, 0x8E, 0x2F, 0xD0, 0x71, - 0xF2, 0x53, 0x34, 0x95, 0x16, 0xB7, - 0x98, 0x39, 0xBA, 0x1B } ; - - #ifdef MULTI_TELEMETRY + const uint8_t PROGMEM Indices[] = { 0x00, 0xA1, 0x22, 0x83, 0xE4, 0x45, + 0xC6, 0x67, 0x48, 0xE9, 0x6A, 0xCB, + 0xAC, 0x0D, 0x8E, 0x2F, 0xD0, 0x71, + 0xF2, 0x53, 0x34, 0x95, 0x16, 0xB7, + 0x98, 0x39, 0xBA, 0x1B } ; void sportSend(uint8_t *p) { multi_send_header(MULTI_TELEMETRY_SPORT, 9); uint16_t crc_s = 0; uint8_t x = p[0] ; if ( x <= 0x1B ) - { x = pgm_read_byte_near( &Indices[x] ) ; - } Serial_write(x) ; for (uint8_t i = 1; i < 9; i++) { @@ -540,8 +456,8 @@ const uint8_t PROGMEM Indices[] = { 0x00, 0xA1, 0x22, 0x83, 0xE4, 0x45, if (i>0) { - crc_s += p[i]; //0-1FF - crc_s += crc_s >> 8; //0-100 + crc_s += p[i]; //0-1FF + crc_s += crc_s >> 8; //0-100 crc_s &= 0x00ff; } } @@ -661,7 +577,7 @@ void proces_sport_data(uint8_t data) pass = 1; break; } - if(data == BYTESTUFF)//if they are stuffed + if(data == BYTESTUFF) //if they are stuffed pass=2; else if (indx < MAX_PKTX) @@ -680,9 +596,7 @@ void proces_sport_data(uint8_t data) uint8_t dest = sport * FRSKY_SPORT_PACKET_SIZE ; uint8_t i ; for ( i = 0 ; i < FRSKY_SPORT_PACKET_SIZE ; i += 1 ) - { pktx1[dest++] = pktx[i] ; // Triple buffer - } sport += 1 ;//ok to send } // else @@ -704,10 +618,9 @@ void TelemetryUpdate() h = SerialControl.head ; t = SerialControl.tail ; if ( h >= t ) - t += 192 - h ; + t += TXBUFFER_SIZE - h ; else t -= h ; -// if ( t < 32 ) if ( t < 64 ) { return ; @@ -741,9 +654,6 @@ void TelemetryUpdate() #if defined SPORT_TELEMETRY if (protocol==MODE_FRSKYX) { // FrSkyX -// struct t_fx_rx_frame *p ; -// uint8_t count ; - for(;;) { struct t_fx_rx_frame *p ; @@ -763,40 +673,12 @@ void TelemetryUpdate() } } -// p = &FrskyxRxFrames[NextFxFrameToForward] ; -// if ( p->valid ) -// { -// count = p->count ; -// for (uint8_t i=0; i < count ; i++) -// proces_sport_data(p->payload[i]) ; -// p->valid = 0 ; // Sent on -// NextFxFrameToForward = ( NextFxFrameToForward + 1 ) & 3 ; -// } - if(telemetry_link) { if(pktt[4] & 0x80) RX_RSSI=pktt[4] & 0x7F ; else RxBt = (pktt[4]<<1) + 1 ; - -// if(pktt[6] && pktt[6]<=6) -// { -// for (uint8_t i=0; i < pktt[6]; i++) -// proces_sport_data(pktt[7+i]); -// if ( FrskyxRxTelemetry.validSequence & 0x80 ) -// { -// // Process out of sequence packet -// for (uint8_t i=0; i < FrskyxRxTelemetry.count ; i++) -// { -// proces_sport_data( FrskyxRxTelemetry.payload[i] ) ; -// } -//// FrX_receive_seq = ( FrskyxRxTelemetry.validSequence + 1 ) & 3 ; -// FrskyxRxTelemetry.validSequence = 0 ; -// } -// } - - telemetry_link=0; } uint32_t now = micros(); @@ -810,7 +692,7 @@ void TelemetryUpdate() #endif } } - #endif + #endif // SPORT_TELEMETRY #if defined DSM_TELEMETRY if(telemetry_link && protocol == MODE_DSM) @@ -820,14 +702,14 @@ void TelemetryUpdate() return; } #endif - #if defined AFHDS2A_FW_TELEMETRY - if(telemetry_link == 2 && protocol == MODE_AFHDS2A) + #if defined AFHDS2A_FW_TELEMETRY + if(telemetry_link == 2 && protocol == MODE_AFHDS2A) { AFHDSA_short_frame(); telemetry_link=0; return; } - #endif + #endif if((telemetry_link & 1 )&& protocol != MODE_FRSKYX) { // FrSkyD + Hubsan + AFHDS2A + Bayang + Cabell @@ -850,6 +732,19 @@ void TelemetryUpdate() /**************************/ /**************************/ +#ifdef SERIAL_DEBUG + void StatusSerial_write(uint8_t data) + { + uint8_t nextHead ; + nextHead = tx_debug_head + 1 ; + if ( nextHead >= TXBUFFER_SIZE ) + nextHead = 0 ; + tx_debug_buff[nextHead]=data; + tx_debug_head = nextHead ; + tx_debug_resume(); + } +#endif // SERIAL_DEBUG + #ifndef BASH_SERIAL // Routines for normal serial output void Serial_write(uint8_t data) @@ -969,11 +864,34 @@ void TelemetryUpdate() #endif } #ifdef STM32_BOARD + #if defined(SERIAL_DEBUG) + void __irq_usart1() + { // Transmit interrupt + if(USART1_BASE->SR & USART_SR_TXE) + { + if(tx_debug_head!=tx_debug_tail) + { + if(++tx_debug_tail>=TXBUFFER_SIZE) //head + tx_debug_tail=0; + USART1_BASE->DR=tx_debug_buff[tx_debug_tail]; //clears TXE bit + } + if (tx_debug_tail == tx_debug_head) + tx_debug_pause(); // Check if all data is transmitted . if yes disable transmitter UDRE interrupt + } + } + void usart1_begin(uint32_t baud,uint32_t config ) + { + usart_init(USART1); + usart_config_gpios_async(USART1,GPIOA,PIN_MAP[PA10].gpio_bit,GPIOA,PIN_MAP[PA9].gpio_bit,config); + usart_set_baud_rate(USART1, STM32_PCLK1, baud); + usart_enable(USART1); + } + #endif void usart2_begin(uint32_t baud,uint32_t config ) { usart_init(USART2); usart_config_gpios_async(USART2,GPIOA,PIN_MAP[PA3].gpio_bit,GPIOA,PIN_MAP[PA2].gpio_bit,config); - usart_set_baud_rate(USART2, STM32_PCLK1, baud);// + usart_set_baud_rate(USART2, STM32_PCLK1, baud); usart_enable(USART2); } void usart3_begin(uint32_t baud,uint32_t config ) @@ -1120,9 +1038,7 @@ ISR(TIMER0_COMPA_vect) GPIOR1 = 3 ; } else - { OCR0A += 20 ; - } } ISR(TIMER0_COMPB_vect) @@ -1150,12 +1066,9 @@ ISR(TIMER0_COMPB_vect) { GPIOR0 = ptr->data[ptr->tail] ; GPIOR2 = ptr->data[ptr->tail+1] ; - uint8_t nextTail ; - nextTail = ptr->tail + 2 ; - if ( nextTail > 192 ) - { + uint8_t nextTail = ptr->tail + 2 ; + if ( nextTail > TXBUFFER_SIZE ) nextTail = 0 ; - } ptr->tail = nextTail ; GPIOR1 = 8 ; OCR0A = OCR0B + 40 ; @@ -1170,44 +1083,36 @@ ISR(TIMER0_COMPB_vect) } } else - { OCR0B += 20 ; - } } ISR(TIMER0_OVF_vect) { uint8_t byte ; if ( GPIOR1 > 2 ) - { byte = GPIOR0 ; - } else - { byte = GPIOR2 ; - } if ( byte & 0x01 ) SERIAL_TX_on; else SERIAL_TX_off; byte /= 2 ; // Generates shorter code than byte >>= 1 if ( GPIOR1 > 2 ) - { GPIOR0 = byte ; - } else - { GPIOR2 = byte ; - } if ( --GPIOR1 == 0 ) - { - // prepare next byte + { // prepare next byte volatile struct t_serial_bash *ptr = &SerialControl ; if ( ptr->head != ptr->tail ) { GPIOR0 = ptr->data[ptr->tail] ; GPIOR2 = ptr->data[ptr->tail+1] ; - ptr->tail = ( ptr->tail + 2 ) & 0x7F ; + uint8_t nextTail = ptr->tail + 2 ; + if ( nextTail > TXBUFFER_SIZE ) + nextTail = 0 ; + ptr->tail = nextTail ; GPIOR1 = 10 ; } else diff --git a/Multiprotocol/WK2x01_cyrf6936.ino b/Multiprotocol/WK2x01_cyrf6936.ino index a5cd83b..59dc71b 100644 --- a/Multiprotocol/WK2x01_cyrf6936.ino +++ b/Multiprotocol/WK2x01_cyrf6936.ino @@ -294,7 +294,7 @@ static void __attribute__((unused)) WK_build_beacon_pkt_2801() BIND_SET_PULLUP; // set pullup if(IS_BIND_BUTTON_on) { - eeprom_write_byte((EE_ADDR)(30+mode_select),0x01); // Set fixed id mode for the current model + eeprom_write_byte((EE_ADDR)(MODELMODE_EEPROM_OFFSET+mode_select),0x01); // Set fixed id mode for the current model option=1; } BIND_SET_OUTPUT;