-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathsbus.c
194 lines (158 loc) · 7.39 KB
/
sbus.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
////////////////////////////////////////////////////////////////////////////////
/// @file
/// @brief S.Bus decode module.
////////////////////////////////////////////////////////////////////////////////
// *****************************************************************************
// ************************** System Include Files *****************************
// *****************************************************************************
// *****************************************************************************
// ************************** User Include Files *******************************
// *****************************************************************************
#include "sbus.h"
#include "uart.h"
// *****************************************************************************
// ************************** Macros *******************************************
// *****************************************************************************
// *****************************************************************************
// ************************** Defines ******************************************
// *****************************************************************************
/// The number of bytes which an S.Bus payload packet.
#define SBUS_PAYLOAD_LEN 23
#define SBUS_HEADER_VALUE 0x0F ///< Value of S.Bus Header byte.
#define SBUS_FOOTER_VALUE 0x00 ///< Value of S.Bus Footer byte.
// *****************************************************************************
// ************************** Definitions **************************************
// *****************************************************************************
static uint16_t sbus_ch_data[ SBUS_CH_MAX ];
// *****************************************************************************
// ************************** Function Prototypes ******************************
// *****************************************************************************
static void SBusProcess( uint8_t byte_in );
// *****************************************************************************
// ************************** Global Functions *********************************
// *****************************************************************************
void SBusTask( void )
{
const UART_RX_BUF_S* uartRxBuf_p;
uint16_t rx_data_byte_idx;
// Get UART buffered data.
uartRxBuf_p = UARTGet( UART_MODULE_2 );
// Process each byte of the received data.
for( rx_data_byte_idx = 0;
rx_data_byte_idx < uartRxBuf_p->data_len;
rx_data_byte_idx++ )
{
// Execute the state machine with the received data.
SBusProcess( uartRxBuf_p->data[ rx_data_byte_idx ] );
}
}
void SBusListGet( uint16_t ch_list[ SBUS_CH_MAX ] )
{
memcpy( &ch_list[ 0 ], &sbus_ch_data[ 0 ], sizeof( sbus_ch_data ) );
}
// *****************************************************************************
// ************************** Static Functions *********************************
// *****************************************************************************
////////////////////////////////////////////////////////////////////////////////
/// @brief Process a byte of received S.Bus data.
///
/// @param byte_in
/// The byte of S.Bus data.
///
/// This function implements a state machine for determining the Start/Data/End
/// bytes of the S.Bus data. Once an entire data payload is received, the
/// payload is decoded and populate in module data variable \a sbus_ch_data.
////////////////////////////////////////////////////////////////////////////////
static void SBusProcess( uint8_t byte_in )
{
static uint8_t byte_in_prev = SBUS_HEADER_VALUE;
/// Structure defining the contents of an S.Bus data payload. Each of the
/// channel fields (ch1:ch16) occupies 11 bytes.
typedef union
{
uint8_t data_array[ SBUS_PAYLOAD_LEN ];
struct __attribute__ ((packed))
{
uint32_t ch1 : 11;
uint32_t ch2 : 11;
uint32_t ch3_lo : 10;
uint32_t ch3_hi : 1;
uint32_t ch4 : 11;
uint32_t ch5 : 11;
uint32_t ch6_lo : 9;
uint32_t ch6_hi : 2;
uint32_t ch7 : 11;
uint32_t ch8 : 11;
uint32_t ch9_lo : 8;
uint32_t ch9_hi : 3;
uint32_t ch10 : 11;
uint32_t ch11 : 11;
uint32_t ch12_lo : 7;
uint32_t ch12_hi : 4;
uint32_t ch13 : 11;
uint32_t ch14 : 11;
uint32_t ch15_lo : 6;
uint32_t ch15_hi : 5;
uint32_t ch16 : 11;
uint32_t ch17 : 1; // digital channel
uint32_t ch18 : 1; // digital channel
uint32_t frame_lost : 1;
uint32_t failsafe_act : 1;
};
} SBUS_DATA_U;
static enum
{
SM_START,
SM_DATA,
} sm_state = SM_START;
static SBUS_DATA_U sbus_data;
// Note: The state machine is started by checking that the end/footer byte
// is received followed by the start/header byte. This checking of two
// consecutive bytes provides a better detection of the start of a data
// packet - i.e. rather than only checking the start/header byte.
switch( sm_state )
{
case SM_START:
{
// Footer followed by Header byte has been received ?
if( ( byte_in_prev == SBUS_FOOTER_VALUE ) &&
( byte_in == SBUS_HEADER_VALUE ) )
{
sm_state++;
}
break;
}
case SM_DATA:
{
static uint8_t ch_data_idx = 0;
sbus_data.data_array[ ch_data_idx ] = byte_in;
ch_data_idx++;
if( ch_data_idx >= SBUS_PAYLOAD_LEN )
{
// Reset for reception of next data stream.
ch_data_idx = 0;
sm_state = SM_START;
// Process the received data.
sbus_ch_data[ 0 ] = sbus_data.ch1;
sbus_ch_data[ 1 ] = sbus_data.ch2;
sbus_ch_data[ 2 ] = ( sbus_data.ch3_hi << 10 ) | sbus_data.ch3_lo;
sbus_ch_data[ 3 ] = sbus_data.ch4;
sbus_ch_data[ 4 ] = sbus_data.ch5;
sbus_ch_data[ 5 ] = ( sbus_data.ch6_hi << 9 ) | sbus_data.ch6_lo;
sbus_ch_data[ 6 ] = sbus_data.ch7;
sbus_ch_data[ 7 ] = sbus_data.ch8;
sbus_ch_data[ 8 ] = ( sbus_data.ch9_hi << 8 ) | sbus_data.ch9_lo;
sbus_ch_data[ 9 ] = sbus_data.ch10;
sbus_ch_data[ 10 ] = sbus_data.ch11;
sbus_ch_data[ 11 ] = ( sbus_data.ch12_hi << 7 ) | sbus_data.ch12_lo;
sbus_ch_data[ 12 ] = sbus_data.ch13;
sbus_ch_data[ 13 ] = sbus_data.ch14;
sbus_ch_data[ 14 ] = ( sbus_data.ch15_hi << 6 ) | sbus_data.ch15_lo;
sbus_ch_data[ 15 ] = sbus_data.ch16;
}
break;
}
}
// Latch current byte for next evaluation.
byte_in_prev = byte_in;
}