PX4 Firmware
PX4 Autopilot Software http://px4.io
st24.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2014 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 st24.h
36  *
37  * RC protocol implementation for Yuneec ST24 transmitter.
38  *
39  * @author Lorenz Meier <lm@inf.ethz.ch>
40  */
41 
42 #include <stdbool.h>
43 #include <stdio.h>
44 #include "st24.h"
45 #include "common_rc.h"
46 
47 const char *decode_states[] = {"UNSYNCED",
48  "GOT_STX1",
49  "GOT_STX2",
50  "GOT_LEN",
51  "GOT_TYPE",
52  "GOT_DATA"
53  };
54 
55 /* define range mapping here, -+100% -> 1000..2000 */
56 #define ST24_RANGE_MIN 500.0f
57 #define ST24_RANGE_MAX 3500.0f
58 
59 #define ST24_TARGET_MIN 1000.0f
60 #define ST24_TARGET_MAX 2000.0f
61 
62 /* pre-calculate the floating point stuff as far as possible at compile time */
63 #define ST24_SCALE_FACTOR ((ST24_TARGET_MAX - ST24_TARGET_MIN) / (ST24_RANGE_MAX - ST24_RANGE_MIN))
64 #define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f))
65 
67 static uint8_t _rxlen;
68 
70 
71 uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len)
72 {
73  uint8_t i, crc ;
74  crc = 0;
75 
76  while (len--) {
77  for (i = 0x80; i != 0; i >>= 1) {
78  if ((crc & 0x80) != 0) {
79  crc <<= 1;
80  crc ^= 0x07;
81 
82  } else {
83  crc <<= 1;
84  }
85 
86  if ((*ptr & i) != 0) {
87  crc ^= 0x07;
88  }
89  }
90 
91  ptr++;
92  }
93 
94  return (crc);
95 }
96 
97 
98 int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *lost_count, uint16_t *channel_count, uint16_t *channels,
99  uint16_t max_chan_count)
100 {
101  int ret = 1;
102 
103  switch (_decode_state) {
105  if (byte == ST24_STX1) {
107 
108  } else {
109  ret = 3;
110  }
111 
112  break;
113 
115  if (byte == ST24_STX2) {
117 
118  } else {
120  }
121 
122  break;
123 
125 
126  /* ensure no data overflow failure or hack is possible */
127  if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) {
128  _rxpacket.length = byte;
129  _rxlen = 0;
131 
132  } else {
134  }
135 
136  break;
137 
139  _rxpacket.type = byte;
140  _rxlen++;
142  break;
143 
145  _rxpacket.st24_data[_rxlen - 1] = byte;
146  _rxlen++;
147 
148  if (_rxlen == (_rxpacket.length - 1)) {
150  }
151 
152  break;
153 
155  _rxpacket.crc8 = byte;
156  _rxlen++;
157 
158  if (st24_common_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) {
159 
160  ret = 0;
161 
162  /* decode the actual packet */
163 
164  switch (_rxpacket.type) {
165 
167  ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data;
168 
169  // Scale from 0..255 to 100%.
170  *rssi = d->rssi * (100.0f / 255.0f);
171  *lost_count = d->lost_count;
172 
173  /* this can lead to rounding of the strides */
174  *channel_count = (max_chan_count < 12) ? max_chan_count : 12;
175 
176  unsigned stride_count = (*channel_count * 3) / 2;
177  unsigned chan_index = 0;
178 
179  for (unsigned i = 0; i < stride_count; i += 3) {
180  channels[chan_index] = ((uint16_t)d->channel[i] << 4);
181  channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
182  /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
183  channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
184  chan_index++;
185 
186  channels[chan_index] = ((uint16_t)d->channel[i + 2]);
187  channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
188  /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
189  channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
190  chan_index++;
191  }
192  }
193  break;
194 
196  ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data;
197 
198  // Scale from 0..255 to 100%.
199  *rssi = d->rssi * (100.0f / 255.0f);
200  *lost_count = d->lost_count;
201 
202  /* this can lead to rounding of the strides */
203  *channel_count = (max_chan_count < 24) ? max_chan_count : 24;
204 
205  unsigned stride_count = (*channel_count * 3) / 2;
206  unsigned chan_index = 0;
207 
208  for (unsigned i = 0; i < stride_count; i += 3) {
209  channels[chan_index] = ((uint16_t)d->channel[i] << 4);
210  channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
211  /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
212  channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
213  chan_index++;
214 
215  channels[chan_index] = ((uint16_t)d->channel[i + 2]);
216  channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
217  /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
218  channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
219  chan_index++;
220  }
221  }
222  break;
223 
225 
226  // ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data;
227  /* we silently ignore this data for now, as it is unused */
228  ret = 5;
229  }
230  break;
231 
232  default:
233  ret = 2;
234  break;
235  }
236 
237  } else {
238  /* decoding failed */
239  ret = 4;
240  }
241 
243  break;
244  }
245 
246  return ret;
247 }
#define ST24_STX1
Definition: st24.h:49
uint8_t lost_count
Number of UART packets sent since reception of last RF frame (100 frame means RC timeout of 1s) ...
Definition: st24.h:96
const char * decode_states[]
Definition: st24.cpp:47
uint8_t channel[18]
channel data, 12 channels (12 bit numbers)
Definition: st24.h:86
static uint8_t _rxlen
Definition: st24.cpp:67
static ReceiverFcPacket & _rxpacket
Definition: st24.cpp:69
uint8_t byte
Definition: sbus.cpp:559
uint8_t type
from enum ST24_PACKET_TYPE
Definition: st24.h:72
#define ST24_SCALE_OFFSET
Definition: st24.cpp:64
uint8_t rssi
signal strength
Definition: st24.h:84
ST24_DECODE_STATE
Definition: st24.h:58
__EXPORT rc_decode_buf_t rc_decode_buf
Definition: common_rc.cpp:4
static enum ST24_DECODE_STATE _decode_state
Definition: st24.cpp:66
uint8_t channel[36]
channel data, 24 channels (12 bit numbers)
Definition: st24.h:97
RC Channel data (12 channels).
Definition: st24.h:82
uint8_t st24_data[ST24_DATA_LEN_MAX]
Definition: st24.h:73
RC protocol definition for Yuneec ST24 transmitter.
uint8_t crc8
crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data ...
Definition: st24.h:74
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
RC Channel data (12 channels).
Definition: st24.h:93
uint8_t rssi
signal strength
Definition: st24.h:95
uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len)
CRC8 implementation for ST24 protocol.
Definition: st24.cpp:71
ReceiverFcPacket _strxpacket
Definition: common_rc.h:18
int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *lost_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count)
Decoder for ST24 protocol.
Definition: st24.cpp:98
uint8_t length
length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8) ...
Definition: st24.h:71
#define ST24_STX2
Definition: st24.h:50
#define ST24_SCALE_FACTOR
Definition: st24.cpp:63
uint8_t lost_count
Number of UART packets sent since reception of last RF frame (100 frame means RC timeout of 1s) ...
Definition: st24.h:85