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;