PX4 Firmware
PX4 Autopilot Software http://px4.io
sumd.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2015 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /*
35  * @file sumd.h
36  *
37  * RC protocol definition for Graupner HoTT transmitter (SUMD/SUMH Protocol)
38  *
39  * @author Marco Bauer <marco@wtns.de>
40  */
41 
42 #include <stdbool.h>
43 #include <stdio.h>
44 #include "sumd.h"
45 #include "common_rc.h"
46 
56 };
57 
58 /*
59 const char *decode_states[] = {"UNSYNCED",
60  "GOT_HEADER",
61  "GOT_STATE",
62  "GOT_LEN",
63  "GOT_DATA",
64  "GOT_CRC",
65  "GOT_CRC16_BYTE_1",
66  "GOT_CRC16_BYTE_2"
67  };
68 */
69 
70 uint8_t _crc8 = 0x00;
71 uint16_t _crc16 = 0x0000;
72 bool _sumd = true;
73 bool _crcOK = false;
74 bool _debug = false;
75 
76 
77 /* define range mapping here, -+100% -> 1000..2000 */
78 #define SUMD_RANGE_MIN 0.0f
79 #define SUMD_RANGE_MAX 4096.0f
80 
81 #define SUMD_TARGET_MIN 1000.0f
82 #define SUMD_TARGET_MAX 2000.0f
83 
84 /* pre-calculate the floating point stuff as far as possible at compile time */
85 #define SUMD_SCALE_FACTOR ((SUMD_TARGET_MAX - SUMD_TARGET_MIN) / (SUMD_RANGE_MAX - SUMD_RANGE_MIN))
86 #define SUMD_SCALE_OFFSET (int)(SUMD_TARGET_MIN - (SUMD_SCALE_FACTOR * SUMD_RANGE_MIN + 0.5f))
87 
89 static uint8_t _rxlen;
90 
92 
93 uint16_t sumd_crc16(uint16_t crc, uint8_t value)
94 {
95  int i;
96  crc ^= (uint16_t)value << 8;
97 
98  for (i = 0; i < 8; i++) {
99  crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1);
100  }
101 
102  return crc;
103 }
104 
105 uint8_t sumd_crc8(uint8_t crc, uint8_t value)
106 {
107  crc += value;
108  return crc;
109 }
110 
111 int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels,
112  uint16_t max_chan_count, bool *failsafe)
113 {
114 
115  int ret = 1;
116 
117  switch (_decode_state) {
119  if (_debug) {
120  printf(" SUMD_DECODE_STATE_UNSYNCED \n") ;
121  }
122 
123  if (byte == SUMD_HEADER_ID) {
124  _rxpacket.header = byte;
125  _sumd = true;
126  _rxlen = 0;
127  _crc16 = 0x0000;
128  _crc8 = 0x00;
129  _crcOK = false;
130  _crc16 = sumd_crc16(_crc16, byte);
131  _crc8 = sumd_crc8(_crc8, byte);
133 
134  if (_debug) {
135  printf(" SUMD_DECODE_STATE_GOT_HEADER: %x \n", byte) ;
136  }
137 
138  } else {
139  ret = 3;
140  }
141 
142  break;
143 
145  if (byte == SUMD_ID_SUMD || byte == SUMD_ID_FAILSAFE || byte == SUMD_ID_SUMH) {
146  _rxpacket.status = byte;
147 
148  if (byte == SUMD_ID_SUMH) {
149  _sumd = false;
150  }
151 
152  if (_sumd) {
153  _crc16 = sumd_crc16(_crc16, byte);
154 
155  } else {
156  _crc8 = sumd_crc8(_crc8, byte);
157  }
158 
160 
161  if (_debug) {
162  printf(" SUMD_DECODE_STATE_GOT_STATE: %x \n", byte) ;
163  }
164 
165  } else {
167  }
168 
169  break;
170 
172  if (byte >= 2 && byte <= SUMD_MAX_CHANNELS) {
173  _rxpacket.length = byte;
174 
175  if (_sumd) {
176  _crc16 = sumd_crc16(_crc16, byte);
177 
178  } else {
179  _crc8 = sumd_crc8(_crc8, byte);
180  }
181 
182  _rxlen++;
184 
185  if (_debug) {
186  printf(" SUMD_DECODE_STATE_GOT_LEN: %x (%d) \n", byte, byte) ;
187  }
188 
189  } else {
191  }
192 
193  break;
194 
196  _rxpacket.sumd_data[_rxlen] = byte;
197 
198  if (_sumd) {
199  _crc16 = sumd_crc16(_crc16, byte);
200 
201  } else {
202  _crc8 = sumd_crc8(_crc8, byte);
203  }
204 
205  _rxlen++;
206 
207  if (_rxlen <= ((_rxpacket.length * 2))) {
208  if (_debug) {
209  printf(" SUMD_DECODE_STATE_GOT_DATA[%d]: %x\n", _rxlen - 2, byte) ;
210  }
211 
212  } else {
214 
215  if (_debug) {
216  printf(" SUMD_DECODE_STATE_GOT_DATA -- finish --\n") ;
217  }
218 
219  }
220 
221  break;
222 
224  _rxpacket.crc16_high = byte;
225 
226  if (_debug) {
227  printf(" SUMD_DECODE_STATE_GOT_CRC16[1]: %x [%x]\n", byte, ((_crc16 >> 8) & 0xff)) ;
228  }
229 
230  if (_sumd) {
232 
233  } else {
235  }
236 
237  break;
238 
240  _rxpacket.crc16_low = byte;
241 
242  if (_debug) {
243  printf(" SUMD_DECODE_STATE_GOT_CRC16[2]: %x [%x]\n", byte, (_crc16 & 0xff)) ;
244  }
245 
247 
248  break;
249 
251  _rxpacket.telemetry = byte;
252 
253  if (_debug) {
254  printf(" SUMD_DECODE_STATE_GOT_SUMH_TELEMETRY: %x\n", byte) ;
255  }
256 
258 
259  break;
260 
262  if (_sumd) {
263  _rxpacket.crc16_low = byte;
264 
265  if (_debug) {
266  printf(" SUMD_DECODE_STATE_GOT_CRC[2]: %x [%x]\n\n", byte, (_crc16 & 0xff)) ;
267  }
268 
269  if (_crc16 == (uint16_t)(_rxpacket.crc16_high << 8) + _rxpacket.crc16_low) {
270  _crcOK = true;
271  }
272 
273  } else {
274  _rxpacket.crc8 = byte;
275 
276  if (_debug) {
277  printf(" SUMD_DECODE_STATE_GOT_CRC8_SUMH: %x [%x]\n\n", byte, _crc8) ;
278  }
279 
280  if (_crc8 == _rxpacket.crc8) {
281  _crcOK = true;
282  }
283  }
284 
285  if (_crcOK) {
286  if (_debug) {
287  printf(" CRC - OK \n") ;
288  }
289 
290  if (_sumd) {
291  if (_debug) {
292  printf(" Got valid SUMD Packet\n") ;
293  }
294 
295  } else {
296  if (_debug) {
297  printf(" Got valid SUMH Packet\n") ;
298  }
299 
300  }
301 
302  if (_debug) {
303  printf(" RXLEN: %d [Chans: %d] \n\n", _rxlen - 1, (_rxlen - 1) / 2) ;
304  }
305 
306  ret = 0;
307  unsigned i;
308  uint8_t _cnt = *rx_count + 1;
309  *rx_count = _cnt;
310 
311  *rssi = 100;
312 
313  /* failsafe flag */
314  *failsafe = (_rxpacket.status == SUMD_ID_FAILSAFE);
315 
316  /* received Channels */
317  if ((uint16_t)_rxpacket.length > max_chan_count) {
318  _rxpacket.length = (uint8_t) max_chan_count;
319  }
320 
321  *channel_count = (uint16_t)_rxpacket.length;
322 
323  /* decode the actual packet */
324  /* reorder first 4 channels */
325 
326  /* ch1 = roll -> sumd = ch2 */
327  channels[0] = (uint16_t)((_rxpacket.sumd_data[1 * 2 + 1] << 8) | _rxpacket.sumd_data[1 * 2 + 2]) >> 3;
328  /* ch2 = pitch -> sumd = ch2 */
329  channels[1] = (uint16_t)((_rxpacket.sumd_data[2 * 2 + 1] << 8) | _rxpacket.sumd_data[2 * 2 + 2]) >> 3;
330  /* ch3 = throttle -> sumd = ch2 */
331  channels[2] = (uint16_t)((_rxpacket.sumd_data[0 * 2 + 1] << 8) | _rxpacket.sumd_data[0 * 2 + 2]) >> 3;
332  /* ch4 = yaw -> sumd = ch2 */
333  channels[3] = (uint16_t)((_rxpacket.sumd_data[3 * 2 + 1] << 8) | _rxpacket.sumd_data[3 * 2 + 2]) >> 3;
334 
335  /* we start at channel 5(index 4) */
336  unsigned chan_index = 4;
337 
338  for (i = 4; i < _rxpacket.length; i++) {
339  if (_debug) {
340  printf("ch[%d] : %x %x [ %x %d ]\n", i + 1, _rxpacket.sumd_data[i * 2 + 1], _rxpacket.sumd_data[i * 2 + 2],
341  ((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3,
342  ((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3);
343  }
344 
345  channels[chan_index] = (uint16_t)((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3;
346  /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
347  //channels[chan_index] = (uint16_t)(channels[chan_index] * SUMD_SCALE_FACTOR + .5f) + SUMD_SCALE_OFFSET;
348 
349  chan_index++;
350  }
351 
352  } else {
353  /* decoding failed */
354  ret = 4;
355 
356  if (_debug) {
357  printf(" CRC - fail \n") ;
358  }
359 
360  }
361 
363  break;
364  }
365 
366  return ret;
367 }
#define SUMD_ID_FAILSAFE
Definition: sumd.h:53
int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count, bool *failsafe)
Decoder for SUMD/SUMH protocol.
Definition: sumd.cpp:111
uint8_t crc16_low
Low Byte of 16 Bit CRC.
Definition: sumd.h:65
RC protocol definition for Graupner HoTT transmitter.
bool _sumd
Definition: sumd.cpp:72
uint8_t sumd_crc8(uint8_t crc, uint8_t value)
CRC8 implementation for SUMH protocol.
Definition: sumd.cpp:105
uint16_t sumd_crc16(uint16_t crc, uint8_t value)
CRC16 implementation for SUMD protocol.
Definition: sumd.cpp:93
static ReceiverFcPacketHoTT & _rxpacket
Definition: sumd.cpp:91
uint8_t crc8
SUMH CRC8.
Definition: sumd.h:67
bool _debug
Definition: sumd.cpp:74
__EXPORT rc_decode_buf_t rc_decode_buf
Definition: common_rc.cpp:4
static uint8_t _rxlen
Definition: sumd.cpp:89
uint8_t length
Channels.
Definition: sumd.h:62
uint8_t sumd_data[SUMD_MAX_CHANNELS *2]
ChannelData (High Byte/ Low Byte)
Definition: sumd.h:63
ReceiverFcPacketHoTT _hottrxpacket
Definition: common_rc.h:19
#define SUMD_ID_SUMH
Definition: sumd.h:51
bool _crcOK
Definition: sumd.cpp:73
uint16_t _crc16
Definition: sumd.cpp:71
static enum SUMD_DECODE_STATE _decode_state
Definition: sumd.cpp:88
uint8_t header
0xA8 for a valid packet
Definition: sumd.h:60
uint8_t telemetry
Telemetry request.
Definition: sumd.h:66
#define SUMD_ID_SUMD
Definition: sumd.h:52
#define SUMD_HEADER_ID
Definition: sumd.h:50
#define SUMD_MAX_CHANNELS
Definition: sumd.h:48
uint8_t _crc8
Definition: sumd.cpp:70
uint8_t crc16_high
High Byte of 16 Bit CRC.
Definition: sumd.h:64
SUMD_DECODE_STATE
Definition: sumd.cpp:47
uint8_t status
0x01 valid and live SUMD data frame / 0x00 = SUMH / 0x81 = Failsafe
Definition: sumd.h:61