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 : SRXL2 protocol decoder using Horizon Hobby's open source library https://github.com/SpektrumRC/SRXL2
17 : Code by Andy Piper
18 : */
19 :
20 : #include "AP_RCProtocol_config.h"
21 :
22 : #if AP_RCPROTOCOL_SRXL2_ENABLED
23 :
24 : #include "AP_RCProtocol.h"
25 : #include "AP_RCProtocol_SRXL2.h"
26 : #include <AP_Math/AP_Math.h>
27 : #include <AP_RCTelemetry/AP_Spektrum_Telem.h>
28 : #include <AP_Vehicle/AP_Vehicle_Type.h>
29 : #include <AP_HAL/utility/sparse-endian.h>
30 : #include <AP_VideoTX/AP_VideoTX.h>
31 :
32 : #include "spm_srxl.h"
33 :
34 : extern const AP_HAL::HAL& hal;
35 : //#define SRXL2_DEBUG
36 : #ifdef SRXL2_DEBUG
37 : # define debug(fmt, args...) hal.console->printf("SRXL2:" fmt "\n", ##args)
38 : #else
39 : # define debug(fmt, args...) do {} while(0)
40 : #endif
41 :
42 : AP_RCProtocol_SRXL2* AP_RCProtocol_SRXL2::_singleton;
43 :
44 2094 : AP_RCProtocol_SRXL2::AP_RCProtocol_SRXL2(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend)
45 : {
46 : #if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
47 2094 : if (_singleton != nullptr) {
48 0 : AP_HAL::panic("Duplicate SRXL2 handler");
49 : }
50 :
51 2094 : _singleton = this;
52 : #else
53 0 : if (_singleton == nullptr) {
54 0 : _singleton = this;
55 : }
56 : #endif
57 2094 : }
58 :
59 0 : void AP_RCProtocol_SRXL2::_bootstrap(uint8_t device_id)
60 : {
61 0 : if (_device_id == device_id) {
62 0 : return;
63 : }
64 :
65 : // Init the local SRXL device
66 0 : if (!srxlInitDevice(device_id, SRXL_DEVICE_PRIORITY, SRXL_DEVICE_INFO, device_id)) {
67 0 : AP_HAL::panic("Failed to initialize SRXL2 device");
68 : }
69 :
70 : // Init the SRXL bus: The bus index must always be < SRXL_NUM_OF_BUSES -- in this case, it can only be 0
71 0 : if (!srxlInitBus(0, 0, SRXL_SUPPORTED_BAUD_RATES)) {
72 0 : AP_HAL::panic("Failed to initialize SRXL2 bus");
73 : }
74 :
75 0 : _device_id = device_id;
76 :
77 : debug("Bootstrapped as 0x%x", _device_id);
78 : }
79 :
80 908 : AP_RCProtocol_SRXL2::~AP_RCProtocol_SRXL2() {
81 454 : _singleton = nullptr;
82 908 : }
83 :
84 0 : void AP_RCProtocol_SRXL2::_process_byte(uint32_t timestamp_us, uint8_t byte)
85 : {
86 0 : if (_decode_state == STATE_IDLE) {
87 0 : switch (byte) {
88 0 : case SPEKTRUM_SRXL_ID:
89 0 : _decode_state = STATE_NEW;
90 0 : break;
91 0 : default:
92 0 : _decode_state = STATE_IDLE;
93 0 : return;
94 : }
95 0 : _frame_len_full = 0U;
96 0 : _buflen = 0;
97 0 : _decode_state_next = STATE_IDLE;
98 : }
99 :
100 0 : switch (_decode_state) {
101 0 : case STATE_NEW: // buffer header byte and prepare for frame reception and decoding
102 0 : _buffer[0U]=byte;
103 0 : _buflen = 1U;
104 0 : _decode_state_next = STATE_COLLECT;
105 0 : break;
106 :
107 0 : case STATE_COLLECT: // receive all bytes. After reception decode frame and provide rc channel information to FMU
108 0 : _buffer[_buflen] = byte;
109 0 : _buflen++;
110 :
111 : // need a header to get the length
112 0 : if (_buflen < SRXL2_HEADER_LEN) {
113 0 : return;
114 : }
115 :
116 : // parse the length
117 0 : if (_buflen == SRXL2_HEADER_LEN) {
118 0 : _frame_len_full = _buffer[2];
119 : // check for garbage frame
120 0 : if (_frame_len_full > SRXL2_FRAMELEN_MAX) {
121 0 : _decode_state = STATE_IDLE;
122 0 : _buflen = 0;
123 0 : _frame_len_full = 0;
124 : }
125 0 : return;
126 : }
127 :
128 0 : if (_buflen > _frame_len_full) {
129 : // a logic bug in the state machine, this shouldn't happen
130 0 : _decode_state = STATE_IDLE;
131 0 : _buflen = 0;
132 0 : _frame_len_full = 0;
133 0 : return;
134 : }
135 :
136 0 : if (_buflen == _frame_len_full) {
137 0 : log_data(AP_RCProtocol::SRXL2, timestamp_us, _buffer, _buflen);
138 : // we got a full frame but never handshaked before
139 0 : if (!is_bootstrapped()) {
140 0 : if (_buffer[1] == 0x21) {
141 0 : _bootstrap(SRXL_DEVICE_ID);
142 0 : _last_handshake_ms = timestamp_us / 1000;
143 : } else {
144 : // not a handshake frame so reset without initializing the SRXL2 engine
145 0 : _decode_state = STATE_IDLE;
146 0 : _buflen = 0;
147 0 : _frame_len_full = 0;
148 0 : return;
149 : }
150 : }
151 : // Try to parse SRXL packet -- this internally calls srxlRun() after packet is parsed and resets timeout
152 0 : if (srxlParsePacket(0, _buffer, _frame_len_full)) {
153 0 : add_input(ARRAY_SIZE(_channels), _channels, _in_failsafe, _new_rssi);
154 : }
155 0 : _last_run_ms = AP_HAL::millis();
156 :
157 0 : _decode_state_next = STATE_IDLE;
158 0 : _buflen = 0;
159 : } else {
160 0 : _decode_state_next = STATE_COLLECT;
161 : }
162 0 : break;
163 :
164 0 : default:
165 0 : break;
166 : }
167 :
168 0 : _decode_state = _decode_state_next;
169 : }
170 :
171 36177494 : void AP_RCProtocol_SRXL2::update(void)
172 : {
173 : // on a SPM4650 with telemetry the frame rate is 91Hz equating to around 10ms per frame
174 : // however only half of them can return telemetry, so the maximum telemetry rate is 46Hz
175 : // also update() is run immediately after check_added_uart() and so in general the delay is < 5ms
176 : // to be safe we will only run if the timeout exceeds 50ms
177 36177494 : if (_last_run_ms > 0) {
178 0 : uint32_t now = AP_HAL::millis();
179 : // there have been no updates since we were last called
180 0 : const uint32_t delay = now - _last_run_ms;
181 0 : if (delay > 50) {
182 0 : srxlRun(0, delay);
183 0 : _last_run_ms = now;
184 : }
185 : }
186 36177494 : }
187 :
188 : // capture SRXL2 encoded values
189 0 : void AP_RCProtocol_SRXL2::capture_scaled_input(const uint8_t *values_p, bool in_failsafe, int16_t new_rssi)
190 : {
191 0 : _in_failsafe = in_failsafe;
192 : // AP rssi: -1 for unknown, 0 for no link, 255 for maximum link
193 : // SRXL2 rssi: -ve rssi in dBM, +ve rssi in percentage
194 0 : if (new_rssi >= 0) {
195 0 : _new_rssi = new_rssi * 255 / 100;
196 : }
197 :
198 0 : for (uint8_t i = 0; i < ARRAY_SIZE(_channels); i++) {
199 : /*
200 : * Store the decoded channel into the R/C input buffer, taking into
201 : * account the different ideas about channel assignement that we have.
202 : *
203 : * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw,
204 : * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw.
205 : */
206 0 : uint8_t channel = i;
207 0 : switch (channel) {
208 0 : case 0:
209 0 : channel = 2;
210 0 : break;
211 :
212 0 : case 1:
213 0 : channel = 0;
214 0 : break;
215 :
216 0 : case 2:
217 0 : channel = 1;
218 0 : break;
219 :
220 0 : default:
221 0 : break;
222 : }
223 : /*
224 : * Each channel data value is sent as an unsigned 16-bit value from 0 to 65532 (0xFFFC)
225 : * with 32768 (0x8000) representing "Servo Center". The channel value must be bit-shifted
226 : * to the right to match the applications's accepted resolution.
227 : *
228 : * So here we scale to DSMX-2048 and then use our regular Spektrum conversion.
229 : */
230 0 : const uint16_t v = le16toh_ptr(&values_p[i*2]);
231 0 : _channels[channel] = ((int32_t)(v >> 5) * 1194) / 2048 + 903;
232 : }
233 0 : }
234 :
235 :
236 : // start bind on DSM satellites
237 0 : void AP_RCProtocol_SRXL2::start_bind(void)
238 : {
239 0 : srxlEnterBind(DSMX_11MS, true);
240 0 : }
241 :
242 : // process a byte provided by a uart
243 0 : void AP_RCProtocol_SRXL2::process_byte(uint8_t byte, uint32_t baudrate)
244 : {
245 0 : if (baudrate != 115200) {
246 0 : return;
247 : }
248 :
249 0 : _process_byte(AP_HAL::micros(), byte);
250 : }
251 :
252 : // handshake
253 4 : void AP_RCProtocol_SRXL2::process_handshake(uint32_t baudrate)
254 : {
255 : // only bootstrap if only SRXL2 is enabled
256 4 : if (baudrate != 115200 || (get_rc_protocols_mask() & ~(1U<<(uint8_t(AP_RCProtocol::SRXL2)+1)))) {
257 4 : _handshake_start_ms = 0;
258 4 : return;
259 : }
260 0 : uint32_t now = AP_HAL::millis();
261 :
262 : // record the time of the first request in this cycle
263 0 : if (_handshake_start_ms == 0) {
264 0 : _handshake_start_ms = now;
265 : // it seems the handshake protocol only sets the baudrate after receiving data
266 : // since we are sending data unprompted make sure that the uart is set up correctly
267 0 : change_baud_rate(baudrate);
268 : }
269 :
270 : // we have not bootstrapped and attempts to listen first have failed
271 : // we should receive a handshake request within the first 250ms
272 0 : if (!is_bootstrapped() && now - _handshake_start_ms > 250) {
273 0 : _bootstrap(SRXL_DEVICE_ID_BASE_RX);
274 : }
275 :
276 : // certain RXs (e.g. AR620) are "listen-only" - they require the flight controller to initiate
277 : // a handshake in order to switch to SRXL2 mode. This requires that we send data on the UART even
278 : // if we have not decoded SRXL2 (recently). We try this every 50ms.
279 0 : if (now - _handshake_start_ms > 250 && (_last_handshake_ms == 0 || (now - _last_run_ms > 50 && now - _last_handshake_ms > 50))) {
280 0 : _in_bootstrap_or_failsafe = true;
281 0 : srxlRun(0, 50); // 50 is a magic number at which the handshake protocol is initiated
282 0 : _in_bootstrap_or_failsafe = false;
283 0 : _last_handshake_ms = now;
284 : }
285 : }
286 :
287 : // send data to the uart
288 0 : void AP_RCProtocol_SRXL2::send_on_uart(uint8_t* pBuffer, uint8_t length)
289 : {
290 0 : AP_HAL::UARTDriver* uart = get_available_UART();
291 :
292 0 : if (uart != nullptr && uart->is_initialized()) {
293 : // check that we haven't been too slow in responding to the new UART data. If we respond too late then we will
294 : // corrupt the next incoming control frame. incoming packets at max 800bits @91Hz @115k baud gives total budget of 11ms
295 : // per packet of which we need 7ms to receive a packet. outgoing packets are 220 bits which require 2ms to send
296 : // leaving at most 2ms of delay that can be tolerated
297 0 : uint64_t tend = uart->receive_time_constraint_us(1);
298 0 : uint64_t now = AP_HAL::micros64();
299 0 : uint64_t tdelay = now - tend;
300 0 : if (tdelay > 2000 && !_in_bootstrap_or_failsafe) {
301 : // we've been too slow in responding
302 0 : return;
303 : }
304 : // debug telemetry packets
305 0 : if (pBuffer[1] == 0x80 && pBuffer[4] != 0) {
306 : debug("0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x: %s",
307 : pBuffer[0], pBuffer[1], pBuffer[2], pBuffer[3], pBuffer[4], pBuffer[5], pBuffer[6], pBuffer[7], pBuffer[8], pBuffer[9], &pBuffer[7]);
308 : }
309 0 : uart->write(pBuffer, length);
310 : }
311 : }
312 :
313 : // change the uart baud rate
314 0 : void AP_RCProtocol_SRXL2::change_baud_rate(uint32_t baudrate)
315 : {
316 0 : AP_HAL::UARTDriver* uart = get_available_UART();
317 0 : if (uart != nullptr) {
318 0 : uart->begin(baudrate);
319 0 : uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
320 0 : uart->set_unbuffered_writes(true);
321 : }
322 0 : }
323 :
324 : // SRXL2 library callbacks below
325 :
326 : // User-provided routine to change the baud rate settings on the given UART:
327 : // uart - the same uint8_t value as the uart parameter passed to srxlInit()
328 : // baudRate - the actual baud rate (currently either 115200 or 400000)
329 0 : void srxlChangeBaudRate(uint8_t uart, uint32_t baudRate)
330 : {
331 0 : AP_RCProtocol_SRXL2* srxl2 = AP_RCProtocol_SRXL2::get_singleton();
332 :
333 0 : if (srxl2 != nullptr) {
334 0 : srxl2->change_baud_rate(baudRate);
335 : }
336 0 : }
337 :
338 : // User-provided routine to actually transmit a packet on the given UART:
339 : // uart - the same uint8_t value as the uart parameter passed to srxlInit()
340 : // pBuffer - a pointer to an array of uint8_t values to send over the UART
341 : // length - the number of bytes contained in pBuffer that should be sent
342 0 : void srxlSendOnUart(uint8_t uart, uint8_t* pBuffer, uint8_t length)
343 : {
344 0 : AP_RCProtocol_SRXL2* srxl2 = AP_RCProtocol_SRXL2::get_singleton();
345 :
346 0 : if (srxl2 != nullptr) {
347 0 : srxl2->send_on_uart(pBuffer, length);
348 : }
349 0 : }
350 :
351 : // User-provided callback routine to fill in the telemetry data to send to the master when requested:
352 : // pTelemetryData - a pointer to the 16-byte SrxlTelemetryData transmit buffer to populate
353 : // NOTE: srxlTelemData is available as a global variable, so the memcpy line commented out below
354 : // could be used if you would prefer to just populate that with the next outgoing telemetry packet.
355 0 : void srxlFillTelemetry(SrxlTelemetryData* pTelemetryData)
356 : {
357 : #if HAL_SPEKTRUM_TELEM_ENABLED && !APM_BUILD_TYPE(APM_BUILD_iofirmware)
358 0 : AP_Spektrum_Telem::get_telem_data(pTelemetryData->raw);
359 : #endif
360 0 : }
361 :
362 : // User-provided callback routine that is called whenever a control data packet is received:
363 : // pChannelData - a pointer to the received SrxlChannelData structure for manual parsing
364 : // isFailsafe - true if channel data is set to failsafe values, else false.
365 : // this is called from within srxlParsePacket() and before the SRXL2 state machine has been run
366 : // so be very careful to only do local operations
367 0 : void srxlReceivedChannelData(SrxlChannelData* pChannelData, bool isFailsafe)
368 : {
369 0 : AP_RCProtocol_SRXL2* srxl2 = AP_RCProtocol_SRXL2::get_singleton();
370 :
371 0 : if (srxl2 == nullptr) {
372 0 : return;
373 : }
374 :
375 0 : if (isFailsafe) {
376 0 : srxl2->capture_scaled_input((const uint8_t *)pChannelData->values, true, pChannelData->rssi);
377 : } else {
378 0 : srxl2->capture_scaled_input((const uint8_t *)srxlChData.values, false, srxlChData.rssi);
379 : }
380 : }
381 :
382 : // User-provided callback routine to handle reception of a bound data report (either requested or unprompted).
383 : // Return true if you want this bind information set automatically for all other receivers on all SRXL buses.
384 0 : bool srxlOnBind(SrxlFullID device, SrxlBindData info)
385 : {
386 0 : return true;
387 : }
388 :
389 : // User-provided callback routine to handle reception of a VTX control packet.
390 0 : void srxlOnVtx(SrxlVtxData* pVtxData)
391 : {
392 : #if AP_VIDEOTX_ENABLED
393 0 : AP_RCProtocol_Backend::configure_vtx(pVtxData->band, pVtxData->channel, pVtxData->power, pVtxData->pit);
394 : #endif
395 0 : }
396 :
397 : #endif // AP_RCPROTOCOL_SRXL2_ENABLED
|
