mirror of
https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git
synced 2025-02-04 15:48:10 +00:00
Removed SBUS trainer option
This commit is contained in:
parent
c0285f81f0
commit
a5428bc180
@ -6,6 +6,7 @@
|
|||||||
14,4,Bayang,DHD_D4,1,Flip,RTH,Pict,Video,HLess,Invert,Rates,TakeOf,EmStop
|
14,4,Bayang,DHD_D4,1,Flip,RTH,Pict,Video,HLess,Invert,Rates,TakeOf,EmStop
|
||||||
14,5,Bayang,QX100,1,Flip,RTH,Pict,Video,HLess,Invert,Rates,TakeOf,EmStop
|
14,5,Bayang,QX100,1,Flip,RTH,Pict,Video,HLess,Invert,Rates,TakeOf,EmStop
|
||||||
59,0,BayangRX,RX,1,AnAux1,AnAux2,Flip,RTH,Pict,Video
|
59,0,BayangRX,RX,1,AnAux1,AnAux2,Flip,RTH,Pict,Video
|
||||||
|
59,1,BayangRX,CPPM,1,AnAux1,AnAux2,Flip,RTH,Pict,Video
|
||||||
41,0,Bugs,3-6-8,0,Arm,Angle,Flip,Pict,Video,LED
|
41,0,Bugs,3-6-8,0,Arm,Angle,Flip,Pict,Video,LED
|
||||||
42,0,BugsMini,Mini,0,Arm,Angle,Flip,Pict,Video,LED
|
42,0,BugsMini,Mini,0,Arm,Angle,Flip,Pict,Video,LED
|
||||||
42,1,BugsMini,3H,0,Arm,Angle,Flip,Pict,Video,LED,AltHol
|
42,1,BugsMini,3H,0,Arm,Angle,Flip,Pict,Video,LED,AltHol
|
||||||
@ -34,6 +35,7 @@
|
|||||||
6,3,DSM,X_11,0,CH5,CH6,CH7,CH8,CH9,CH10,CH11,CH12,n-a,ThKill
|
6,3,DSM,X_11,0,CH5,CH6,CH7,CH8,CH9,CH10,CH11,CH12,n-a,ThKill
|
||||||
6,4,DSM,AUTO,0,CH5,CH6,CH7,CH8,CH9,CH10,CH11,CH12,n-a,ThKill
|
6,4,DSM,AUTO,0,CH5,CH6,CH7,CH8,CH9,CH10,CH11,CH12,n-a,ThKill
|
||||||
70,0,DSM_RX,RX,0,CH5,CH6,CH7,CH8,CH9,CH10,CH11,CH12
|
70,0,DSM_RX,RX,0,CH5,CH6,CH7,CH8,CH9,CH10,CH11,CH12
|
||||||
|
70,1,DSM_RX,CPPM,0,CH5,CH6,CH7,CH8,CH9,CH10,CH11,CH12
|
||||||
45,0,E01X,E012,1,n-a,Flip,n-a,HLess,RTH
|
45,0,E01X,E012,1,n-a,Flip,n-a,HLess,RTH
|
||||||
45,1,E01X,E015,1,Arm,Flip,LED,HLess,RTH
|
45,1,E01X,E015,1,Arm,Flip,LED,HLess,RTH
|
||||||
45,2,E01X,E016H,1,Stop,Flip,n-a,HLess,RTH
|
45,2,E01X,E016H,1,Stop,Flip,n-a,HLess,RTH
|
||||||
@ -54,6 +56,7 @@
|
|||||||
28,4,Flysky_AFHDS2A,PWM_IB16,0,CH5,CH6,CH7,CH8,CH9,CH10,CH11,CH12,CH13,CH14,CH15,CH16
|
28,4,Flysky_AFHDS2A,PWM_IB16,0,CH5,CH6,CH7,CH8,CH9,CH10,CH11,CH12,CH13,CH14,CH15,CH16
|
||||||
28,5,Flysky_AFHDS2A,PPM_IB16,0,CH5,CH6,CH7,CH8,CH9,CH10,CH11,CH12,CH13,CH14,CH15,CH16
|
28,5,Flysky_AFHDS2A,PPM_IB16,0,CH5,CH6,CH7,CH8,CH9,CH10,CH11,CH12,CH13,CH14,CH15,CH16
|
||||||
56,0,Flysky2A_RX,RX,0,CH5,CH6,CH7,CH8,CH9,CH10,CH11,CH12,CH13,CH14
|
56,0,Flysky2A_RX,RX,0,CH5,CH6,CH7,CH8,CH9,CH10,CH11,CH12,CH13,CH14
|
||||||
|
56,1,Flysky2A_RX,CPPM,0,CH5,CH6,CH7,CH8,CH9,CH10,CH11,CH12,CH13,CH14
|
||||||
53,0,Height,5ch,0,Gear
|
53,0,Height,5ch,0,Gear
|
||||||
53,1,Height,8ch,0,Gear,Gyro,Flap,Light
|
53,1,Height,8ch,0,Gear,Gyro,Flap,Light
|
||||||
25,0,FrSkyV,V8,0,CH5,CH6,CH7,CH8
|
25,0,FrSkyV,V8,0,CH5,CH6,CH7,CH8
|
||||||
@ -80,6 +83,7 @@
|
|||||||
55,0,FrSkyRX,RX,0,CH5,CH6,CH7,CH8,CH9,CH10,CH11,CH12,CH13,CH14,CH15,CH16
|
55,0,FrSkyRX,RX,0,CH5,CH6,CH7,CH8,CH9,CH10,CH11,CH12,CH13,CH14,CH15,CH16
|
||||||
55,1,FrSkyRX,CloneTX,0
|
55,1,FrSkyRX,CloneTX,0
|
||||||
55,2,FrSkyRX,EraseTX,0
|
55,2,FrSkyRX,EraseTX,0
|
||||||
|
55,3,FrSkyRX,CPPM,0,CH5,CH6,CH7,CH8,CH9,CH10,CH11,CH12,CH13,CH14,CH15,CH16
|
||||||
58,0,FX816,P38,1
|
58,0,FX816,P38,1
|
||||||
20,0,FY326,FY326,1,Flip,RTH,HLess,Expert,Calib
|
20,0,FY326,FY326,1,Flip,RTH,HLess,Expert,Calib
|
||||||
20,1,FY326,FY319,1,Flip,RTH,HLess,Expert,Calib
|
20,1,FY326,FY319,1,Flip,RTH,HLess,Expert,Calib
|
||||||
|
@ -189,9 +189,9 @@ 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)
|
#ifdef SEND_CPPM
|
||||||
if(sub_protocol>0)
|
if(sub_protocol>0)
|
||||||
telemetry_link = 0x80 + sub_protocol; // Disable telemetry output, type SBUS=1, type CPPM=2
|
telemetry_link |= 0x80; // Disable telemetry output
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
rx_data_started = true;
|
rx_data_started = true;
|
||||||
|
@ -151,9 +151,9 @@ uint16_t BAYANG_RX_callback()
|
|||||||
if ((telemetry_link & 0x7F) == 0) {
|
if ((telemetry_link & 0x7F) == 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)
|
#ifdef SEND_CPPM
|
||||||
if(sub_protocol>0)
|
if(sub_protocol>0)
|
||||||
telemetry_link = 0x80 + sub_protocol; // Disable telemetry output, type SBUS=1, type CPPM=2
|
telemetry_link |= 0x80; // Disable telemetry output
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
rx_data_started = true;
|
rx_data_started = true;
|
||||||
|
@ -155,9 +155,9 @@ 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)
|
#ifdef SEND_CPPM
|
||||||
if(sub_protocol>0)
|
if(sub_protocol>0)
|
||||||
telemetry_link = 0x80 + sub_protocol; // Disable telemetry output, type SBUS=1, type CPPM=2
|
telemetry_link |= 0x80; // Disable telemetry output
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -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 || sub_protocol >= FRSKY_SBUS) // FRSKY_RX, FRSKY_SBUS, FRSKY_CPPM
|
if(sub_protocol == FRSKY_RX || sub_protocol == FRSKY_CPPM) // FRSKY_RX, 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,9 +579,9 @@ 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)
|
#ifdef SEND_CPPM
|
||||||
if(sub_protocol >= FRSKY_SBUS)
|
if(sub_protocol == FRSKY_CPPM)
|
||||||
telemetry_link = 0x81 + sub_protocol - FRSKY_SBUS; // Disable telemetry output, type SBUS=1, type CPPM=2
|
telemetry_link |= 0x80; // Disable telemetry output
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
pps_counter++;
|
pps_counter++;
|
||||||
|
@ -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,Multi,CloneTX,EraseTX,SBUS,CPPM
|
55,Frsky_RX,Multi,CloneTX,EraseTX,CPPM
|
||||||
56,AFHDS2A_RX,Multi,SBUS,CPPM
|
56,AFHDS2A_RX,Multi,CPPM
|
||||||
57,HoTT,Sync,No_Sync
|
57,HoTT,Sync,No_Sync
|
||||||
58,FX816,P38
|
58,FX816,P38
|
||||||
59,Bayang_RX,Multi,SBUS,CPPM
|
59,Bayang_RX,Multi,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,Multi,SBUS,CPPM
|
70,DSM_RX,Multi,CPPM
|
||||||
71,JJRC345,JJRC345,SkyTmblr
|
71,JJRC345,JJRC345,SkyTmblr
|
||||||
72,Q90C
|
72,Q90C
|
||||||
73,Kyosho,FHSS,Hype
|
73,Kyosho,FHSS,Hype
|
||||||
|
@ -156,16 +156,16 @@ const char STR_SUBTYPE_JJRC345[] = "\x08""JJRC345\0""SkyTmblr";
|
|||||||
|
|
||||||
#define NO_SUBTYPE nullptr
|
#define NO_SUBTYPE nullptr
|
||||||
|
|
||||||
#if defined (SEND_SBUS_SERIAL) || defined (SEND_CPPM)
|
#ifdef SEND_CPPM
|
||||||
const char STR_SUBTYPE_FRSKY_RX[] = "\x07""Multi\0 ""CloneTX""EraseTX""SBUS\0 ""CPPM\0 ";
|
const char STR_SUB_FRSKY_RX[] = "\x07""Multi\0 ""CloneTX""EraseTX""CPPM\0 ";
|
||||||
#define FRSBUSCPPM 5
|
#define FRCPPM 4
|
||||||
const char STR_SBUS_CPPM[] = "\x05""Multi""SBUS\0""CPPM\0";
|
const char STR_CPPM[] = "\x05""Multi""CPPM\0";
|
||||||
#define SBUS_CPPM 3
|
#define NBR_CPPM 2
|
||||||
#else
|
#else
|
||||||
const char STR_SUBTYPE_FRSKY_RX[] = "\x07""Multi\0 ""CloneTX""EraseTX";
|
const char STR_SUB_FRSKY_RX[] = "\x07""Multi\0 ""CloneTX""EraseTX";
|
||||||
#define FRSBUSCPPM 3
|
#define FRCPPM 3
|
||||||
#define STR_SBUS_CPPM NO_SUBTYPE
|
#define STR_CPPM NO_SUBTYPE
|
||||||
#define SBUS_CPPM 0
|
#define NBR_CPPM 0
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
enum
|
enum
|
||||||
@ -195,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, STR_SBUS_CPPM, SBUS_CPPM, OPTION_NONE, 0, 0, SW_NRF, BAYANG_RX_init, BAYANG_RX_callback },
|
{PROTO_BAYANG_RX, STR_BAYANG_RX, STR_CPPM, NBR_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 },
|
||||||
@ -228,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, STR_SBUS_CPPM, SBUS_CPPM, OPTION_NONE, 0, 1, SW_CYRF, DSM_RX_init, DSM_RX_callback },
|
{PROTO_DSM_RX, STR_DSM_RX, STR_CPPM, NBR_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 },
|
||||||
@ -258,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,STR_SBUS_CPPM, SBUS_CPPM, OPTION_NONE, 0, 0, SW_A7105, AFHDS2A_RX_init, AFHDS2A_RX_callback },
|
{PROTO_AFHDS2A_RX, STR_AFHDS2A_RX,STR_CPPM, NBR_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, FRSBUSCPPM, OPTION_RFTUNE, 0, 0, SW_CC2500, FRSKY_RX_init, FRSKY_RX_callback },
|
{PROTO_FRSKY_RX, STR_FRSKY_RX, STR_SUB_FRSKY_RX, FRCPPM, 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 },
|
||||||
|
@ -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 57
|
#define VERSION_PATCH_LEVEL 58
|
||||||
|
|
||||||
#define MODE_SERIAL 0
|
#define MODE_SERIAL 0
|
||||||
|
|
||||||
@ -378,8 +378,7 @@ 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 = 3,
|
||||||
FRSKY_CPPM = 4,
|
|
||||||
};
|
};
|
||||||
|
|
||||||
enum FRSKYL
|
enum FRSKYL
|
||||||
|
@ -834,24 +834,17 @@ bool Update_All()
|
|||||||
#endif //ENABLE_PPM
|
#endif //ENABLE_PPM
|
||||||
update_led_status();
|
update_led_status();
|
||||||
|
|
||||||
#if defined(SEND_SBUS_SERIAL) || defined(SEND_CPPM)
|
#ifdef SEND_CPPM
|
||||||
if ( telemetry_link & 0x80 )
|
if ( telemetry_link & 0x80 )
|
||||||
{ // Protocol requests telemetry to be disabled
|
{ // Protocol requests telemetry to be disabled
|
||||||
if( protocol == PROTO_FRSKY_RX || protocol == PROTO_AFHDS2A_RX || protocol == PROTO_BAYANG_RX || protocol == PROTO_DSM_RX )
|
if( protocol == PROTO_FRSKY_RX || protocol == PROTO_AFHDS2A_RX || protocol == PROTO_BAYANG_RX || protocol == PROTO_DSM_RX )
|
||||||
{ // RX protocol
|
{ // RX protocol
|
||||||
if(RX_LQI == 0)
|
if(RX_LQI == 0)
|
||||||
telemetry_link = 0x00; // restore normal telemetry on connection loss
|
telemetry_link = 0x00; // restore normal telemetry on connection loss
|
||||||
else
|
else if(telemetry_link & 1)
|
||||||
{
|
{ // New data available
|
||||||
#ifdef SEND_SBUS_SERIAL
|
Send_CCPM_USART1();
|
||||||
if(telemetry_link & 1)
|
telemetry_link &= 0xFE; // update done
|
||||||
Send_SBUS_USART1();
|
|
||||||
#endif
|
|
||||||
#ifdef SEND_CPPM
|
|
||||||
if(telemetry_link & 2)
|
|
||||||
Send_CCPM_USART1();
|
|
||||||
#endif
|
|
||||||
telemetry_link = 0x80; // update done
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -1169,6 +1162,11 @@ static void protocol_init()
|
|||||||
#endif
|
#endif
|
||||||
binding_idx=0;
|
binding_idx=0;
|
||||||
|
|
||||||
|
//Stop CPPM if it was previously running
|
||||||
|
#ifdef SEND_CPPM
|
||||||
|
release_trainer_ppm();
|
||||||
|
#endif
|
||||||
|
|
||||||
//Set global ID and rx_tx_addr
|
//Set global ID and rx_tx_addr
|
||||||
MProtocol_id = RX_num + MProtocol_id_master;
|
MProtocol_id = RX_num + MProtocol_id_master;
|
||||||
set_rx_tx_addr(MProtocol_id);
|
set_rx_tx_addr(MProtocol_id);
|
||||||
@ -2113,95 +2111,13 @@ static void __attribute__((unused)) crc8_update(uint8_t byte)
|
|||||||
|
|
||||||
/**************************/
|
/**************************/
|
||||||
/**************************/
|
/**************************/
|
||||||
/** SBUS/CPPM routines **/
|
/** CPPM routines **/
|
||||||
/**************************/
|
/**************************/
|
||||||
/**************************/
|
/**************************/
|
||||||
#if defined (SEND_SBUS_SERIAL) || defined (SEND_CPPM)
|
|
||||||
uint32_t TrainerTimer ;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef SEND_CPPM
|
|
||||||
bool CppmInitialised = false;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef SEND_SBUS_SERIAL
|
|
||||||
uint8_t SbusFrame[26] ;
|
|
||||||
uint16_t SbusChannels[16] ;
|
|
||||||
bool SbusInitialised = false;
|
|
||||||
|
|
||||||
void Init_SBUS_USART1()
|
|
||||||
{
|
|
||||||
RCC_BASE->APB2ENR |= RCC_APB2ENR_USART1EN ; // Enable 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;
|
|
||||||
#ifdef SEND_CPPM
|
|
||||||
if(CppmInitialised)
|
|
||||||
{
|
|
||||||
TIMER1_BASE->CR1 = 0 ;
|
|
||||||
CppmInitialised = false;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
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
|
#ifdef SEND_CPPM
|
||||||
#define PPM_CENTER 1500*2
|
#define PPM_CENTER 1500*2
|
||||||
|
uint32_t TrainerTimer ;
|
||||||
|
bool CppmInitialised = false;
|
||||||
uint16_t *TrainerPulsePtr ;
|
uint16_t *TrainerPulsePtr ;
|
||||||
uint16_t TrainerPpmStream[10] ;
|
uint16_t TrainerPpmStream[10] ;
|
||||||
int16_t CppmChannels[8] ;
|
int16_t CppmChannels[8] ;
|
||||||
@ -2241,13 +2157,6 @@ static void __attribute__((unused)) crc8_update(uint8_t byte)
|
|||||||
RCC_BASE->APB2ENR |= RCC_APB2ENR_TIM1EN ; // Enable clock
|
RCC_BASE->APB2ENR |= RCC_APB2ENR_TIM1EN ; // Enable clock
|
||||||
setupTrainerPulses() ;
|
setupTrainerPulses() ;
|
||||||
RCC_BASE->APB2ENR |= RCC_APB2ENR_IOPAEN ; // Enable portA clock
|
RCC_BASE->APB2ENR |= RCC_APB2ENR_IOPAEN ; // Enable portA clock
|
||||||
#ifdef SEND_SBUS_SERIAL
|
|
||||||
if(SbusInitialised)
|
|
||||||
{
|
|
||||||
SbusInitialised = false ;
|
|
||||||
USART1_BASE->CR1 = 0 ;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
RCC_BASE->APB2ENR &= ~RCC_APB2ENR_USART1EN ; // Disable USART1
|
RCC_BASE->APB2ENR &= ~RCC_APB2ENR_USART1EN ; // Disable USART1
|
||||||
|
|
||||||
GPIOA_BASE->CRH &= ~0x00F0 ;
|
GPIOA_BASE->CRH &= ~0x00F0 ;
|
||||||
@ -2261,19 +2170,27 @@ static void __attribute__((unused)) crc8_update(uint8_t byte)
|
|||||||
TIMER1_BASE->CCMR1 = 0x6000 ; // PWM mode 1 (header file has incorrect bits)
|
TIMER1_BASE->CCMR1 = 0x6000 ; // PWM mode 1 (header file has incorrect bits)
|
||||||
TIMER1_BASE->EGR = 1 ;
|
TIMER1_BASE->EGR = 1 ;
|
||||||
TIMER1_BASE->CCER = TIMER_CCER_CC2E ;
|
TIMER1_BASE->CCER = TIMER_CCER_CC2E ;
|
||||||
// TIMER1_BASE->DIER |= TIMER_DIER_CC2IE ;
|
|
||||||
TIMER1_BASE->DIER |= TIMER_DIER_UIE ;
|
TIMER1_BASE->DIER |= TIMER_DIER_UIE ;
|
||||||
TIMER1_BASE->CR1 = TIMER_CR1_CEN ;
|
TIMER1_BASE->CR1 = TIMER_CR1_CEN ;
|
||||||
nvic_irq_set_priority(NVIC_TIMER1_CC, 4 ) ;
|
nvic_irq_set_priority(NVIC_TIMER1_CC, 4 ) ;
|
||||||
nvic_irq_set_priority(NVIC_TIMER1_UP, 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_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_CH1,tim1_cc); // Assign function to Timer1/Comp2 interrupt
|
||||||
// HWTimer1.attachInterrupt(TIMER_CH2,tim1_cc); // Assign function to Timer1/Comp2 interrupt
|
|
||||||
|
|
||||||
CppmInitialised = true ;
|
CppmInitialised = true ;
|
||||||
HWTimer1.resume() ;
|
HWTimer1.resume() ;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void release_trainer_ppm()
|
||||||
|
{
|
||||||
|
if ( CppmInitialised )
|
||||||
|
{
|
||||||
|
TIMER1_BASE->CR1 = 0 ;
|
||||||
|
pinMode(PA9,INPUT) ;
|
||||||
|
CppmInitialised = false ;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void tim1_up()
|
void tim1_up()
|
||||||
{
|
{
|
||||||
#define TIMER1_SR_MASK 0x1FFF
|
#define TIMER1_SR_MASK 0x1FFF
|
||||||
@ -2294,12 +2211,6 @@ static void __attribute__((unused)) crc8_update(uint8_t byte)
|
|||||||
|
|
||||||
void tim1_cc()
|
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 ) )
|
if ( ( TIMER1_BASE->DIER & TIMER_DIER_CC1IE ) && ( TIMER1_BASE->SR & TIMER_SR_CC1IF ) )
|
||||||
{
|
{
|
||||||
// compare interrupt
|
// compare interrupt
|
||||||
|
@ -529,14 +529,12 @@
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if defined (STM32_BOARD) && defined (DEBUG_SERIAL)
|
#if defined (STM32_BOARD) && defined (DEBUG_SERIAL)
|
||||||
#undef SEND_SBUS_SERIAL
|
|
||||||
#undef SEND_CPPM
|
#undef SEND_CPPM
|
||||||
#ifdef NRF24L01_INSTALLED
|
#ifdef NRF24L01_INSTALLED
|
||||||
#define XN297DUMP_NRF24L01_INO
|
#define XN297DUMP_NRF24L01_INO
|
||||||
#endif
|
#endif
|
||||||
#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))
|
#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
|
#undef SEND_CPPM
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -347,12 +347,9 @@
|
|||||||
/**************************/
|
/**************************/
|
||||||
// By default Multi uses the telemetry line to send the received channels using a RX protocol (FrSky, DSM, AFHDS2A, Bayang) to the radio.
|
// 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).
|
// 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.
|
// On a STM32 module and with a simple hardware modification, you can go around this limitation using CPPM to send the trainer information to the radio.
|
||||||
|
// Hardware modification: add a 1K resistor between the STM32 USART1 TX pin (Boot0 programming TX pin) and the radio bay pin 2.
|
||||||
//SBUS signal needs an inverter between the STM32 USART1 TX pin and the radio bay pin 2
|
//Uncomment to enable
|
||||||
//#define SEND_SBUS_SERIAL
|
|
||||||
|
|
||||||
//CPPM signal needs a resistor between the STM32 USART1 TX pin and the radio bay pin 2
|
|
||||||
//#define SEND_CPPM
|
//#define SEND_CPPM
|
||||||
|
|
||||||
/****************************/
|
/****************************/
|
||||||
|
Loading…
x
Reference in New Issue
Block a user