Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : SRXL protocol decoder, tested against AR7700 SRXL port
17 : Andrew Tridgell, September 2016
18 :
19 : Co author: Roman Kirchner, September 2016
20 : - 2016.10.23: SRXL variant V1 sucessfully (Testbench and Pixhawk/MissionPlanner) tested with RX-9-DR M-LINK (SW v1.26)
21 : */
22 :
23 : #include "AP_RCProtocol_config.h"
24 :
25 : #if AP_RCPROTOCOL_SRXL_ENABLED
26 :
27 : #include "AP_RCProtocol_SRXL.h"
28 : #include <AP_Math/crc.h>
29 : #include <AP_Math/AP_Math.h>
30 :
31 : // #define SUMD_DEBUG
32 : extern const AP_HAL::HAL& hal;
33 :
34 0 : void AP_RCProtocol_SRXL::process_pulse(uint32_t width_s0, uint32_t width_s1)
35 : {
36 : uint8_t b;
37 0 : if (ss.process_pulse(width_s0, width_s1, b)) {
38 0 : _process_byte(ss.get_byte_timestamp_us(), b);
39 : }
40 0 : }
41 :
42 :
43 : /**
44 : * Get RC channel information as microsecond pulsewidth representation from srxl version 1 and 2
45 : *
46 : * This function extracts RC channel information from srxl dataframe. The function expects the whole dataframe
47 : * in correct order in static array "buffer[SRXL_FRAMELEN_MAX]". After extracting all RC channel information, the data
48 : * is transferred to "values" array from parameter list. If the pixhawk does not support all channels from srxl datastream,
49 : * only supported number of channels will be refreshed.
50 : *
51 : * IMPORTANT SAFETY NOTICE: This function shall only be used after CRC has been successful.
52 : *
53 : * Structure of SRXL v1 dataframe --> 12 channels, 12 Bit per channel
54 : * Byte0: Header 0xA1
55 : * Byte1: Bits7-4:Empty Bits3-0:Channel1 MSB
56 : * Byte2: Bits7-0: Channel1 LSB
57 : * (....)
58 : * Byte23: Bits7-4:Empty Bits3-0:Channel12 MSB
59 : * Byte24: Bits7-0: Channel12 LSB
60 : * Byte25: CRC16 MSB
61 : * Byte26: CRC16 LSB
62 : *
63 : * Structure of SRXL v2 dataframe --> 16 channels, 12 Bit per channel
64 : * Byte0: Header 0xA2
65 : * Byte1: Bits7-4:Empty Bits3-0:Channel1 MSB
66 : * Byte2: Bits7-0: Channel1 LSB
67 : * (....)
68 : * Byte31: Bits7-4:Empty Bits3-0:Channel16 MSB
69 : * Byte32: Bits7-0: Channel16 LSB
70 : * Byte33: CRC16 MSB
71 : * Byte34: CRC16 LSB
72 : *
73 : * @param[in] max_values - maximum number of values supported by the pixhawk
74 : * @param[out] num_values - number of RC channels extracted from srxl frame
75 : * @param[out] values - array of RC channels with refreshed information as pulsewidth in microseconds Range: 800us - 2200us
76 : * @param[out] failsafe_state - true: RC-receiver is in failsafe state, false: RC-receiver is not in failsafe state
77 : * @retval 0 success
78 : */
79 0 : int AP_RCProtocol_SRXL::srxl_channels_get_v1v2(uint16_t max_values, uint8_t *num_values, uint16_t *values, bool *failsafe_state)
80 : {
81 : uint8_t loop;
82 : uint32_t channel_raw_value;
83 :
84 0 : *num_values = (uint8_t)((frame_len_full - 3U)/2U);
85 0 : *failsafe_state = 0U; /* this protocol version does not support failsafe information */
86 :
87 : /* get data channel data from frame */
88 0 : for (loop=0U; loop < *num_values; loop++) {
89 0 : channel_raw_value = ((((uint32_t)buffer[loop*2U+1U])& 0x0000000FU) << 8U) | ((uint32_t)(buffer[loop*2U+2U])); /* get 12bit channel raw value from srxl datastream (mask out unused bits with 0x0000000F) */
90 0 : channels[loop] = (uint16_t)(((channel_raw_value * (uint32_t)1400U) >> 12U) + (uint32_t)800U); /* convert raw value to servo/esc signal pulsewidth in us */
91 : }
92 :
93 : /* provide channel data to FMU */
94 0 : if ((uint16_t)*num_values > max_values) {
95 0 : *num_values = (uint8_t)max_values;
96 : }
97 0 : memcpy(values, channels, (*num_values)*2);
98 :
99 0 : return 0; /* for srxl protocol version 1 and 2 it is not expected, that any error happen during decode process */
100 : }
101 :
102 : /**
103 : * Get RC channel information as microsecond pulsewidth representation from srxl version 5
104 : *
105 : * This function extracts RC channel information from srxl dataframe. The function expects the whole dataframe
106 : * in correct order in static array "buffer[SRXL_FRAMELEN_MAX]". After extracting all RC channel information, the data
107 : * is transferred to "values" array from parameter list. If the pixhawk does not support all channels from srxl datastream,
108 : * only supported number of channels will be refreshed.
109 : *
110 : * IMPORTANT SAFETY NOTICE: This function shall only be used after CRC has been successful.
111 : *
112 : * Structure of SRXL v5 dataframe
113 : * Byte0: Header 0xA5
114 : * Byte1 - Byte16: Payload
115 : * Byte17: CRC16 MSB
116 : * Byte18: CRC16 LSB
117 : *
118 : * @param[in] max_values - maximum number of values supported by the pixhawk
119 : * @param[out] num_values - number of RC channels extracted from srxl frame
120 : * @param[out] values - array of RC channels with refreshed information as pulsewidth in microseconds Range: 800us - 2200us
121 : * @param[out] failsafe_state - true: RC-receiver is in failsafe state, false: RC-receiver is not in failsafe state
122 : * @retval 0 success
123 : */
124 0 : int AP_RCProtocol_SRXL::srxl_channels_get_v5(uint16_t max_values, uint8_t *num_values, uint16_t *values, bool *failsafe_state)
125 : {
126 : // up to 7 channel values per packet. Each channel value is 16
127 : // bits, with 11 bits of data and 4 bits of channel number. The
128 : // top bit indicates a special X-Plus channel
129 0 : for (uint8_t i=0; i<7; i++) {
130 0 : uint16_t b = buffer[i*2+2] << 8 | buffer[i*2+3];
131 0 : uint16_t c = b >> 11; // channel number
132 0 : int32_t v = b & 0x7FF;
133 0 : if (b & 0x8000) {
134 0 : continue;
135 : }
136 0 : if (c == 12) {
137 : // special handling for channel 12
138 : // see http://www.deviationtx.com/forum/protocol-development/2088-18-channels-for-dsm2-dsmx?start=40
139 : //printf("c12: 0x%x %02x %02x\n", (unsigned)(b>>9), (unsigned)buffer[0], (unsigned)buffer[1]);
140 0 : v = (b & 0x1FF) << 2;
141 0 : c = 10 + ((b >> 9) & 0x7);
142 0 : if (buffer[1] & 1) {
143 0 : c += 4;
144 : }
145 0 : } else if (c > 12) {
146 : // invalid
147 0 : v = 0;
148 : }
149 :
150 : // if channel number if greater than 16 then it is a X-Plus
151 : // channel. We don't yet know how to decode those. There is some information here:
152 : // http://www.deviationtx.com/forum/protocol-development/2088-18-channels-for-dsm2-dsmx?start=40
153 : // but we really need some sample data to confirm
154 0 : if (c < SRXL_MAX_CHANNELS) {
155 0 : v = (((v - 0x400) * 500) / 876) + 1500;
156 0 : channels[c] = v;
157 0 : if (c >= max_channels) {
158 0 : max_channels = c+1;
159 : }
160 : }
161 :
162 : //printf("%u:%u ", (unsigned)c, (unsigned)v);
163 : }
164 : //printf("\n");
165 :
166 0 : *num_values = max_channels;
167 0 : if (*num_values > max_values) {
168 0 : *num_values = max_values;
169 : }
170 0 : memcpy(values, channels, (*num_values)*2);
171 :
172 : // check failsafe bit, this goes low when connection to the
173 : // transmitter is lost
174 0 : *failsafe_state = ((buffer[1] & 2) == 0);
175 :
176 : // success
177 0 : return 0;
178 : }
179 :
180 0 : void AP_RCProtocol_SRXL::_process_byte(uint32_t timestamp_us, uint8_t byte)
181 : {
182 : /*----------------------------------------distinguish different srxl variants at the beginning of each frame---------------------------------------------- */
183 : /* Check if we have a new begin of a frame --> indicators: Time gap in datastream + SRXL header 0xA<VARIANT>*/
184 0 : if ((timestamp_us - last_data_us) >= SRXL_MIN_FRAMESPACE_US) {
185 : /* Now detect SRXL variant based on header */
186 0 : switch (byte) {
187 0 : case SRXL_HEADER_V1:
188 0 : frame_len_full = SRXL_FRAMELEN_V1;
189 0 : frame_header = SRXL_HEADER_V1;
190 0 : decode_state = STATE_NEW;
191 0 : break;
192 0 : case SRXL_HEADER_V2:
193 0 : frame_len_full = SRXL_FRAMELEN_V2;
194 0 : frame_header = SRXL_HEADER_V2;
195 0 : decode_state = STATE_NEW;
196 0 : break;
197 0 : case SRXL_HEADER_V5:
198 0 : frame_len_full = SRXL_FRAMELEN_V5;
199 0 : frame_header = SRXL_HEADER_V5;
200 0 : decode_state = STATE_NEW;
201 0 : break;
202 0 : default:
203 0 : frame_len_full = 0U;
204 0 : frame_header = SRXL_HEADER_NOT_IMPL;
205 0 : decode_state = STATE_IDLE;
206 0 : buflen = 0;
207 0 : return; /* protocol version not implemented --> no channel data --> unknown packet */
208 : }
209 : }
210 :
211 :
212 : /*--------------------------------------------collect all data from stream and decode-------------------------------------------------------*/
213 0 : switch (decode_state) {
214 0 : case STATE_NEW: /* buffer header byte and prepare for frame reception and decoding */
215 0 : buffer[0U]=byte;
216 0 : crc_fmu = crc_xmodem_update(0U,byte);
217 0 : buflen = 1U;
218 0 : decode_state_next = STATE_COLLECT;
219 0 : break;
220 :
221 0 : case STATE_COLLECT: /* receive all bytes. After reception decode frame and provide rc channel information to FMU */
222 0 : if (buflen >= frame_len_full) {
223 : // a logic bug in the state machine, this shouldn't happen
224 0 : decode_state = STATE_IDLE;
225 0 : buflen = 0;
226 0 : frame_len_full = 0;
227 0 : frame_header = SRXL_HEADER_NOT_IMPL;
228 0 : return;
229 : }
230 0 : buffer[buflen] = byte;
231 0 : buflen++;
232 : /* CRC not over last 2 frame bytes as these bytes inhabitate the crc */
233 0 : if (buflen <= (frame_len_full-2)) {
234 0 : crc_fmu = crc_xmodem_update(crc_fmu,byte);
235 : }
236 0 : if (buflen == frame_len_full) {
237 0 : log_data(AP_RCProtocol::SRXL, timestamp_us, buffer, buflen);
238 : /* CRC check here */
239 0 : crc_receiver = ((uint16_t)buffer[buflen-2] << 8U) | ((uint16_t)buffer[buflen-1]);
240 0 : if (crc_receiver == crc_fmu) {
241 : /* at this point buffer contains all frame data and crc is valid --> extract channel info according to SRXL variant */
242 0 : const uint8_t max_values = MIN((unsigned)SRXL_MAX_CHANNELS,(unsigned)MAX_RCIN_CHANNELS);
243 0 : uint16_t values[max_values];
244 : uint8_t num_values;
245 : bool failsafe_state;
246 0 : switch (frame_header) {
247 0 : case SRXL_HEADER_V1:
248 0 : srxl_channels_get_v1v2(max_values, &num_values, values, &failsafe_state);
249 0 : add_input(num_values, values, failsafe_state);
250 0 : break;
251 0 : case SRXL_HEADER_V2:
252 0 : srxl_channels_get_v1v2(max_values, &num_values, values, &failsafe_state);
253 0 : add_input(num_values, values, failsafe_state);
254 0 : break;
255 0 : case SRXL_HEADER_V5:
256 0 : srxl_channels_get_v5(max_values, &num_values, values, &failsafe_state);
257 0 : add_input(num_values, values, failsafe_state);
258 0 : break;
259 0 : default:
260 0 : break;
261 : }
262 0 : }
263 0 : decode_state_next = STATE_IDLE; /* frame data buffering and decoding finished --> statemachine not in use until new header drops is */
264 : } else {
265 : /* frame not completely received --> frame data buffering still ongoing */
266 0 : decode_state_next = STATE_COLLECT;
267 : }
268 0 : break;
269 :
270 0 : default:
271 0 : break;
272 : } /* switch (decode_state) */
273 :
274 0 : decode_state = decode_state_next;
275 0 : last_data_us = timestamp_us;
276 : }
277 :
278 : /*
279 : process a byte provided by a uart
280 : */
281 0 : void AP_RCProtocol_SRXL::process_byte(uint8_t byte, uint32_t baudrate)
282 : {
283 0 : if (baudrate != 115200) {
284 0 : return;
285 : }
286 0 : _process_byte(AP_HAL::micros(), byte);
287 : }
288 :
289 : #endif // AP_RCPROTOCOL_SRXL_ENABLED
|
