mirror of
https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git
synced 2025-07-01 01:57:52 +00:00
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.
This commit is contained in:
Pascal Langer
parent
24fd5ba361
commit
7d41017850
@ -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;
|
||||
|
||||
@ -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
|
||||
//******************
|
||||
@ -230,8 +230,23 @@ 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")
|
||||
@ -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
|
||||
|
||||
*/
|
||||
|
||||
@ -21,20 +21,23 @@
|
||||
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#include <avr/pgmspace.h>
|
||||
|
||||
//#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);
|
||||
#if defined TELEMETRY
|
||||
pinMode(TX_INV_pin,OUTPUT);
|
||||
pinMode(RX_INV_pin,OUTPUT);
|
||||
#if defined TELEMETRY
|
||||
#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;
|
||||
@ -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;
|
||||
@ -361,6 +381,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
|
||||
if(mode_select != MODE_SERIAL)
|
||||
@ -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
|
||||
|
||||
@ -226,6 +226,7 @@
|
||||
#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
|
||||
@ -264,6 +265,9 @@
|
||||
#define CYRF_CSN_on digitalWrite(CYRF_CSN_pin,HIGH)
|
||||
#define CYRF_CSN_off digitalWrite(CYRF_CSN_pin,LOW)
|
||||
|
||||
#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) //
|
||||
|
||||
|
||||
@ -21,53 +21,46 @@ uint8_t RetrySequence ;
|
||||
|
||||
#if ( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) )
|
||||
#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 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 ;
|
||||
|
||||
uint8_t FrskyxRxTelemetryValidSequence ;
|
||||
|
||||
struct t_fx_rx_frame
|
||||
{
|
||||
// Store for out of sequence packet
|
||||
uint8_t FrskyxRxTelemetryValidSequence ;
|
||||
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 ;
|
||||
// Store for FrskyX telemetry
|
||||
struct t_fx_rx_frame FrskyxRxFrames[4] ;
|
||||
uint8_t NextFxFrameToForward ;
|
||||
#endif // SPORT_TELEMETRY
|
||||
|
||||
#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 MAX_PKTX 10
|
||||
uint8_t pktx[MAX_PKTX];
|
||||
uint8_t indx;
|
||||
uint8_t frame[18];
|
||||
|
||||
#if ( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) )
|
||||
@ -183,10 +176,6 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len)
|
||||
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,27 +207,13 @@ 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]) )
|
||||
{
|
||||
@ -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,18 +252,10 @@ 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
|
||||
@ -319,56 +276,19 @@ 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
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
}
|
||||
@ -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,
|
||||
#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 } ;
|
||||
|
||||
|
||||
#ifdef MULTI_TELEMETRY
|
||||
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++)
|
||||
{
|
||||
@ -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)
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user