Line data Source code
1 : /*
2 : * This file is free software: you can redistribute it and/or modify it
3 : * under the terms of the GNU General Public License as published by the
4 : * Free Software Foundation, either version 3 of the License, or
5 : * (at your option) any later version.
6 : *
7 : * This file is distributed in the hope that it will be useful, but
8 : * WITHOUT ANY WARRANTY; without even the implied warranty of
9 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
10 : * See the GNU General Public License for more details.
11 : *
12 : * You should have received a copy of the GNU General Public License along
13 : * with this program. If not, see <http://www.gnu.org/licenses/>.
14 : *
15 : * Code by Andrew Tridgell and Siddharth Bharat Purohit
16 : */
17 : /*
18 : with thanks to PX4 dsm.c for DSM decoding approach
19 : */
20 :
21 : #include "AP_RCProtocol_config.h"
22 :
23 : #if AP_RCPROTOCOL_DSM_ENABLED
24 :
25 : #include "AP_RCProtocol_DSM.h"
26 : #include <AP_VideoTX/AP_VideoTX_config.h>
27 :
28 : extern const AP_HAL::HAL& hal;
29 :
30 : // #define DSM_DEBUG
31 : #ifdef DSM_DEBUG
32 : #include <stdio.h>
33 : # define debug(fmt, args...) printf(fmt "\n", ##args)
34 : #else
35 : # define debug(fmt, args...) do {} while(0)
36 : #endif
37 :
38 :
39 : #define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/
40 : #define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/
41 : #define SPEKTRUM_VTX_CONTROL_FRAME_MASK 0xf000f000
42 : #define SPEKTRUM_VTX_CONTROL_FRAME 0xe000e000
43 :
44 : #define SPEKTRUM_VTX_BAND_MASK 0x00e00000
45 : #define SPEKTRUM_VTX_CHANNEL_MASK 0x000f0000
46 : #define SPEKTRUM_VTX_PIT_MODE_MASK 0x00000010
47 : #define SPEKTRUM_VTX_POWER_MASK 0x00000007
48 :
49 : #define SPEKTRUM_VTX_BAND_SHIFT 21
50 : #define SPEKTRUM_VTX_CHANNEL_SHIFT 16
51 : #define SPEKTRUM_VTX_PIT_MODE_SHIFT 4
52 : #define SPEKTRUM_VTX_POWER_SHIFT 0
53 :
54 0 : void AP_RCProtocol_DSM::process_pulse(uint32_t width_s0, uint32_t width_s1)
55 : {
56 : uint8_t b;
57 0 : if (ss.process_pulse(width_s0, width_s1, b)) {
58 0 : _process_byte(ss.get_byte_timestamp_us()/1000U, b);
59 : }
60 0 : }
61 :
62 : /**
63 : * Attempt to decode a single channel raw channel datum
64 : *
65 : * The DSM* protocol doesn't provide any explicit framing,
66 : * so we detect dsm frame boundaries by the inter-dsm frame delay.
67 : *
68 : * The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
69 : * dsm frame transmission time is ~1.4ms.
70 : *
71 : * We expect to only be called when bytes arrive for processing,
72 : * and if an interval of more than 5ms passes between calls,
73 : * the first byte we read will be the first byte of a dsm frame.
74 : *
75 : * In the case where byte(s) are dropped from a dsm frame, this also
76 : * provides a degree of protection. Of course, it would be better
77 : * if we didn't drop bytes...
78 : *
79 : * Upon receiving a full dsm frame we attempt to decode it
80 : *
81 : * @param[in] raw 16 bit raw channel value from dsm frame
82 : * @param[in] shift position of channel number in raw data
83 : * @param[out] channel pointer to returned channel number
84 : * @param[out] value pointer to returned channel value
85 : * @return true=raw value successfully decoded
86 : */
87 0 : bool AP_RCProtocol_DSM::dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
88 : {
89 :
90 0 : if (raw == 0xffff) {
91 0 : return false;
92 : }
93 :
94 0 : *channel = (raw >> shift) & 0xf;
95 :
96 0 : uint16_t data_mask = (1 << shift) - 1;
97 0 : *value = raw & data_mask;
98 :
99 : //debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value);
100 :
101 0 : return true;
102 : }
103 :
104 : /**
105 : * Attempt to guess if receiving 10 or 11 bit channel values
106 : *
107 : * @param[in] reset true=reset the 10/11 bit state to unknown
108 : */
109 0 : void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16], unsigned frame_channels)
110 : {
111 : /* reset the 10/11 bit sniffed channel masks */
112 0 : if (reset) {
113 0 : cs10 = 0;
114 0 : cs11 = 0;
115 0 : samples = 0;
116 0 : channel_shift = 0;
117 0 : return;
118 : }
119 :
120 : /* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
121 0 : for (unsigned i = 0; i < frame_channels; i++) {
122 :
123 0 : const uint8_t *dp = &dsm_frame[2 + (2 * i)];
124 0 : uint16_t raw = (dp[0] << 8) | dp[1];
125 : unsigned channel, value;
126 :
127 : /* if the channel decodes, remember the assigned number */
128 0 : if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 16)) {
129 0 : cs10 |= (1 << channel);
130 : }
131 :
132 0 : if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 16)) {
133 0 : cs11 |= (1 << channel);
134 : }
135 :
136 : /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
137 : }
138 :
139 : /* wait until we have seen plenty of frames - 5 should normally be enough */
140 0 : if (samples++ < 5) {
141 0 : return;
142 : }
143 :
144 : /*
145 : * Iterate the set of sensible sniffed channel sets and see whether
146 : * decoding in 10 or 11-bit mode has yielded anything we recognize.
147 : *
148 : * XXX Note that due to what seem to be bugs in the DSM2 high-resolution
149 : * stream, we may want to sniff for longer in some cases when we think we
150 : * are talking to a DSM2 receiver in high-resolution mode (so that we can
151 : * reject it, ideally).
152 : * See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion
153 : * of this issue.
154 : */
155 : static const uint32_t masks[] = {
156 : 0x1f, /* 5 channels (DX6 VTX frame) */
157 : 0x3f, /* 6 channels (DX6) */
158 : 0x7f, /* 7 channels (DX7) */
159 : 0xff, /* 8 channels (DX8) */
160 : 0x1ff, /* 9 channels (DX9, etc.) */
161 : 0x3ff, /* 10 channels (DX10) */
162 : 0x7ff, /* 11 channels */
163 : 0xfff, /* 12 channels */
164 : 0x1fff, /* 13 channels */
165 : 0x3fff, /* 14 channels */
166 : 0x7fff, /* 15 channels */
167 : 0xffff /* 16 channels */ // the remote receiver protocol supports max 16 channels
168 : };
169 0 : unsigned votes10 = 0;
170 0 : unsigned votes11 = 0;
171 :
172 0 : for (unsigned i = 0; i < sizeof(masks)/sizeof(masks[0]); i++) {
173 :
174 0 : if (cs10 == masks[i]) {
175 0 : votes10++;
176 : }
177 :
178 0 : if (cs11 == masks[i]) {
179 0 : votes11++;
180 : }
181 : }
182 :
183 0 : if ((votes11 == 1) && (votes10 == 0)) {
184 0 : channel_shift = 11;
185 : debug("DSM: 11-bit format");
186 0 : return;
187 : }
188 :
189 0 : if ((votes10 == 1) && (votes11 == 0)) {
190 0 : channel_shift = 10;
191 : debug("DSM: 10-bit format");
192 0 : return;
193 : }
194 :
195 : /* call ourselves to reset our state ... we have to try again */
196 : debug("DSM: format detect fail, 10: 0x%08x %u 11: 0x%08x %u", cs10, votes10, cs11, votes11);
197 0 : dsm_guess_format(true, dsm_frame, frame_channels);
198 : }
199 :
200 : /**
201 : * Decode the entire dsm frame (all contained channels)
202 : *
203 : */
204 0 : bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time_ms, const uint8_t dsm_frame[16],
205 : uint16_t *values, uint16_t *num_values, uint16_t max_values)
206 : {
207 : /*
208 : * If we have lost signal for at least 200ms, reset the
209 : * format guessing heuristic.
210 : */
211 0 : if (((frame_time_ms - last_frame_time_ms) > 200U) && (channel_shift != 0)) {
212 0 : dsm_guess_format(true, dsm_frame, DSM_FRAME_CHANNELS);
213 : }
214 :
215 : /* we have received something we think is a dsm_frame */
216 0 : last_frame_time_ms = frame_time_ms;
217 :
218 : // Get the VTX control bytes in a frame
219 0 : uint32_t vtxControl = ((dsm_frame[DSM_FRAME_SIZE-4] << 24)
220 0 : | (dsm_frame[DSM_FRAME_SIZE-3] << 16)
221 0 : | (dsm_frame[DSM_FRAME_SIZE-2] << 8)
222 0 : | (dsm_frame[DSM_FRAME_SIZE-1] << 0));
223 0 : const bool haveVtxControl =
224 0 : ((vtxControl & SPEKTRUM_VTX_CONTROL_FRAME_MASK) == SPEKTRUM_VTX_CONTROL_FRAME &&
225 0 : (dsm_frame[2] & 0x80) == 0);
226 :
227 0 : unsigned frame_channels = DSM_FRAME_CHANNELS;
228 : // Handle VTX control frame.
229 0 : if (haveVtxControl) {
230 0 : frame_channels = DSM_FRAME_CHANNELS - 2;
231 : }
232 :
233 : /* if we don't know the dsm_frame format, update the guessing state machine */
234 0 : if (channel_shift == 0) {
235 0 : dsm_guess_format(false, dsm_frame, frame_channels);
236 0 : return false;
237 : }
238 :
239 : // Handle VTX control frame.
240 : #if AP_VIDEOTX_ENABLED
241 0 : if (haveVtxControl) {
242 0 : configure_vtx(
243 0 : (vtxControl & SPEKTRUM_VTX_BAND_MASK) >> SPEKTRUM_VTX_BAND_SHIFT,
244 0 : (vtxControl & SPEKTRUM_VTX_CHANNEL_MASK) >> SPEKTRUM_VTX_CHANNEL_SHIFT,
245 : (vtxControl & SPEKTRUM_VTX_POWER_MASK) >> SPEKTRUM_VTX_POWER_SHIFT,
246 0 : (vtxControl & SPEKTRUM_VTX_PIT_MODE_MASK) >> SPEKTRUM_VTX_PIT_MODE_SHIFT);
247 : }
248 : #endif
249 :
250 : /*
251 : * The encoding of the first two bytes is uncertain, so we're
252 : * going to ignore them for now.
253 : *
254 : * Each channel is a 16-bit unsigned value containing either a 10-
255 : * or 11-bit channel value and a 4-bit channel number, shifted
256 : * either 10 or 11 bits. The MSB may also be set to indicate the
257 : * second dsm_frame in variants of the protocol where more than
258 : * seven channels are being transmitted.
259 : */
260 :
261 0 : for (unsigned i = 0; i < frame_channels; i++) {
262 :
263 0 : const uint8_t *dp = &dsm_frame[2 + (2 * i)];
264 0 : uint16_t raw = (dp[0] << 8) | dp[1];
265 : unsigned channel, value;
266 :
267 0 : if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) {
268 0 : continue;
269 : }
270 :
271 : /* ignore channels out of range */
272 0 : if (channel >= max_values) {
273 0 : continue;
274 : }
275 :
276 : /* update the decoded channel count */
277 0 : if (channel >= *num_values) {
278 0 : *num_values = channel + 1;
279 : }
280 :
281 : /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */
282 0 : if (channel_shift == 10) {
283 0 : value *= 2;
284 : }
285 :
286 : /*
287 : * Spektrum scaling is special. There are these basic considerations
288 : *
289 : * * Midpoint is 1520 us
290 : * * 100% travel channels are +- 400 us
291 : *
292 : * We obey the original Spektrum scaling (so a default setup will scale from
293 : * 1100 - 1900 us), but we do not obey the weird 1520 us center point
294 : * and instead (correctly) center the center around 1500 us. This is in order
295 : * to get something useful without requiring the user to calibrate on a digital
296 : * link for no reason.
297 : */
298 :
299 : /* scaled integer for decent accuracy while staying efficient */
300 0 : value = ((((int)value - 1024) * 1000) / 1700) + 1500;
301 :
302 : /*
303 : * Store the decoded channel into the R/C input buffer, taking into
304 : * account the different ideas about channel assignement that we have.
305 : *
306 : * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw,
307 : * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw.
308 : */
309 0 : switch (channel) {
310 0 : case 0:
311 0 : channel = 2;
312 0 : break;
313 :
314 0 : case 1:
315 0 : channel = 0;
316 0 : break;
317 :
318 0 : case 2:
319 0 : channel = 1;
320 0 : break;
321 :
322 0 : default:
323 0 : break;
324 : }
325 :
326 0 : values[channel] = value;
327 : }
328 :
329 : /*
330 : * Spektrum likes to send junk in higher channel numbers to fill
331 : * their packets. We don't know about a 13 channel model in their TX
332 : * lines, so if we get a channel count of 13, we'll return 12 (the last
333 : * data index that is stable).
334 : */
335 0 : if (*num_values == 13) {
336 0 : *num_values = 12;
337 : }
338 :
339 : #if 0
340 : if (channel_shift == 11) {
341 : /* Set the 11-bit data indicator */
342 : *num_values |= 0x8000;
343 : }
344 : #endif
345 :
346 : /*
347 : * XXX Note that we may be in failsafe here; we need to work out how to detect that.
348 : */
349 0 : return true;
350 : }
351 :
352 :
353 : /*
354 : start bind on DSM satellites
355 : */
356 0 : void AP_RCProtocol_DSM::start_bind(void)
357 : {
358 : #if defined(HAL_GPIO_SPEKTRUM_RC) && HAL_GPIO_SPEKTRUM_RC
359 : if (!hal.gpio->get_mode(HAL_GPIO_SPEKTRUM_RC, bind_mode_saved)) {
360 : return;
361 : }
362 : #endif
363 0 : bind_state = BIND_STATE1;
364 0 : }
365 :
366 :
367 : /*
368 : update function used for bind state machine
369 : */
370 36177494 : void AP_RCProtocol_DSM::update(void)
371 : {
372 : #if defined(HAL_GPIO_SPEKTRUM_PWR) && defined(HAL_GPIO_SPEKTRUM_RC)
373 : switch (bind_state) {
374 : case BIND_STATE_NONE:
375 : break;
376 :
377 : case BIND_STATE1:
378 : hal.gpio->write(HAL_GPIO_SPEKTRUM_PWR, !HAL_SPEKTRUM_PWR_ENABLED);
379 : hal.gpio->pinMode(HAL_GPIO_SPEKTRUM_RC, 1);
380 : hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 1);
381 : bind_last_ms = AP_HAL::millis();
382 : bind_state = BIND_STATE2;
383 : break;
384 :
385 : case BIND_STATE2: {
386 : uint32_t now = AP_HAL::millis();
387 : if (now - bind_last_ms > 500) {
388 : hal.gpio->write(HAL_GPIO_SPEKTRUM_PWR, HAL_SPEKTRUM_PWR_ENABLED);
389 : bind_last_ms = now;
390 : bind_state = BIND_STATE3;
391 : }
392 : break;
393 : }
394 :
395 : case BIND_STATE3: {
396 : uint32_t now = AP_HAL::millis();
397 : if (now - bind_last_ms > 72) {
398 : // 9 pulses works with all satellite receivers, and supports the highest
399 : // available protocol
400 : const uint8_t num_pulses = 9;
401 : for (uint8_t i=0; i<num_pulses; i++) {
402 : hal.scheduler->delay_microseconds(120);
403 : hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 0);
404 : hal.scheduler->delay_microseconds(120);
405 : hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 1);
406 : }
407 : bind_last_ms = now;
408 : bind_state = BIND_STATE4;
409 : }
410 : break;
411 : }
412 :
413 : case BIND_STATE4: {
414 : uint32_t now = AP_HAL::millis();
415 : if (now - bind_last_ms > 50) {
416 : hal.gpio->pinMode(HAL_GPIO_SPEKTRUM_RC, 0);
417 : bind_state = BIND_STATE_NONE;
418 : hal.gpio->set_mode(HAL_GPIO_SPEKTRUM_RC, bind_mode_saved);
419 : }
420 : break;
421 : }
422 : }
423 : #endif
424 36177494 : }
425 :
426 : /*
427 : parse one DSM byte, maintaining decoder state
428 : */
429 0 : bool AP_RCProtocol_DSM::dsm_parse_byte(uint32_t frame_time_ms, uint8_t b, uint16_t *values,
430 : uint16_t *num_values, uint16_t max_channels)
431 : {
432 : /* this is set by the decoding state machine and will default to false
433 : * once everything that was decodable has been decoded.
434 : */
435 0 : bool decode_ret = false;
436 :
437 : /* overflow check */
438 0 : if (byte_input.ofs == sizeof(byte_input.buf) / sizeof(byte_input.buf[0])) {
439 0 : byte_input.ofs = 0;
440 0 : dsm_decode_state = DSM_DECODE_STATE_DESYNC;
441 : debug("DSM: RESET (BUF LIM)\n");
442 0 : reset_rc_frame_count();
443 : }
444 :
445 0 : if (byte_input.ofs == DSM_FRAME_SIZE) {
446 0 : byte_input.ofs = 0;
447 0 : dsm_decode_state = DSM_DECODE_STATE_DESYNC;
448 : debug("DSM: RESET (PACKET LIM)\n");
449 0 : reset_rc_frame_count();
450 : }
451 :
452 : #ifdef DSM_DEBUG
453 : debug("dsm state: %s%s, count: %d, val: %02x\n",
454 : (dsm_decode_state == DSM_DECODE_STATE_DESYNC) ? "DSM_DECODE_STATE_DESYNC" : "",
455 : (dsm_decode_state == DSM_DECODE_STATE_SYNC) ? "DSM_DECODE_STATE_SYNC" : "",
456 : byte_input.ofs,
457 : (unsigned)b);
458 : #endif
459 :
460 0 : switch (dsm_decode_state) {
461 0 : case DSM_DECODE_STATE_DESYNC:
462 :
463 : /* we are de-synced and only interested in the frame marker */
464 0 : if ((frame_time_ms - last_rx_time_ms) >= 5) {
465 0 : dsm_decode_state = DSM_DECODE_STATE_SYNC;
466 0 : byte_input.ofs = 0;
467 0 : byte_input.buf[byte_input.ofs++] = b;
468 : }
469 0 : break;
470 :
471 0 : case DSM_DECODE_STATE_SYNC: {
472 0 : if ((frame_time_ms - last_rx_time_ms) >= 5 && byte_input.ofs > 0) {
473 0 : byte_input.ofs = 0;
474 0 : dsm_decode_state = DSM_DECODE_STATE_DESYNC;
475 0 : break;
476 : }
477 0 : byte_input.buf[byte_input.ofs++] = b;
478 :
479 : /* decode whatever we got and expect */
480 0 : if (byte_input.ofs < DSM_FRAME_SIZE) {
481 0 : break;
482 : }
483 :
484 : /*
485 : * Great, it looks like we might have a frame. Go ahead and
486 : * decode it.
487 : */
488 0 : log_data(AP_RCProtocol::DSM, frame_time_ms * 1000, byte_input.buf, byte_input.ofs);
489 :
490 0 : decode_ret = dsm_decode(frame_time_ms, byte_input.buf, values, &chan_count, max_channels);
491 :
492 : /* we consumed the partial frame, reset */
493 0 : byte_input.ofs = 0;
494 :
495 : /* if decoding failed, set proto to desync */
496 0 : if (decode_ret == false) {
497 0 : dsm_decode_state = DSM_DECODE_STATE_DESYNC;
498 0 : reset_rc_frame_count();
499 : }
500 0 : break;
501 : }
502 :
503 0 : default:
504 : debug("UNKNOWN PROTO STATE");
505 0 : decode_ret = false;
506 : }
507 :
508 :
509 0 : if (decode_ret) {
510 0 : *num_values = chan_count;
511 : }
512 :
513 0 : last_rx_time_ms = frame_time_ms;
514 :
515 : /* return false as default */
516 0 : return decode_ret;
517 : }
518 :
519 : // support byte input
520 0 : void AP_RCProtocol_DSM::_process_byte(uint32_t timestamp_ms, uint8_t b)
521 : {
522 : uint16_t v[AP_DSM_MAX_CHANNELS];
523 : uint16_t nchan;
524 0 : memcpy(v, last_values, sizeof(v));
525 0 : if (dsm_parse_byte(timestamp_ms, b, v, &nchan, AP_DSM_MAX_CHANNELS)) {
526 0 : memcpy(last_values, v, sizeof(v));
527 0 : if (nchan >= MIN_RCIN_CHANNELS) {
528 0 : add_input(nchan, last_values, false);
529 : }
530 : }
531 0 : }
532 :
533 : // support byte input
534 0 : void AP_RCProtocol_DSM::process_byte(uint8_t b, uint32_t baudrate)
535 : {
536 0 : if (baudrate != 115200) {
537 0 : return;
538 : }
539 0 : _process_byte(AP_HAL::millis(), b);
540 : }
541 :
542 : #endif // AP_RCPROTOCOL_DSM_ENABLED
|
