6 #include "drivers/system_common.h"
8 #include "drivers/serial_common.h"
9 #include "drivers/serial_uart.h"
10 #include "runtime_config.h"
14 #include "rx_common.h"
17 // driver for SBUS receiver using UART2
19 #define SBUS_MAX_CHANNEL 8
20 #define SBUS_FRAME_SIZE 25
21 #define SBUS_SYNCBYTE 0x0F
22 #define SBUS_OFFSET 988
24 static bool sbusFrameDone
= false;
25 static void sbusDataReceive(uint16_t c
);
26 static uint16_t sbusReadRawRC(rxConfig_t
*rxConfig
, rxRuntimeConfig_t
*rxRuntimeConfig
, uint8_t chan
);
28 static uint32_t sbusChannelData
[SBUS_MAX_CHANNEL
];
30 //rxConfig_t *rxConfig;
32 void sbusInit(rxConfig_t
*rxConfig
, rxRuntimeConfig_t
*rxRuntimeConfig
, rcReadRawDataPtr
*callback
)
36 //rxConfig = initialRxConfig;
38 for (b
= 0; b
< SBUS_MAX_CHANNEL
; b
++)
39 sbusChannelData
[b
] = 2 * (rxConfig
->midrc
- SBUS_OFFSET
);
40 core
.rcvrport
= uartOpen(USART2
, sbusDataReceive
, 100000, (portMode_t
)(MODE_RX
| MODE_SBUS
));
42 *callback
= sbusReadRawRC
;
43 rxRuntimeConfig
->channelCount
= SBUS_MAX_CHANNEL
;
48 unsigned int chan0
: 11;
49 unsigned int chan1
: 11;
50 unsigned int chan2
: 11;
51 unsigned int chan3
: 11;
52 unsigned int chan4
: 11;
53 unsigned int chan5
: 11;
54 unsigned int chan6
: 11;
55 unsigned int chan7
: 11;
56 unsigned int chan8
: 11;
57 unsigned int chan9
: 11;
58 unsigned int chan10
: 11;
59 unsigned int chan11
: 11;
60 } __attribute__ ((__packed__
));
64 uint8_t in
[SBUS_FRAME_SIZE
];
70 // Receive ISR callback
71 static void sbusDataReceive(uint16_t c
)
74 static uint32_t sbusTimeLast
;
75 static uint8_t sbusFramePosition
;
78 if ((sbusTime
- sbusTimeLast
) > 2500) // sbus2 fast timing
79 sbusFramePosition
= 0;
80 sbusTimeLast
= sbusTime
;
82 if (sbusFramePosition
== 0 && c
!= SBUS_SYNCBYTE
)
85 sbusFrameDone
= false; // lazy main loop didnt fetch the stuff
86 if (sbusFramePosition
!= 0)
87 sbus
.in
[sbusFramePosition
- 1] = (uint8_t)c
;
89 if (sbusFramePosition
== SBUS_FRAME_SIZE
- 1) {
91 sbusFramePosition
= 0;
97 bool sbusFrameComplete(void)
100 if (!((sbus
.in
[22] >> 3) & 0x0001)) { // failsave flag
101 failsafeCnt
= 0; // clear FailSafe counter
102 sbusChannelData
[0] = sbus
.msg
.chan0
;
103 sbusChannelData
[1] = sbus
.msg
.chan1
;
104 sbusChannelData
[2] = sbus
.msg
.chan2
;
105 sbusChannelData
[3] = sbus
.msg
.chan3
;
106 sbusChannelData
[4] = sbus
.msg
.chan4
;
107 sbusChannelData
[5] = sbus
.msg
.chan5
;
108 sbusChannelData
[6] = sbus
.msg
.chan6
;
109 sbusChannelData
[7] = sbus
.msg
.chan7
;
110 // need more channels? No problem. Add them.
111 sbusFrameDone
= false;
114 sbusFrameDone
= false;
119 static uint16_t sbusReadRawRC(rxConfig_t
*rxConfig
, rxRuntimeConfig_t
*rxRuntimeConfig
, uint8_t chan
)
121 return sbusChannelData
[rxConfig
->rcmap
[chan
]] / 2 + SBUS_OFFSET
;