SBUS and CPPM output

This commit is contained in:
Pascal Langer 2021-03-03 10:07:19 +01:00
parent d7c24d62ed
commit bb70116df0
10 changed files with 313 additions and 18 deletions

View File

@ -189,6 +189,10 @@ uint16_t AFHDS2A_RX_callback()
RX_RSSI = map16b(rssi, 160, 8, 0, 128); RX_RSSI = map16b(rssi, 160, 8, 0, 128);
AFHDS2A_RX_build_telemetry_packet(); AFHDS2A_RX_build_telemetry_packet();
telemetry_link = 1; telemetry_link = 1;
#if defined (SEND_SBUS_SERIAL) || defined (SEND_CPPM)
if(sub_protocol>0)
telemetry_link = 0x80 + sub_protocol; // Disable telemetry output, type SBUS=1, type CPPM=2
#endif
} }
rx_data_started = true; rx_data_started = true;
read_retry = 10; // hop to next channel read_retry = 10; // hop to next channel

View File

@ -151,6 +151,10 @@ uint16_t BAYANG_RX_callback()
if (telemetry_link == 0) { if (telemetry_link == 0) {
Bayang_Rx_build_telemetry_packet(); Bayang_Rx_build_telemetry_packet();
telemetry_link = 1; telemetry_link = 1;
#if defined (SEND_SBUS_SERIAL) || defined (SEND_CPPM)
if(sub_protocol>0)
telemetry_link = 0x80 + sub_protocol; // Disable telemetry output, type SBUS=1, type CPPM=2
#endif
} }
rx_data_started = true; rx_data_started = true;
rx_data_received = true; rx_data_received = true;

View File

@ -155,6 +155,10 @@ static void __attribute__((unused)) DSM_RX_build_telemetry_packet()
packet_in[idx++] = bits & 0xff; packet_in[idx++] = bits & 0xff;
// Send telemetry // Send telemetry
telemetry_link = 1; telemetry_link = 1;
#if defined (SEND_SBUS_SERIAL) || defined (SEND_CPPM)
if(sub_protocol>0)
telemetry_link = 0x80 + sub_protocol; // Disable telemetry output, type SBUS=1, type CPPM=2
#endif
} }
static bool __attribute__((unused)) DSM_RX_bind_check_validity() static bool __attribute__((unused)) DSM_RX_bind_check_validity()

View File

@ -528,7 +528,7 @@ uint16_t FRSKY_RX_callback()
eeprom_write_byte((EE_ADDR)temp++, rx_tx_addr[3]); eeprom_write_byte((EE_ADDR)temp++, rx_tx_addr[3]);
eeprom_write_byte((EE_ADDR)temp++, rx_tx_addr[2]); eeprom_write_byte((EE_ADDR)temp++, rx_tx_addr[2]);
eeprom_write_byte((EE_ADDR)temp++, rx_tx_addr[1]); eeprom_write_byte((EE_ADDR)temp++, rx_tx_addr[1]);
if(sub_protocol==FRSKY_RX) if(sub_protocol == FRSKY_RX || sub_protocol >= FRSKY_SBUS) // FRSKY_RX, FRSKY_SBUS, FRSKY_CPPM
eeprom_write_byte((EE_ADDR)temp++, FRSKY_RX_finetune); eeprom_write_byte((EE_ADDR)temp++, FRSKY_RX_finetune);
if(FRSKY_RX_format != FRSKY_RX_D16v2FCC && FRSKY_RX_format != FRSKY_RX_D16v2LBT) if(FRSKY_RX_format != FRSKY_RX_D16v2FCC && FRSKY_RX_format != FRSKY_RX_D16v2LBT)
for (ch = 0; ch < 47; ch++) for (ch = 0; ch < 47; ch++)
@ -579,6 +579,10 @@ uint16_t FRSKY_RX_callback()
{ // send channels to TX { // send channels to TX
FRSKY_RX_build_telemetry_packet(); FRSKY_RX_build_telemetry_packet();
telemetry_link = 1; telemetry_link = 1;
#if defined (SEND_SBUS_SERIAL) || defined (SEND_CPPM)
if(sub_protocol >= FRSKY_SBUS)
telemetry_link = 0x81 + sub_protocol - FRSKY_SBUS; // Disable telemetry output, type SBUS=1, type CPPM=2
#endif
} }
pps_counter++; pps_counter++;
} }

View File

@ -52,11 +52,11 @@
52,ZSX,280 52,ZSX,280
53,Height,5ch,8ch 53,Height,5ch,8ch
54,Scanner 54,Scanner
55,Frsky_RX,RX,CloneTX,EraseTX 55,Frsky_RX,Multi,CloneTX,EraseTX,SBUS,CPPM
56,AFHDS2A_RX 56,AFHDS2A_RX,Multi,SBUS,CPPM
57,HoTT,Sync,No_Sync 57,HoTT,Sync,No_Sync
58,FX816,P38 58,FX816,P38
59,Bayang_RX 59,Bayang_RX,Multi,SBUS,CPPM
60,Pelikan,Pro,Lite 60,Pelikan,Pro,Lite
61,Tiger 61,Tiger
62,XK,X450,X420 62,XK,X450,X420
@ -67,7 +67,7 @@
67,LR12,LR12,LR12_6ch 67,LR12,LR12,LR12_6ch
68,Skyartec 68,Skyartec
69,ESKYv2,150V2 69,ESKYv2,150V2
70,DSM_RX 70,DSM_RX,Multi,SBUS,CPPM
71,JJRC345,JJRC345,SkyTmblr 71,JJRC345,JJRC345,SkyTmblr
72,Q90C 72,Q90C
73,Kyosho,FHSS,Hype 73,Kyosho,FHSS,Hype

View File

@ -142,7 +142,6 @@ const char STR_SUBTYPE_XK[] = "\x04""X450""X420";
const char STR_SUBTYPE_FRSKYR9[] = "\x07""915MHz\0""868MHz\0""915 8ch""868 8ch""FCC\0 ""--\0 ""FCC 8ch""-- 8ch\0"; const char STR_SUBTYPE_FRSKYR9[] = "\x07""915MHz\0""868MHz\0""915 8ch""868 8ch""FCC\0 ""--\0 ""FCC 8ch""-- 8ch\0";
const char STR_SUBTYPE_ESKY[] = "\x03""Std""ET4"; const char STR_SUBTYPE_ESKY[] = "\x03""Std""ET4";
const char STR_SUBTYPE_PROPEL[] = "\x04""74-Z"; const char STR_SUBTYPE_PROPEL[] = "\x04""74-Z";
const char STR_SUBTYPE_FRSKY_RX[] = "\x07""RX\0 ""CloneTX""EraseTX";
const char STR_SUBTYPE_FRSKYL[] = "\x08""LR12\0 ""LR12 6ch"; const char STR_SUBTYPE_FRSKYL[] = "\x08""LR12\0 ""LR12 6ch";
const char STR_SUBTYPE_WFLY[] = "\x05""WFR0x"; const char STR_SUBTYPE_WFLY[] = "\x05""WFR0x";
const char STR_SUBTYPE_WFLY2[] = "\x05""RF20x"; const char STR_SUBTYPE_WFLY2[] = "\x05""RF20x";
@ -155,6 +154,20 @@ const char STR_SUBTYPE_KYOSHO[] = "\x04""FHSS""Hype";
const char STR_SUBTYPE_FUTABA[] = "\x05""SFHSS"; const char STR_SUBTYPE_FUTABA[] = "\x05""SFHSS";
const char STR_SUBTYPE_JJRC345[] = "\x08""JJRC345\0""SkyTmblr"; const char STR_SUBTYPE_JJRC345[] = "\x08""JJRC345\0""SkyTmblr";
#define NO_SUBTYPE nullptr
#if defined (SEND_SBUS_SERIAL) || defined (SEND_CPPM)
const char STR_SUBTYPE_FRSKY_RX[] = "\x07""Multi\0 ""CloneTX""EraseTX""SBUS\0 ""CPPM\0 ";
#define FRSBUSCPPM 5
const char STR_SBUS_CPPM[] = "\x05""Multi""SBUS\0""CPPM\0";
#define SBUS_CPPM 3
#else
const char STR_SUBTYPE_FRSKY_RX[] = "\x07""Multi\0 ""CloneTX""EraseTX";
#define FRSBUSCPPM 3
#define STR_SBUS_CPPM NO_SUBTYPE
#define SBUS_CPPM 0
#endif
enum enum
{ {
OPTION_NONE, OPTION_NONE,
@ -170,8 +183,6 @@ enum
OPTION_WBUS, OPTION_WBUS,
}; };
#define NO_SUBTYPE nullptr
const mm_protocol_definition multi_protocols[] = { const mm_protocol_definition multi_protocols[] = {
// Protocol number, Protocol String, Sub_protocol strings, Number of sub_protocols, Option type, Failsafe, ChMap, RF switch, Init, Callback // Protocol number, Protocol String, Sub_protocol strings, Number of sub_protocols, Option type, Failsafe, ChMap, RF switch, Init, Callback
#if defined(MULTI_CONFIG_INO) #if defined(MULTI_CONFIG_INO)
@ -184,7 +195,7 @@ const mm_protocol_definition multi_protocols[] = {
{PROTO_BAYANG, STR_BAYANG, STR_SUBTYPE_BAYANG, 6, OPTION_TELEM, 0, 0, SW_NRF, BAYANG_init, BAYANG_callback }, {PROTO_BAYANG, STR_BAYANG, STR_SUBTYPE_BAYANG, 6, OPTION_TELEM, 0, 0, SW_NRF, BAYANG_init, BAYANG_callback },
#endif #endif
#if defined(BAYANG_RX_NRF24L01_INO) #if defined(BAYANG_RX_NRF24L01_INO)
{PROTO_BAYANG_RX, STR_BAYANG_RX, NO_SUBTYPE, 0, OPTION_NONE, 0, 0, SW_NRF, BAYANG_RX_init, BAYANG_RX_callback }, {PROTO_BAYANG_RX, STR_BAYANG_RX, STR_SBUS_CPPM, SBUS_CPPM, OPTION_NONE, 0, 0, SW_NRF, BAYANG_RX_init, BAYANG_RX_callback },
#endif #endif
#if defined(BUGS_A7105_INO) #if defined(BUGS_A7105_INO)
{PROTO_BUGS, STR_BUGS, NO_SUBTYPE, 0, OPTION_NONE, 0, 0, SW_A7105, BUGS_init, BUGS_callback }, {PROTO_BUGS, STR_BUGS, NO_SUBTYPE, 0, OPTION_NONE, 0, 0, SW_A7105, BUGS_init, BUGS_callback },
@ -217,7 +228,7 @@ const mm_protocol_definition multi_protocols[] = {
{PROTO_DSM, STR_DSM, STR_SUBTYPE_DSM, 5, OPTION_MAXTHR, 0, 1, SW_CYRF, DSM_init, DSM_callback }, {PROTO_DSM, STR_DSM, STR_SUBTYPE_DSM, 5, OPTION_MAXTHR, 0, 1, SW_CYRF, DSM_init, DSM_callback },
#endif #endif
#if defined(DSM_RX_CYRF6936_INO) #if defined(DSM_RX_CYRF6936_INO)
{PROTO_DSM_RX, STR_DSM_RX, NO_SUBTYPE, 0, OPTION_NONE, 0, 1, SW_CYRF, DSM_RX_init, DSM_RX_callback }, {PROTO_DSM_RX, STR_DSM_RX, STR_SBUS_CPPM, SBUS_CPPM, OPTION_NONE, 0, 1, SW_CYRF, DSM_RX_init, DSM_RX_callback },
#endif #endif
#if defined(E010R5_CYRF6936_INO) #if defined(E010R5_CYRF6936_INO)
{PROTO_E010R5, STR_E010R5, NO_SUBTYPE, 0, OPTION_NONE, 0, 0, SW_CYRF, E010R5_init, E010R5_callback }, {PROTO_E010R5, STR_E010R5, NO_SUBTYPE, 0, OPTION_NONE, 0, 0, SW_CYRF, E010R5_init, E010R5_callback },
@ -247,14 +258,14 @@ const mm_protocol_definition multi_protocols[] = {
{PROTO_AFHDS2A, STR_AFHDS2A, STR_SUBTYPE_AFHDS2A, 8, OPTION_SRVFREQ, 1, 1, SW_A7105, AFHDS2A_init, AFHDS2A_callback }, {PROTO_AFHDS2A, STR_AFHDS2A, STR_SUBTYPE_AFHDS2A, 8, OPTION_SRVFREQ, 1, 1, SW_A7105, AFHDS2A_init, AFHDS2A_callback },
#endif #endif
#if defined(AFHDS2A_RX_A7105_INO) #if defined(AFHDS2A_RX_A7105_INO)
{PROTO_AFHDS2A_RX, STR_AFHDS2A_RX,NO_SUBTYPE, 0, OPTION_NONE, 0, 0, SW_A7105, AFHDS2A_RX_init, AFHDS2A_RX_callback }, {PROTO_AFHDS2A_RX, STR_AFHDS2A_RX,STR_SBUS_CPPM, SBUS_CPPM, OPTION_NONE, 0, 0, SW_A7105, AFHDS2A_RX_init, AFHDS2A_RX_callback },
#endif #endif
#if defined(FQ777_NRF24L01_INO) #if defined(FQ777_NRF24L01_INO)
{PROTO_FQ777, STR_FQ777, NO_SUBTYPE, 0, OPTION_NONE, 0, 0, SW_NRF, FQ777_init, FQ777_callback }, {PROTO_FQ777, STR_FQ777, NO_SUBTYPE, 0, OPTION_NONE, 0, 0, SW_NRF, FQ777_init, FQ777_callback },
#endif #endif
//OpenTX 2.3.x issue: DO NOT CHANGE ORDER below //OpenTX 2.3.x issue: DO NOT CHANGE ORDER below
#if defined(FRSKY_RX_CC2500_INO) #if defined(FRSKY_RX_CC2500_INO)
{PROTO_FRSKY_RX, STR_FRSKY_RX, STR_SUBTYPE_FRSKY_RX, 3, OPTION_RFTUNE, 0, 0, SW_CC2500, FRSKY_RX_init, FRSKY_RX_callback }, {PROTO_FRSKY_RX, STR_FRSKY_RX, STR_SUBTYPE_FRSKY_RX, FRSBUSCPPM, OPTION_RFTUNE, 0, 0, SW_CC2500, FRSKY_RX_init, FRSKY_RX_callback },
#endif #endif
#if defined(FRSKYD_CC2500_INO) #if defined(FRSKYD_CC2500_INO)
{PROTO_FRSKYD, STR_FRSKYD, STR_SUBTYPE_FRSKYD, 2, OPTION_RFTUNE, 0, 0, SW_CC2500, FRSKYD_init, FRSKYD_callback }, {PROTO_FRSKYD, STR_FRSKYD, STR_SUBTYPE_FRSKYD, 2, OPTION_RFTUNE, 0, 0, SW_CC2500, FRSKYD_init, FRSKYD_callback },

View File

@ -19,7 +19,7 @@
#define VERSION_MAJOR 1 #define VERSION_MAJOR 1
#define VERSION_MINOR 3 #define VERSION_MINOR 3
#define VERSION_REVISION 2 #define VERSION_REVISION 2
#define VERSION_PATCH_LEVEL 55 #define VERSION_PATCH_LEVEL 56
#define MODE_SERIAL 0 #define MODE_SERIAL 0
@ -378,6 +378,8 @@ enum FRSKY_RX
FRSKY_RX = 0, FRSKY_RX = 0,
FRSKY_CLONE = 1, FRSKY_CLONE = 1,
FRSKY_ERASE = 2, FRSKY_ERASE = 2,
FRSKY_SBUS = 3,
FRSKY_CPPM = 4,
}; };
enum FRSKYL enum FRSKYL

View File

@ -66,6 +66,9 @@
void __irq_usart2(void); void __irq_usart2(void);
void __irq_usart3(void); void __irq_usart3(void);
} }
#ifdef SEND_CPPM
HardwareTimer HWTimer1(1) ;
#endif
#endif #endif
//Global constants/variables //Global constants/variables
@ -830,6 +833,25 @@ bool Update_All()
} }
#endif //ENABLE_PPM #endif //ENABLE_PPM
update_led_status(); update_led_status();
#if defined(SEND_SBUS_SERIAL) || defined(SEND_CPPM)
if ( telemetry_link & 0x80 )
{ // Protocol requests telemetry to be disabled
if( protocol == PROTO_FRSKY_RX || protocol == PROTO_AFHDS2A_RX || protocol == PROTO_BAYANG_RX || protocol == PROTO_DSM_RX )
{ // RX protocol
#ifdef SEND_SBUS_SERIAL
if(telemetry_link & 1)
Send_SBUS_USART1();
#endif
#ifdef SEND_CPPM
if(telemetry_link & 2)
Send_CCPM_USART1();
#endif
telemetry_link = 0x80; // update done
}
}
else
#endif
#if defined(TELEMETRY) #if defined(TELEMETRY)
#ifndef MULTI_TELEMETRY #ifndef MULTI_TELEMETRY
if((protocol == PROTO_BAYANG_RX) || (protocol == PROTO_AFHDS2A_RX) || (protocol == PROTO_FRSKY_RX) || (protocol == PROTO_SCANNER) || (protocol==PROTO_FRSKYD) || (protocol==PROTO_BAYANG) || (protocol==PROTO_NCC1701) || (protocol==PROTO_BUGS) || (protocol==PROTO_BUGSMINI) || (protocol==PROTO_HUBSAN) || (protocol==PROTO_AFHDS2A) || (protocol==PROTO_FRSKYX) || (protocol==PROTO_FRSKYX2) || (protocol==PROTO_DSM) || (protocol==PROTO_CABELL) || (protocol==PROTO_HITEC) || (protocol==PROTO_HOTT) || (protocol==PROTO_PROPEL) || (protocol==PROTO_OMP) || (protocol==PROTO_DEVO) || (protocol==PROTO_DSM_RX) || (protocol==PROTO_FRSKY_R9) || (protocol==PROTO_RLINK) || (protocol==PROTO_WFLY2) || (protocol==PROTO_LOLI) || (protocol==PROTO_MLINK)) if((protocol == PROTO_BAYANG_RX) || (protocol == PROTO_AFHDS2A_RX) || (protocol == PROTO_FRSKY_RX) || (protocol == PROTO_SCANNER) || (protocol==PROTO_FRSKYD) || (protocol==PROTO_BAYANG) || (protocol==PROTO_NCC1701) || (protocol==PROTO_BUGS) || (protocol==PROTO_BUGSMINI) || (protocol==PROTO_HUBSAN) || (protocol==PROTO_AFHDS2A) || (protocol==PROTO_FRSKYX) || (protocol==PROTO_FRSKYX2) || (protocol==PROTO_DSM) || (protocol==PROTO_CABELL) || (protocol==PROTO_HITEC) || (protocol==PROTO_HOTT) || (protocol==PROTO_PROPEL) || (protocol==PROTO_OMP) || (protocol==PROTO_DEVO) || (protocol==PROTO_DSM_RX) || (protocol==PROTO_FRSKY_R9) || (protocol==PROTO_RLINK) || (protocol==PROTO_WFLY2) || (protocol==PROTO_LOLI) || (protocol==PROTO_MLINK))
@ -840,8 +862,8 @@ bool Update_All()
#ifdef ENABLE_BIND_CH #ifdef ENABLE_BIND_CH
if(IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_off && Channel_data[BIND_CH-1]>CHANNEL_MAX_COMMAND) if(IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_off && Channel_data[BIND_CH-1]>CHANNEL_MAX_COMMAND)
{ // Autobind is on and BIND_CH went up { // Autobind is on and BIND_CH went up
CHANGE_PROTOCOL_FLAG_on; //reload protocol CHANGE_PROTOCOL_FLAG_on; // reload protocol
BIND_IN_PROGRESS; //enable bind BIND_IN_PROGRESS; // enable bind
BIND_CH_PREV_on; BIND_CH_PREV_on;
} }
if(IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_on && Channel_data[BIND_CH-1]<CHANNEL_MIN_COMMAND) if(IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_on && Channel_data[BIND_CH-1]<CHANNEL_MIN_COMMAND)
@ -852,7 +874,7 @@ bool Update_All()
#endif //ENABLE_BIND_CH #endif //ENABLE_BIND_CH
if(IS_CHANGE_PROTOCOL_FLAG_on) if(IS_CHANGE_PROTOCOL_FLAG_on)
{ // Protocol needs to be changed or relaunched for bind { // Protocol needs to be changed or relaunched for bind
protocol_init(); //init new protocol protocol_init(); // init new protocol
return true; return true;
} }
return false; return false;
@ -2084,6 +2106,229 @@ static void __attribute__((unused)) crc8_update(uint8_t byte)
} }
#endif //ENABLE_SERIAL #endif //ENABLE_SERIAL
/**************************/
/**************************/
/** SBUS/CPPM routines **/
/**************************/
/**************************/
#if defined (SEND_SBUS_SERIAL) || defined (SEND_CPPM)
uint32_t TrainerTimer ;
#endif
#ifdef SEND_SBUS_SERIAL
uint8_t SbusFrame[26] ;
uint16_t SbusChannels[16] ;
bool SbusInitialised = false;
void Init_SBUS_USART1()
{
for ( uint8_t i = 0 ; i < NUM_CHN ; i += 1 )
SbusChannels[i] = 0x03E0 ; // Centre position
usart_init(USART1);
usart_config_gpios_async(USART1,GPIOA,PIN_MAP[PA10].gpio_bit,GPIOA,PIN_MAP[PA9].gpio_bit,SERIAL_8E2);
usart_set_baud_rate(USART1, STM32_PCLK2, 100000);
USART1_BASE->CR1 |= USART_CR1_PCE_BIT;
usart_enable(USART1);
SbusInitialised = true;
}
void Send_SBUS_USART1()
{
if(SbusInitialised == false)
Init_SBUS_USART1();
TrainerTimer = millis() ;
len = packet_in[3] ;
// pad unused channels to 0x03E0 ;
// Scale used channels to centre of 0x03E0
uint32_t bitsavailable = 0 ;
uint32_t bits = 0 ; ;
uint32_t i ;
uint32_t value ;
uint8_t *packet ;
packet = &packet_in[4] ;
i = packet_in[2] ; // Start channel
// Load changed channels
while ( len )
{
while ( bitsavailable < 11 )
{
bits |= *packet++ << bitsavailable ;
bitsavailable += 8 ;
}
value = bits & 0x07FF ;
value -= (0x0400 - 0x03E0) ;
SbusChannels[i++] = value ;
bitsavailable -= 11 ;
bits >>= 11 ;
len-- ;
}
bitsavailable = 0 ;
bits = 0 ;
packet = SbusFrame ;
*packet++ = 0x0F ;
for ( i = 0 ; i < 16 ; i += 1 )
{
bits |= SbusChannels[i] << bitsavailable ;
bitsavailable += 11 ;
while ( bitsavailable >= 8 )
{
*packet++ = bits ;
bits >>= 8 ;
bitsavailable -= 8 ;
}
}
*packet++ = 0 ;
*packet = 0 ;
usart_tx( USART1, SbusFrame, 25 ) ;
}
#endif
#ifdef SEND_CPPM
#define PPM_CENTER 1500*2
uint16_t *TrainerPulsePtr ;
uint16_t TrainerPpmStream[10] ;
int16_t CppmChannels[8] ;
bool CppmInitialised = false;
void setupTrainerPulses()
{
uint32_t i ;
uint32_t total ;
uint32_t pulse ;
uint16_t *ptr ;
uint32_t p = 8 ;
int16_t PPM_range = 512*2 ; //range of 0.7..1.7msec
ptr = TrainerPpmStream ;
total = 22500u*2; //Minimum Framelen=22.5 ms
if ( (millis() - TrainerTimer) < 400 )
{
for ( i = 0 ; i < p ; i += 1 )
{
pulse = max( (int)min(CppmChannels[i],PPM_range),-PPM_range) + PPM_CENTER ;
total -= pulse ;
*ptr++ = pulse ;
}
}
*ptr++ = total ;
*ptr = 0 ;
TIMER1_BASE->CCR1 = total - 1500 ; // Update time
TIMER1_BASE->CCR2 = 300*2 ;
}
void init_trainer_ppm()
{
// Timer 1, channel 2 on PA9
RCC_BASE->APB2ENR |= RCC_APB2ENR_TIM1EN ; // Enable clock
setupTrainerPulses() ;
RCC_BASE->APB2ENR |= RCC_APB2ENR_IOPAEN ; // Enable portA clock
RCC_BASE->APB2ENR &= ~RCC_APB2ENR_USART1EN ; // Disable USART1
GPIOA_BASE->CRH &= ~0x00F0 ;
GPIOA_BASE->CRH |= 0x00A0 ; // AF PP OP2MHz
HWTimer1.pause() ; // Pause the timer1 while we're configuring it
TIMER1_BASE->ARR = *TrainerPulsePtr++ ;
TIMER1_BASE->PSC = 72000000 / 2000000 - 1 ; // 0.5uS
TIMER1_BASE->CCR2 = 600 ; // 300 uS pulse
TIMER1_BASE->CCR1 = 5000 ; // 2500 uS pulse
TIMER1_BASE->CCMR1 = 0x6000 ; // PWM mode 1 (header file has incorrect bits)
TIMER1_BASE->EGR = 1 ;
TIMER1_BASE->CCER = TIMER_CCER_CC2E ;
// TIMER1_BASE->DIER |= TIMER_DIER_CC2IE ;
TIMER1_BASE->DIER |= TIMER_DIER_UIE ;
TIMER1_BASE->CR1 = TIMER_CR1_CEN ;
nvic_irq_set_priority(NVIC_TIMER1_CC, 4 ) ;
nvic_irq_set_priority(NVIC_TIMER1_UP, 4 ) ;
HWTimer1.attachInterrupt(TIMER_UPDATE_INTERRUPT,tim1_up); // Assign function to Timer1/Comp2 interrupt
HWTimer1.attachInterrupt(TIMER_CH1,tim1_cc); // Assign function to Timer1/Comp2 interrupt
// HWTimer1.attachInterrupt(TIMER_CH2,tim1_cc); // Assign function to Timer1/Comp2 interrupt
CppmInitialised = true ;
HWTimer1.resume() ;
}
void tim1_up()
{
#define TIMER1_SR_MASK 0x1FFF
// PPM out update interrupt
if ( (TIMER1_BASE->DIER & TIMER_DIER_UIE) && ( TIMER1_BASE->SR & TIMER_SR_UIF ) )
{
GPIOA_BASE->BRR = 0x0200 ;
TIMER1_BASE->SR = TIMER1_SR_MASK & ~TIMER_SR_UIF ; // Clear flag
TIMER1_BASE->ARR = *TrainerPulsePtr++ ;
if ( *TrainerPulsePtr == 0 )
{
TIMER1_BASE->SR = 0x1FFF & ~TIMER_SR_CC1IF ; // Clear this flag
TIMER1_BASE->DIER |= TIMER_DIER_CC1IE ; // Enable this interrupt
TIMER1_BASE->DIER &= ~TIMER_DIER_UIE ; // Stop this interrupt
}
}
}
void tim1_cc()
{
// if ( ( TIMER1_BASE->DIER & TIMER_DIER_CC2IE ) && ( TIMER1_BASE->SR & TIMER_SR_CC2IF ) )
// {
// GPIOA_BASE->BSRR = 0x0200 ;
// TIMER1_BASE->SR = 0x1FFF & ~TIMER_SR_CC2IF ; // Clear flag
// }
if ( ( TIMER1_BASE->DIER & TIMER_DIER_CC1IE ) && ( TIMER1_BASE->SR & TIMER_SR_CC1IF ) )
{
// compare interrupt
TIMER1_BASE->DIER &= ~TIMER_DIER_CC1IE ; // Stop this interrupt
TIMER1_BASE->SR = 0x1FFF & ~TIMER_SR_CC1IF ; // Clear flag
setupTrainerPulses() ;
TrainerPulsePtr = TrainerPpmStream ;
TIMER1_BASE->SR = 0x1FFF & ~TIMER_SR_UIF ; // Clear this flag
TIMER1_BASE->DIER |= TIMER_DIER_UIE ; // Enable this interrupt
}
}
void Send_CCPM_USART1()
{
if ( CppmInitialised == false )
init_trainer_ppm() ;
TrainerTimer = millis() ;
len = packet_in[3] ;
uint32_t bitsavailable = 0 ;
uint32_t bits = 0 ; ;
uint32_t i ;
int16_t value ;
uint8_t *packet ;
packet = &packet_in[4] ;
i = packet_in[2] ; // Start channel
// Load changed channels
while ( len )
{
while ( bitsavailable < 11 )
{
bits |= *packet++ << bitsavailable ;
bitsavailable += 8 ;
}
value = bits & 0x07FF ;
value -= 0x0400 ;
bitsavailable -= 11 ;
bits >>= 11 ;
if ( i < 8 )
CppmChannels[i] = value * 5 / 4 ;
i++ ;
len-- ;
}
}
#endif
/**************************/
/**************************/
/** Arduino random **/
/**************************/
/**************************/
#if not defined (ORANGE_TX) && not defined (STM32_BOARD) #if not defined (ORANGE_TX) && not defined (STM32_BOARD)
static void random_init(void) static void random_init(void)
{ {

View File

@ -528,8 +528,16 @@
#error MAX_PPM_CHANNELS must be below or equal to 16. The default for this value is 16. #error MAX_PPM_CHANNELS must be below or equal to 16. The default for this value is 16.
#endif #endif
#if defined (STM32_BOARD) && defined (DEBUG_SERIAL) && defined (NRF24L01_INSTALLED) #if defined (STM32_BOARD) && defined (DEBUG_SERIAL)
#define XN297DUMP_NRF24L01_INO #undef SEND_SBUS_SERIAL
#undef SEND_CPPM
#ifdef NRF24L01_INSTALLED
#define XN297DUMP_NRF24L01_INO
#endif
#endif
#if not defined (STM32_BOARD) || not defined (TELEMETRY) || (not defined (FRSKY_RX_TELEMETRY) && not defined (AFHDS2A_RX_TELEMETRY) && not defined (BAYANG_RX_TELEMETRY) && not defined (DSM_RX_CYRF6936_INO))
#undef SEND_SBUS_SERIAL
#undef SEND_CPPM
#endif #endif
//Check if Direct inputs defined correctly //Check if Direct inputs defined correctly

View File

@ -342,6 +342,19 @@
#define HOTT_FW_TELEMETRY // Forward received telemetry packets to be decoded by erskyTX and OpenTX #define HOTT_FW_TELEMETRY // Forward received telemetry packets to be decoded by erskyTX and OpenTX
#define BAYANG_RX_TELEMETRY // Forward channels data to TX #define BAYANG_RX_TELEMETRY // Forward channels data to TX
/**************************/
/*** TRAINER SETTINGS ***/
/**************************/
// By default Multi uses the telemetry line to send the received channels using a RX protocol (FrSky, DSM, AFHDS2A, Bayang) to the radio.
// But this does not work on FrSky radios since the telemetry lines of the internal and external modules are shared (hardware limitation).
// Using one of the method below (both can be enabled) on a STM32 module enables to go around that issue but needs a hardware modification on the module itself.
//SBUS signal needs an inverter between the STM32 USART1 TX pin and the radio bay pin 2
//#define SEND_SBUS_SERIAL
//CPPM signal needs a resistor between the STM32 USART1 TX pin and the radio bay pin 2
//#define SEND_CPPM
/****************************/ /****************************/
/*** SERIAL MODE SETTINGS ***/ /*** SERIAL MODE SETTINGS ***/
/****************************/ /****************************/