Line data Source code
1 : #include <errno.h>
2 : #include <fcntl.h>
3 : #include <poll.h>
4 : #include <stdint.h>
5 : #include <stdio.h>
6 : #include <stdlib.h>
7 : #include <string.h>
8 : #include <sys/mman.h>
9 : #include <sys/stat.h>
10 : #include <sys/time.h>
11 : #include <unistd.h>
12 :
13 : #include <AP_HAL/AP_HAL.h>
14 : #include <AP_HAL/utility/dsm.h>
15 : #include <AP_RCProtocol/AP_RCProtocol_SBUS.h>
16 :
17 : #include "RCInput.h"
18 :
19 : #define MIN_NUM_CHANNELS 5
20 :
21 : extern const AP_HAL::HAL& hal;
22 :
23 : using namespace Linux;
24 :
25 48 : RCInput::RCInput()
26 : {
27 48 : ppm_state._channel_counter = -1;
28 48 : }
29 :
30 0 : void RCInput::init()
31 : {
32 0 : }
33 :
34 0 : bool RCInput::new_input()
35 : {
36 0 : bool ret = rc_input_count != last_rc_input_count;
37 0 : if (ret) {
38 0 : last_rc_input_count.store(rc_input_count);
39 : }
40 0 : return ret;
41 : }
42 :
43 0 : uint8_t RCInput::num_channels()
44 : {
45 0 : return _num_channels;
46 : }
47 :
48 0 : void RCInput::set_num_channels(uint8_t num)
49 : {
50 0 : _num_channels = num;
51 0 : }
52 :
53 0 : uint16_t RCInput::read(uint8_t ch)
54 : {
55 0 : if (ch >= _num_channels) {
56 0 : return 0;
57 : }
58 0 : return _pwm_values[ch];
59 : }
60 :
61 0 : uint8_t RCInput::read(uint16_t* periods, uint8_t len)
62 : {
63 : uint8_t i;
64 0 : for (i=0; i<len; i++) {
65 0 : periods[i] = read(i);
66 : }
67 0 : return len;
68 : }
69 :
70 : /*
71 : process a PPM-sum pulse of the given width
72 : */
73 0 : void RCInput::_process_ppmsum_pulse(uint16_t width_usec)
74 : {
75 0 : if (width_usec >= 2700) {
76 : // a long pulse indicates the end of a frame. Reset the
77 : // channel counter so next pulse is channel 0
78 0 : if (ppm_state._channel_counter >= MIN_NUM_CHANNELS) {
79 0 : for (uint8_t i=0; i<ppm_state._channel_counter; i++) {
80 0 : _pwm_values[i] = ppm_state._pulse_capt[i];
81 : }
82 0 : _num_channels = ppm_state._channel_counter;
83 0 : rc_input_count++;
84 : }
85 0 : ppm_state._channel_counter = 0;
86 0 : return;
87 : }
88 0 : if (ppm_state._channel_counter == -1) {
89 : // we are not synchronised
90 0 : return;
91 : }
92 :
93 : /*
94 : we limit inputs to between 700usec and 2300usec. This allows us
95 : to decode SBUS on the same pin, as SBUS will have a maximum
96 : pulse width of 100usec
97 : */
98 0 : if (width_usec > 700 && width_usec < 2300) {
99 : // take a reading for the current channel
100 : // buffer these
101 0 : ppm_state._pulse_capt[ppm_state._channel_counter] = width_usec;
102 :
103 : // move to next channel
104 0 : ppm_state._channel_counter++;
105 : }
106 :
107 : // if we have reached the maximum supported channels then
108 : // mark as unsynchronised, so we wait for a wide pulse
109 0 : if (ppm_state._channel_counter >= LINUX_RC_INPUT_NUM_CHANNELS) {
110 0 : for (uint8_t i=0; i<ppm_state._channel_counter; i++) {
111 0 : _pwm_values[i] = ppm_state._pulse_capt[i];
112 : }
113 0 : _num_channels = ppm_state._channel_counter;
114 0 : rc_input_count++;
115 0 : ppm_state._channel_counter = -1;
116 : }
117 : }
118 :
119 : /*
120 : process a SBUS input pulse of the given width
121 : */
122 0 : void RCInput::_process_sbus_pulse(uint16_t width_s0, uint16_t width_s1)
123 : {
124 : // convert to bit widths, allowing for up to 1usec error, assuming 100000 bps
125 0 : uint16_t bits_s0 = (width_s0+1) / 10;
126 0 : uint16_t bits_s1 = (width_s1+1) / 10;
127 : uint16_t nlow;
128 :
129 0 : uint8_t byte_ofs = sbus_state.bit_ofs/12;
130 0 : uint8_t bit_ofs = sbus_state.bit_ofs%12;
131 :
132 0 : if (bits_s0 == 0 || bits_s1 == 0) {
133 : // invalid data
134 0 : goto reset;
135 : }
136 :
137 0 : if (bits_s0+bit_ofs > 10) {
138 : // invalid data as last two bits must be stop bits
139 0 : goto reset;
140 : }
141 :
142 : // pull in the high bits
143 0 : sbus_state.bytes[byte_ofs] |= ((1U<<bits_s0)-1) << bit_ofs;
144 0 : sbus_state.bit_ofs += bits_s0;
145 0 : bit_ofs += bits_s0;
146 :
147 : // pull in the low bits
148 0 : nlow = bits_s1;
149 0 : if (nlow + bit_ofs > 12) {
150 0 : nlow = 12 - bit_ofs;
151 : }
152 0 : bits_s1 -= nlow;
153 0 : sbus_state.bit_ofs += nlow;
154 :
155 0 : if (sbus_state.bit_ofs == 25*12 && bits_s1 > 12) {
156 : // we have a full frame
157 : uint8_t bytes[25];
158 : uint8_t i;
159 0 : for (i=0; i<25; i++) {
160 : // get inverted data
161 0 : uint16_t v = ~sbus_state.bytes[i];
162 : // check start bit
163 0 : if ((v & 1) != 0) {
164 0 : goto reset;
165 : }
166 : // check stop bits
167 0 : if ((v & 0xC00) != 0xC00) {
168 0 : goto reset;
169 : }
170 : // check parity
171 0 : uint8_t parity = 0, j;
172 0 : for (j=1; j<=8; j++) {
173 0 : parity ^= (v & (1U<<j))?1:0;
174 : }
175 0 : if (parity != (v&0x200)>>9) {
176 0 : goto reset;
177 : }
178 0 : bytes[i] = ((v>>1) & 0xFF);
179 : }
180 : uint16_t values[LINUX_RC_INPUT_NUM_CHANNELS];
181 0 : uint16_t num_values=0;
182 0 : bool sbus_failsafe=false;
183 0 : if (AP_RCProtocol_SBUS::sbus_decode(bytes, values, &num_values,
184 : sbus_failsafe,
185 0 : LINUX_RC_INPUT_NUM_CHANNELS) &&
186 0 : num_values >= MIN_NUM_CHANNELS) {
187 0 : for (i=0; i<num_values; i++) {
188 0 : _pwm_values[i] = values[i];
189 : }
190 0 : _num_channels = num_values;
191 0 : if (!sbus_failsafe) {
192 0 : rc_input_count++;
193 : }
194 : }
195 0 : goto reset;
196 0 : } else if (bits_s1 > 12) {
197 : // break
198 0 : goto reset;
199 : }
200 0 : return;
201 0 : reset:
202 0 : memset(&sbus_state, 0, sizeof(sbus_state));
203 : }
204 :
205 0 : void RCInput::_process_dsm_pulse(uint16_t width_s0, uint16_t width_s1)
206 : {
207 : // convert to bit widths, allowing for up to 1usec error, assuming 115200 bps
208 0 : uint16_t bits_s0 = ((width_s0+4)*(uint32_t)115200) / 1000000;
209 0 : uint16_t bits_s1 = ((width_s1+4)*(uint32_t)115200) / 1000000;
210 : uint8_t bit_ofs, byte_ofs;
211 : uint16_t nbits;
212 :
213 0 : if (bits_s0 == 0 || bits_s1 == 0) {
214 : // invalid data
215 0 : goto reset;
216 : }
217 :
218 0 : byte_ofs = dsm_state.bit_ofs/10;
219 0 : bit_ofs = dsm_state.bit_ofs%10;
220 :
221 0 : if(byte_ofs > 15) {
222 : // invalid data
223 0 : goto reset;
224 : }
225 :
226 : // pull in the high bits
227 0 : nbits = bits_s0;
228 0 : if (nbits+bit_ofs > 10) {
229 0 : nbits = 10 - bit_ofs;
230 : }
231 0 : dsm_state.bytes[byte_ofs] |= ((1U<<nbits)-1) << bit_ofs;
232 0 : dsm_state.bit_ofs += nbits;
233 0 : bit_ofs += nbits;
234 :
235 0 : if (bits_s0 - nbits > 10) {
236 0 : if (dsm_state.bit_ofs == 16*10) {
237 : // we have a full frame
238 : uint8_t bytes[16];
239 : uint8_t i;
240 0 : for (i=0; i<16; i++) {
241 : // get raw data
242 0 : uint16_t v = dsm_state.bytes[i];
243 :
244 : // check start bit
245 0 : if ((v & 1) != 0) {
246 0 : goto reset;
247 : }
248 : // check stop bits
249 0 : if ((v & 0x200) != 0x200) {
250 0 : goto reset;
251 : }
252 0 : bytes[i] = ((v>>1) & 0xFF);
253 : }
254 : uint16_t values[8];
255 0 : uint16_t num_values=0;
256 0 : if (dsm_decode(AP_HAL::micros64(), bytes, values, &num_values, 8) &&
257 0 : num_values >= MIN_NUM_CHANNELS) {
258 0 : for (i=0; i<num_values; i++) {
259 0 : _pwm_values[i] = values[i];
260 : }
261 0 : _num_channels = num_values;
262 0 : rc_input_count++;
263 : }
264 : }
265 0 : memset(&dsm_state, 0, sizeof(dsm_state));
266 : }
267 :
268 0 : byte_ofs = dsm_state.bit_ofs/10;
269 0 : bit_ofs = dsm_state.bit_ofs%10;
270 :
271 0 : if (bits_s1+bit_ofs > 10) {
272 : // invalid data
273 0 : goto reset;
274 : }
275 :
276 : // pull in the low bits
277 0 : dsm_state.bit_ofs += bits_s1;
278 0 : return;
279 0 : reset:
280 0 : memset(&dsm_state, 0, sizeof(dsm_state));
281 : }
282 :
283 0 : void RCInput::_process_pwm_pulse(uint16_t channel, uint16_t width_s0, uint16_t width_s1)
284 : {
285 0 : if (channel < _num_channels) {
286 0 : _pwm_values[channel] = width_s1; // range: 700usec ~ 2300usec
287 0 : rc_input_count++;
288 : }
289 0 : }
290 :
291 : /*
292 : process a RC input pulse of the given width
293 : */
294 0 : void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1)
295 : {
296 : #if 0
297 : // useful for debugging
298 : static FILE *rclog;
299 : if (rclog == nullptr) {
300 : rclog = fopen("/tmp/rcin.log", "w");
301 : }
302 : if (rclog) {
303 : fprintf(rclog, "%u %u\n", (unsigned)width_s0, (unsigned)width_s1);
304 : }
305 : #endif
306 : // treat as PPM-sum
307 0 : _process_ppmsum_pulse(width_s0 + width_s1);
308 :
309 : // treat as SBUS
310 0 : _process_sbus_pulse(width_s0, width_s1);
311 :
312 : // treat as DSM
313 0 : _process_dsm_pulse(width_s0, width_s1);
314 0 : }
315 :
316 : /*
317 : * Update channel values directly
318 : */
319 0 : void RCInput::_update_periods(uint16_t *periods, uint8_t len)
320 : {
321 0 : if (len > LINUX_RC_INPUT_NUM_CHANNELS) {
322 0 : len = LINUX_RC_INPUT_NUM_CHANNELS;
323 : }
324 0 : for (unsigned int i=0; i < len; i++) {
325 0 : _pwm_values[i] = periods[i];
326 : }
327 0 : _num_channels = len;
328 0 : rc_input_count++;
329 0 : }
|
