Line data Source code
1 : /*
2 : DSM decoder, based on src/modules/px4iofirmware/dsm.c from PX4Firmware
3 : modified for use in AP_HAL_* by Andrew Tridgell
4 : */
5 : /****************************************************************************
6 : *
7 : * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
8 : *
9 : * Redistribution and use in source and binary forms, with or without
10 : * modification, are permitted provided that the following conditions
11 : * are met:
12 : *
13 : * 1. Redistributions of source code must retain the above copyright
14 : * notice, this list of conditions and the following disclaimer.
15 : * 2. Redistributions in binary form must reproduce the above copyright
16 : * notice, this list of conditions and the following disclaimer in
17 : * the documentation and/or other materials provided with the
18 : * distribution.
19 : * 3. Neither the name PX4 nor the names of its contributors may be
20 : * used to endorse or promote products derived from this software
21 : * without specific prior written permission.
22 : *
23 : * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 : * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 : * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 : * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 : * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 : * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 : * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
30 : * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
31 : * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 : * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 : * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 : * POSSIBILITY OF SUCH DAMAGE.
35 : *
36 : ****************************************************************************/
37 :
38 : #include <stdint.h>
39 : #include <stdio.h>
40 :
41 : #include "dsm.h"
42 :
43 : #define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/
44 : #define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/
45 :
46 : static uint64_t dsm_last_frame_time; /**< Timestamp for start of last dsm frame */
47 : static unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 10=10 bit, 11=11 bit */
48 :
49 : //#define DEBUG
50 :
51 : #ifdef DEBUG
52 : # define debug(fmt, args...) printf(fmt "\n", ##args)
53 : #else
54 : # define debug(fmt, args...) do {} while(0)
55 : #endif
56 :
57 : /**
58 : * Attempt to decode a single channel raw channel datum
59 : *
60 : * The DSM* protocol doesn't provide any explicit framing,
61 : * so we detect dsm frame boundaries by the inter-dsm frame delay.
62 : *
63 : * The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
64 : * dsm frame transmission time is ~1.4ms.
65 : *
66 : * We expect to only be called when bytes arrive for processing,
67 : * and if an interval of more than 5ms passes between calls,
68 : * the first byte we read will be the first byte of a dsm frame.
69 : *
70 : * In the case where byte(s) are dropped from a dsm frame, this also
71 : * provides a degree of protection. Of course, it would be better
72 : * if we didn't drop bytes...
73 : *
74 : * Upon receiving a full dsm frame we attempt to decode it
75 : *
76 : * @param[in] raw 16 bit raw channel value from dsm frame
77 : * @param[in] shift position of channel number in raw data
78 : * @param[out] channel pointer to returned channel number
79 : * @param[out] value pointer to returned channel value
80 : * @return true=raw value successfully decoded
81 : */
82 : static bool
83 1624 : dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
84 : {
85 :
86 1624 : if (raw == 0xffff)
87 504 : return false;
88 :
89 1120 : *channel = (raw >> shift) & 0xf;
90 :
91 1120 : uint16_t data_mask = (1 << shift) - 1;
92 1120 : *value = raw & data_mask;
93 :
94 : //debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value);
95 :
96 1120 : return true;
97 : }
98 :
99 : /**
100 : * Attempt to guess if receiving 10 or 11 bit channel values
101 : *
102 : * @param[in] reset true=reset the 10/11 bit state to unknown
103 : */
104 : static void
105 128 : dsm_guess_format(bool reset, const uint8_t dsm_frame[16])
106 : {
107 : static uint32_t cs10;
108 : static uint32_t cs11;
109 : static unsigned samples;
110 :
111 : /* reset the 10/11 bit sniffed channel masks */
112 128 : if (reset) {
113 18 : cs10 = 0;
114 18 : cs11 = 0;
115 18 : samples = 0;
116 18 : dsm_channel_shift = 0;
117 18 : return;
118 : }
119 :
120 : /* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
121 880 : for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
122 :
123 770 : const uint8_t *dp = &dsm_frame[2 + (2 * i)];
124 770 : uint16_t raw = (dp[0] << 8) | dp[1];
125 : unsigned channel, value;
126 :
127 : /* if the channel decodes, remember the assigned number */
128 770 : if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31))
129 518 : cs10 |= (1 << channel);
130 :
131 770 : if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
132 518 : cs11 |= (1 << channel);
133 :
134 : /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
135 : }
136 :
137 : /* wait until we have seen plenty of frames - 5 should normally be enough */
138 110 : if (samples++ < 5)
139 92 : return;
140 :
141 : /*
142 : * Iterate the set of sensible sniffed channel sets and see whether
143 : * decoding in 10 or 11-bit mode has yielded anything we recognize.
144 : *
145 : * XXX Note that due to what seem to be bugs in the DSM2 high-resolution
146 : * stream, we may want to sniff for longer in some cases when we think we
147 : * are talking to a DSM2 receiver in high-resolution mode (so that we can
148 : * reject it, ideally).
149 : * See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion
150 : * of this issue.
151 : */
152 : static uint32_t masks[] = {
153 : 0x3f, /* 6 channels (DX6) */
154 : 0x7f, /* 7 channels (DX7) */
155 : 0xff, /* 8 channels (DX8) */
156 : 0x1ff, /* 9 channels (DX9, etc.) */
157 : 0x3ff, /* 10 channels (DX10) */
158 : 0x1fff, /* 13 channels (DX10t) */
159 : 0x3fff /* 18 channels (DX10) */
160 : };
161 18 : unsigned votes10 = 0;
162 18 : unsigned votes11 = 0;
163 :
164 144 : for (unsigned i = 0; i < sizeof(masks)/sizeof(masks[0]); i++) {
165 :
166 126 : if (cs10 == masks[i])
167 2 : votes10++;
168 :
169 126 : if (cs11 == masks[i])
170 10 : votes11++;
171 : }
172 :
173 18 : if ((votes11 == 1) && (votes10 == 0)) {
174 10 : dsm_channel_shift = 11;
175 : debug("DSM: 11-bit format");
176 10 : return;
177 : }
178 :
179 8 : if ((votes10 == 1) && (votes11 == 0)) {
180 2 : dsm_channel_shift = 10;
181 : debug("DSM: 10-bit format");
182 2 : return;
183 : }
184 :
185 : /* call ourselves to reset our state ... we have to try again */
186 : debug("DSM: format detect fail, 10: 0x%08x %u 11: 0x%08x %u", cs10, votes10, cs11, votes11);
187 6 : dsm_guess_format(true, dsm_frame);
188 : }
189 :
190 : /**
191 : * Decode the entire dsm frame (all contained channels)
192 : *
193 : */
194 : bool
195 122 : dsm_decode(uint64_t frame_time, const uint8_t dsm_frame[16], uint16_t *values, uint16_t *num_values, uint16_t max_values)
196 : {
197 : /*
198 : debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
199 : dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7],
200 : dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]);
201 : */
202 : /*
203 : * If we have lost signal for at least a second, reset the
204 : * format guessing heuristic.
205 : */
206 122 : if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0))
207 12 : dsm_guess_format(true, dsm_frame);
208 :
209 : /* we have received something we think is a dsm_frame */
210 122 : dsm_last_frame_time = frame_time;
211 :
212 : /* if we don't know the dsm_frame format, update the guessing state machine */
213 122 : if (dsm_channel_shift == 0) {
214 110 : dsm_guess_format(false, dsm_frame);
215 110 : return false;
216 : }
217 :
218 : /*
219 : * The encoding of the first two bytes is uncertain, so we're
220 : * going to ignore them for now.
221 : *
222 : * Each channel is a 16-bit unsigned value containing either a 10-
223 : * or 11-bit channel value and a 4-bit channel number, shifted
224 : * either 10 or 11 bits. The MSB may also be set to indicate the
225 : * second dsm_frame in variants of the protocol where more than
226 : * seven channels are being transmitted.
227 : */
228 :
229 96 : for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
230 :
231 84 : const uint8_t *dp = &dsm_frame[2 + (2 * i)];
232 84 : uint16_t raw = (dp[0] << 8) | dp[1];
233 : unsigned channel, value;
234 :
235 84 : if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value))
236 6 : continue;
237 :
238 : /* ignore channels out of range */
239 84 : if (channel >= max_values)
240 6 : continue;
241 :
242 : /* update the decoded channel count */
243 78 : if (channel >= *num_values)
244 78 : *num_values = channel + 1;
245 :
246 : /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */
247 78 : if (dsm_channel_shift == 10)
248 14 : value *= 2;
249 :
250 : /*
251 : * Spektrum scaling is special. There are these basic considerations
252 : *
253 : * * Midpoint is 1520 us
254 : * * 100% travel channels are +- 400 us
255 : *
256 : * We obey the original Spektrum scaling (so a default setup will scale from
257 : * 1100 - 1900 us), but we do not obey the weird 1520 us center point
258 : * and instead (correctly) center the center around 1500 us. This is in order
259 : * to get something useful without requiring the user to calibrate on a digital
260 : * link for no reason.
261 : */
262 :
263 : /* scaled integer for decent accuracy while staying efficient */
264 78 : value = ((((int)value - 1024) * 1000) / 1700) + 1500;
265 :
266 : /*
267 : * Store the decoded channel into the R/C input buffer, taking into
268 : * account the different ideas about channel assignement that we have.
269 : *
270 : * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw,
271 : * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw.
272 : */
273 78 : switch (channel) {
274 12 : case 0:
275 12 : channel = 2;
276 12 : break;
277 :
278 12 : case 1:
279 12 : channel = 0;
280 12 : break;
281 :
282 12 : case 2:
283 12 : channel = 1;
284 12 : break;
285 :
286 42 : default:
287 42 : break;
288 : }
289 :
290 78 : values[channel] = value;
291 : }
292 :
293 : /*
294 : * Spektrum likes to send junk in higher channel numbers to fill
295 : * their packets. We don't know about a 13 channel model in their TX
296 : * lines, so if we get a channel count of 13, we'll return 12 (the last
297 : * data index that is stable).
298 : */
299 12 : if (*num_values == 13)
300 0 : *num_values = 12;
301 :
302 : #if 0
303 : if (dsm_channel_shift == 11) {
304 : /* Set the 11-bit data indicator */
305 : *num_values |= 0x8000;
306 : }
307 : #endif
308 :
309 : /*
310 : * XXX Note that we may be in failsafe here; we need to work out how to detect that.
311 : */
312 12 : return true;
313 : }
314 :
315 : #if defined(TEST_MAIN_PROGRAM) || defined(TEST_HEX_STRING)
316 : static uint8_t dsm_partial_frame_count;
317 : static uint8_t dsm_frame[DSM_FRAME_SIZE];
318 : static enum DSM_DECODE_STATE {
319 : DSM_DECODE_STATE_DESYNC = 0,
320 : DSM_DECODE_STATE_SYNC
321 : } dsm_decode_state = DSM_DECODE_STATE_DESYNC;
322 : static uint64_t dsm_last_rx_time; /**< Timestamp when we last received data */
323 : static uint16_t dsm_chan_count;
324 : static uint16_t dsm_frame_drops;
325 :
326 : static bool
327 : dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
328 : uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels)
329 : {
330 :
331 : /* this is set by the decoding state machine and will default to false
332 : * once everything that was decodable has been decoded.
333 : */
334 : bool decode_ret = false;
335 :
336 : /* keep decoding until we have consumed the buffer */
337 : for (unsigned d = 0; d < len; d++) {
338 :
339 : /* overflow check */
340 : if (dsm_partial_frame_count == sizeof(dsm_frame) / sizeof(dsm_frame[0])) {
341 : dsm_partial_frame_count = 0;
342 : dsm_decode_state = DSM_DECODE_STATE_DESYNC;
343 : #ifdef DSM_DEBUG
344 : printf("DSM: RESET (BUF LIM)\n");
345 : #endif
346 : }
347 :
348 : if (dsm_partial_frame_count == DSM_FRAME_SIZE) {
349 : dsm_partial_frame_count = 0;
350 : dsm_decode_state = DSM_DECODE_STATE_DESYNC;
351 : #ifdef DSM_DEBUG
352 : printf("DSM: RESET (PACKET LIM)\n");
353 : #endif
354 : }
355 :
356 : #ifdef DSM_DEBUG
357 : #if 1
358 : printf("dsm state: %s%s, count: %d, val: %02x\n",
359 : (dsm_decode_state == DSM_DECODE_STATE_DESYNC) ? "DSM_DECODE_STATE_DESYNC" : "",
360 : (dsm_decode_state == DSM_DECODE_STATE_SYNC) ? "DSM_DECODE_STATE_SYNC" : "",
361 : dsm_partial_frame_count,
362 : (unsigned)frame[d]);
363 : #endif
364 : #endif
365 :
366 : switch (dsm_decode_state) {
367 : case DSM_DECODE_STATE_DESYNC:
368 :
369 : /* we are de-synced and only interested in the frame marker */
370 : if ((now - dsm_last_rx_time) > 5000) {
371 : printf("resync %u\n", dsm_partial_frame_count);
372 : dsm_decode_state = DSM_DECODE_STATE_SYNC;
373 : dsm_partial_frame_count = 0;
374 : dsm_chan_count = 0;
375 : dsm_frame[dsm_partial_frame_count++] = frame[d];
376 : }
377 :
378 : break;
379 :
380 : case DSM_DECODE_STATE_SYNC: {
381 : dsm_frame[dsm_partial_frame_count++] = frame[d];
382 :
383 : /* decode whatever we got and expect */
384 : if (dsm_partial_frame_count < DSM_FRAME_SIZE) {
385 : break;
386 : }
387 :
388 : /*
389 : * Great, it looks like we might have a frame. Go ahead and
390 : * decode it.
391 : */
392 : decode_ret = dsm_decode(now, dsm_frame, values, &dsm_chan_count, max_channels);
393 :
394 : #if 1
395 : printf("%u %u: ", ((unsigned)(now/1000)) % 1000000, len);
396 : for (uint8_t i=0; i<DSM_FRAME_SIZE; i++) {
397 : printf("%02x ", (unsigned)dsm_frame[i]);
398 : }
399 : printf("\n");
400 : #endif
401 :
402 :
403 : /* we consumed the partial frame, reset */
404 : dsm_partial_frame_count = 0;
405 :
406 : /* if decoding failed, set proto to desync */
407 : if (decode_ret == false) {
408 : dsm_decode_state = DSM_DECODE_STATE_DESYNC;
409 : dsm_frame_drops++;
410 : printf("drop ");
411 : for (uint8_t i=0; i<DSM_FRAME_SIZE; i++) {
412 : printf("%02x ", (unsigned)dsm_frame[i]);
413 : }
414 : printf("\n");
415 : }
416 : }
417 : break;
418 :
419 : default:
420 : #ifdef DSM_DEBUG
421 : printf("UNKNOWN PROTO STATE");
422 : #endif
423 : decode_ret = false;
424 : }
425 :
426 :
427 : }
428 :
429 : if (frame_drops) {
430 : *frame_drops = dsm_frame_drops;
431 : }
432 :
433 : if (decode_ret) {
434 : *num_values = dsm_chan_count;
435 : }
436 :
437 : dsm_last_rx_time = now;
438 :
439 : /* return false as default */
440 : return decode_ret;
441 : }
442 : #endif
443 :
444 :
445 : #ifdef TEST_MAIN_PROGRAM
446 : /*
447 : test harness for use under Linux with USB serial adapter
448 : */
449 : #include <sys/types.h>
450 : #include <sys/stat.h>
451 : #include <fcntl.h>
452 : #include <time.h>
453 : #include <unistd.h>
454 : #include <stdlib.h>
455 : #include <termios.h>
456 : #include <string.h>
457 :
458 : static uint64_t micros64(void)
459 : {
460 : struct timespec ts;
461 : clock_gettime(CLOCK_MONOTONIC, &ts);
462 : return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)));
463 : }
464 :
465 : int main(int argc, const char *argv[])
466 : {
467 : int fd = open(argv[1], O_RDONLY|O_CLOEXEC);
468 : if (fd == -1) {
469 : perror(argv[1]);
470 : exit(1);
471 : }
472 :
473 : struct termios options;
474 :
475 : tcgetattr(fd, &options);
476 :
477 : cfsetispeed(&options, B115200);
478 : cfsetospeed(&options, B115200);
479 :
480 : options.c_cflag &= ~(PARENB|CSTOPB|CSIZE);
481 : options.c_cflag |= CS8;
482 :
483 : options.c_lflag &= ~(ICANON|ECHO|ECHOE|ISIG);
484 : options.c_iflag &= ~(IXON|IXOFF|IXANY);
485 : options.c_oflag &= ~OPOST;
486 :
487 : if (tcsetattr(fd, TCSANOW, &options) != 0) {
488 : perror("tcsetattr");
489 : exit(1);
490 : }
491 : tcflush(fd, TCIOFLUSH);
492 :
493 : uint16_t values[18];
494 : memset(values, 0, sizeof(values));
495 :
496 : while (true) {
497 : uint8_t b[16];
498 : uint16_t num_values = 0;
499 : fd_set fds;
500 : struct timeval tv;
501 :
502 : FD_ZERO(&fds);
503 : FD_SET(fd, &fds);
504 :
505 : tv.tv_sec = 1;
506 : tv.tv_usec = 0;
507 :
508 : // check if any bytes are available
509 : if (select(fd+1, &fds, nullptr, nullptr, &tv) != 1) {
510 : break;
511 : }
512 :
513 : ssize_t nread;
514 : if ((nread = read(fd, b, sizeof(b))) < 1) {
515 : break;
516 : }
517 :
518 : bool dsm_11_bit;
519 : unsigned frame_drops;
520 :
521 : if (dsm_parse(micros64(), b, nread, values, &num_values, &dsm_11_bit, &frame_drops, 18)) {
522 : #if 1
523 : printf("%u: ", num_values);
524 : for (uint8_t i=0; i<num_values; i++) {
525 : printf("%u:%4u ", i+1, values[i]);
526 : }
527 : printf("\n");
528 : #endif
529 : }
530 : }
531 : }
532 : #elif defined(TEST_HEX_STRING)
533 : /*
534 : test harness providing hex string to decode
535 : */
536 : #include <string.h>
537 :
538 : int main(int argc, const char *argv[])
539 : {
540 : uint8_t b[16];
541 : uint64_t t = 0;
542 :
543 : for (uint8_t i=1; i<argc; i++) {
544 : unsigned v;
545 : if (sscanf(argv[i], "%02x", &v) != 1 || v > 255) {
546 : printf("Bad hex value at %u : %s\n", (unsigned)i, argv[i]);
547 : return 1;
548 : }
549 : b[i-1] = v;
550 : }
551 : uint16_t values[18];
552 : memset(values, 0, sizeof(values));
553 :
554 : while (true) {
555 : uint16_t num_values = 0;
556 : bool dsm_11_bit;
557 : unsigned frame_drops;
558 :
559 : t += 11000;
560 :
561 : if (dsm_parse(t, b, sizeof(b), values, &num_values, &dsm_11_bit, &frame_drops, 18)) {
562 : #if 1
563 : printf("%u: ", num_values);
564 : for (uint8_t i=0; i<num_values; i++) {
565 : printf("%u:%4u ", i+1, values[i]);
566 : }
567 : printf("\n");
568 : #endif
569 : }
570 : }
571 : }
572 : #endif
|
