Line data Source code
1 : /*
2 : MIT License
3 :
4 : Copyright (c) 2019 Horizon Hobby, LLC
5 :
6 : Permission is hereby granted, free of charge, to any person obtaining a copy
7 : of this software and associated documentation files (the "Software"), to deal
8 : in the Software without restriction, including without limitation the rights
9 : to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
10 : copies of the Software, and to permit persons to whom the Software is
11 : furnished to do so, subject to the following conditions:
12 :
13 : The above copyright notice and this permission notice shall be included in all
14 : copies or substantial portions of the Software.
15 :
16 : THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17 : IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
18 : FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
19 : AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
20 : LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
21 : OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
22 : SOFTWARE.
23 : */
24 :
25 : #include <string.h>
26 : #include <stdint.h>
27 :
28 : #include "spm_srxl.h"
29 :
30 : /// LOCAL TYPES AND CONSTANTS ///
31 :
32 : #if(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_SPEED)
33 : const uint16_t srxlCRCTable[] =
34 : {
35 : 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
36 : 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
37 : 0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
38 : 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
39 : 0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
40 : 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
41 : 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
42 : 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
43 :
44 : 0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
45 : 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
46 : 0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
47 : 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
48 : 0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
49 : 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
50 : 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
51 : 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
52 :
53 : 0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
54 : 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
55 : 0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
56 : 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
57 : 0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
58 : 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
59 : 0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
60 : 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
61 :
62 : 0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
63 : 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
64 : 0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
65 : 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
66 : 0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
67 : 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
68 : 0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
69 : 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
70 : };
71 : #endif
72 :
73 : #define SRXL_TELEM_SUPPRESS_MAX (100)
74 :
75 : /// PUBLIC VARIABLES ///
76 :
77 : SrxlChannelData srxlChData;
78 : SrxlTelemetryData srxlTelemData;
79 : SrxlVtxData srxlVtxData = {0, 0, 1, 0, 0, 1};
80 :
81 : /// LOCAL VARIABLES ///
82 :
83 : static SrxlDevice srxlThisDev;
84 : static SrxlBus srxlBus[SRXL_NUM_OF_BUSES];
85 : static bool srxlChDataIsFailsafe = false;
86 : static bool srxlTelemetryPhase = false;
87 : #ifdef SRXL_INCLUDE_MASTER_CODE
88 : static uint32_t srxlFailsafeChMask; // Tracks all active channels for use during failsafe transmission
89 : #endif
90 : static SrxlBindData srxlBindInfo;
91 : static SrxlReceiverStats srxlRx;
92 : static uint16_t srxlTelemSuppressCount;
93 :
94 : #ifdef SRXL_INCLUDE_FWD_PGM_CODE
95 : static SrxlFullID srxlFwdPgmDevice; // Device that should accept Forward Programming connection by default
96 : static uint8_t srxlFwdPgmBuffer[FWD_PGM_MAX_DATA_SIZE];
97 : static uint8_t srxlFwdPgmBufferLength;
98 : #endif
99 :
100 : // Include additional header and externs if using STM32 hardware acceleration
101 : #if(SRXL_CRC_OPTIMIZE_MODE > SRXL_CRC_OPTIMIZE_SIZE)
102 : #ifdef __cplusplus
103 : extern "C"
104 : {
105 : #endif
106 : #if(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_STM_HAL)
107 : #if(SRXL_STM_TARGET_FAMILY == SRXL_STM_TARGET_F7)
108 : #include "stm32f7xx_hal.h"
109 : #else
110 : #include "stm32f3xx_hal.h" // Default to F3 if not given
111 : #endif
112 : extern CRC_HandleTypeDef hcrc;
113 : #else
114 : #if(SRXL_STM_TARGET_FAMILY == SRXL_STM_TARGET_F7)
115 : #error "STM32F7 targets not yet supported for register-based STM HW optimization"
116 : #else
117 : #include "stm32f30x.h" // Default to F3 if not given
118 : #endif
119 : #endif
120 : #ifdef __cplusplus
121 : }
122 : #endif
123 : #endif
124 :
125 : /// LOCAL HELPER FUNCTIONS ///
126 :
127 : // Compute SRXL CRC over packet buffer (assumes length is correctly set)
128 0 : static uint16_t srxlCrc16(uint8_t* packet)
129 : {
130 0 : uint16_t crc = 0; // Seed with 0
131 0 : uint8_t length = packet[2] - 2; // Exclude 2 CRC bytes at end of packet from the length
132 :
133 0 : if(length <= SRXL_MAX_BUFFER_SIZE - 2)
134 : {
135 : #if(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_SIZE)
136 : // Use bitwise method
137 : for(uint8_t i = 0; i < length; ++i)
138 : {
139 : crc = crc ^ ((uint16_t)packet[i] << 8);
140 : for(int b = 0; b < 8; b++)
141 : {
142 : if(crc & 0x8000)
143 : crc = (crc << 1) ^ 0x1021;
144 : else
145 : crc = crc << 1;
146 : }
147 : }
148 : #elif(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_STM_HW)
149 : // Use direct STM32 HW CRC register access
150 : uint8_t* pEnd = &packet[length];
151 : srxlEnterCriticalSection();
152 : #ifdef SRXL_SAVE_HW_CRC_CONTEXT
153 : uint32 savedPOL = CRC->POL;
154 : uint32 savedINIT = CRC->INIT;
155 : uint32 savedCR = CRC->CR;
156 : uint32 savedDR = CRC->DR;
157 : #endif
158 : CRC->POL = 0x1021;
159 : CRC->INIT = 0;
160 : CRC->CR = 0x09; // 16-bit polynomial, no input or output reversal, reset to init of 0
161 : while(packet < pEnd)
162 : *(uint8_t*)(CRC_BASE) = *(packet++);
163 : crc = (uint16_t)CRC->DR;
164 : #ifdef SRXL_SAVE_HW_CRC_CONTEXT
165 : // We have to use this convoluted method to restore things because writing INIT sets DR too
166 : CRC->CR = 0;
167 : CRC->POL = 0x04C11DB7;
168 : CRC->INIT = savedINIT;
169 : CRC->DR = savedINIT;
170 : CRC->POL = savedDR;
171 : CRC->DR = 1;
172 : CRC->CR = savedCR;
173 : CRC->POL = savedPOL;
174 : #endif
175 : srxlExitCriticalSection();
176 : #elif(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_STM_HAL)
177 : // STM32f3/f7 hardware optimization using the STM32Cube libraries in HAL mode requires the following
178 : // configuration, set in the STM32CubeMX "Pinout & Configuration" tab under "Computing > CRC":
179 : // Basic Parameters
180 : // Default Polynomial State = Disable
181 : // CRC Length = 16-bit
182 : // CRC Generating Polynomial = X12+X5+X0
183 : // Default Init Value State = Disable
184 : // Init Value For CRC Computation = 0
185 : // Advanced Parameters
186 : // Input Data Inversion Mode = None
187 : // Output Data Inversion Mode = Disable
188 : // Input Data Format = Bytes
189 : crc = (uint16_t)HAL_CRC_Calculate(&hcrc, (uint32_t*)packet, length);
190 : #elif(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_EXTERNAL)
191 0 : crc = SRXL_CRC_CALCULATE(packet, length, crc);
192 : #else
193 : // Default to table-lookup method
194 : uint8_t i;
195 : for(i = 0; i < length; ++i)
196 : {
197 : // Get indexed position in lookup table using XOR of current CRC hi byte
198 : uint8_t pos = (uint8_t)((crc >> 8) ^ packet[i]);
199 : // Shift LSB up and XOR with the resulting lookup table entry
200 : crc = (uint16_t)((crc << 8) ^ (uint16_t)(srxlCRCTable[pos]));
201 : }
202 : #endif
203 : }
204 0 : return crc;
205 : }
206 :
207 : // Get the receiver entry for the requested bus and device ID
208 0 : static inline SrxlRcvrEntry* srxlGetReceiverEntry(uint8_t busIndex, uint8_t deviceID)
209 : {
210 0 : SrxlRcvrEntry* pRcvr = 0;
211 : uint8_t i;
212 0 : for(i = 0; i < srxlRx.rcvrCount; ++i)
213 : {
214 0 : if((srxlRx.rcvr[i].busBits & (1u << busIndex)) && (srxlRx.rcvr[i].deviceID == deviceID))
215 : {
216 0 : pRcvr = &srxlRx.rcvr[i];
217 0 : break;
218 : }
219 : }
220 0 : return pRcvr;
221 : }
222 :
223 : // Add a new receiver entry for the given device
224 0 : static inline SrxlRcvrEntry* srxlAddReceiverEntry(SrxlBus* pBus, SrxlDevEntry devEntry)
225 : {
226 : // Only allow receivers (or flight controllers in certain circumstances) to be added
227 0 : if(!pBus || devEntry.deviceID < 0x10 || devEntry.deviceID >= 0x40)
228 0 : return 0;
229 :
230 : // If we didn't previously add this receiver, add it now if we have room
231 0 : SrxlRcvrEntry* pRcvr = srxlGetReceiverEntry(pBus->fullID.busIndex, devEntry.deviceID);
232 0 : if(!pRcvr)
233 : {
234 0 : if(srxlRx.rcvrCount >= SRXL_MAX_RCVRS)
235 0 : return 0;
236 :
237 0 : uint8_t i = srxlRx.rcvrCount++;
238 0 : pRcvr = &srxlRx.rcvr[i];
239 0 : pRcvr->deviceID = devEntry.deviceID;
240 0 : pRcvr->busBits = (1u << pBus->fullID.busIndex);
241 0 : pRcvr->info = devEntry.info;
242 :
243 : // If this receiver is full-range, insert into our sorted list after the other full-range telemetry receivers
244 0 : if(pRcvr->info & SRXL_DEVINFO_TELEM_FULL_RANGE)
245 : {
246 : uint8_t n;
247 0 : for(n = i; n > srxlRx.rcvrSortInsert; --n)
248 0 : srxlRx.rcvrSorted[n] = srxlRx.rcvrSorted[n - 1];
249 0 : srxlRx.rcvrSorted[(srxlRx.rcvrSortInsert)++] = pRcvr;
250 : }
251 : // Else just tack onto the end
252 : else
253 : {
254 0 : srxlRx.rcvrSorted[i] = pRcvr;
255 : }
256 : }
257 :
258 : // If this new receiver is a base receiver that supports telemetry or we haven't set a default active telemetry receiver, set it
259 0 : if(!srxlRx.pTelemRcvr || (pRcvr->deviceID >= 0x20 && pRcvr->deviceID < 0x30 && (pRcvr->info & SRXL_DEVINFO_TELEM_TX_ENABLED)))
260 : {
261 0 : srxlRx.pTelemRcvr = pRcvr;
262 : }
263 :
264 0 : return pRcvr;
265 : }
266 :
267 : // Pick the best receiver to send telemetry on
268 0 : static inline SrxlRcvrEntry* srxlChooseTelemRcvr(void)
269 : {
270 : // If we only know about one receiver, set it to that
271 0 : if(srxlRx.rcvrCount == 1)
272 0 : return srxlRx.rcvrSorted[0];
273 :
274 : // If we were previously sending telemetry
275 0 : if(srxlRx.pTelemRcvr && srxlRx.pTelemRcvr->channelMask)
276 : {
277 : // If the current choice is not full-range
278 0 : if((srxlRx.pTelemRcvr->info & SRXL_DEVINFO_TELEM_FULL_RANGE) == 0)
279 : {
280 : // Then see if there is a full-range choice that received channel data to switch to
281 : uint8_t i;
282 0 : for(i = 0; i < srxlRx.rcvrSortInsert; ++i)
283 : {
284 0 : if(srxlRx.rcvrSorted[i]->channelMask)
285 0 : return srxlRx.rcvrSorted[i];
286 : }
287 : }
288 :
289 : // Else keep using the current receiver
290 0 : return srxlRx.pTelemRcvr;
291 : }
292 : // Else just pick the first one that got channel data this past frame
293 : else
294 : {
295 : uint8_t i;
296 0 : for(i = 0; i < srxlRx.rcvrCount; ++i)
297 : {
298 0 : if(srxlRx.rcvrSorted[i]->channelMask)
299 0 : return srxlRx.rcvrSorted[i];
300 : }
301 : }
302 :
303 0 : return 0;
304 : }
305 :
306 : // Return pointer to device entry matching the given ID, or NULL if not found
307 0 : static SrxlDevEntry* srxlGetDeviceEntry(SrxlBus* pBus, uint8_t deviceID)
308 : {
309 0 : if(pBus)
310 : {
311 : uint8_t i;
312 0 : for(i = 0; i < pBus->rxDevCount; ++i)
313 : {
314 0 : if(pBus->rxDev[i].deviceID == deviceID)
315 0 : return &(pBus->rxDev[i]);
316 : }
317 : }
318 0 : return 0;
319 : }
320 :
321 : // Add an entry to our list of devices found on the SRXL bus (or update an entry if it already exists)
322 0 : static SrxlDevEntry* srxlAddDeviceEntry(SrxlBus* pBus, SrxlDevEntry devEntry)
323 : {
324 : // Don't allow broadcast or unknown device types to be added
325 0 : if(!pBus || devEntry.deviceID < 0x10 || devEntry.deviceID > 0xEF)
326 0 : return 0;
327 :
328 : // Limit device priority
329 0 : if(devEntry.priority > 100)
330 0 : devEntry.priority = 100;
331 :
332 : // Update device entry if it already exists
333 0 : SrxlDevEntry* retVal = srxlGetDeviceEntry(pBus, devEntry.deviceID);
334 0 : if(retVal)
335 : {
336 0 : pBus->rxDevPrioritySum -= retVal->priority;
337 0 : *retVal = devEntry;
338 0 : pBus->rxDevPrioritySum += retVal->priority;
339 : }
340 : // Else add to the list if we have room
341 0 : else if(pBus->rxDevCount < SRXL_MAX_DEVICES)
342 : {
343 0 : retVal = &(pBus->rxDev[pBus->rxDevCount++]);
344 0 : *retVal = devEntry;
345 0 : pBus->rxDevPrioritySum += retVal->priority;
346 :
347 : #ifdef SRXL_INCLUDE_FWD_PGM_CODE
348 : // If the new device supports Forward Programming and is a base receiver or flight controller, choose as the default
349 : if(devEntry.info & SRXL_DEVINFO_FWD_PROG_SUPPORT)
350 : {
351 : uint8_t devType = devEntry.deviceID >> 4;
352 : if(devType > (srxlFwdPgmDevice.deviceID >> 4) && devType < SrxlDevType_ESC)
353 : {
354 : srxlFwdPgmDevice.deviceID = devEntry.deviceID;
355 : srxlFwdPgmDevice.busIndex = pBus->fullID.busIndex;
356 : }
357 : }
358 : #endif
359 :
360 : // If the new device is a receiver, add to our receiver list
361 0 : if(retVal->deviceID < 0x30)
362 : {
363 0 : srxlAddReceiverEntry(pBus, *retVal);
364 : }
365 : }
366 :
367 0 : return retVal;
368 : }
369 :
370 : /// PUBLIC FUNCTIONS ///
371 :
372 : /**
373 : @brief Initialize common SRXL info for this device
374 :
375 : @param deviceID: SRXL Device ID (see section 7.1.1 of SRXL2 Spec)
376 : @param priority: Requested telemetry priority (1-100; typical is 10 per unique message type)
377 : @param info: Device info bits (see SRXL_DEVINFO_XXX bits in spm_srxl.h)
378 : @param uid: Unique 32-bit id to avoid device ID collision during handshake
379 : @return bool: True if device info was successfully initialized
380 : */
381 0 : bool srxlInitDevice(uint8_t deviceID, uint8_t priority, uint8_t info, uint32_t uid)
382 : {
383 0 : if(deviceID < 0x10 || deviceID > 0xEF)
384 0 : return false;
385 :
386 0 : srxlThisDev.devEntry.deviceID = deviceID;
387 0 : srxlThisDev.devEntry.info = info;
388 0 : srxlThisDev.devEntry.priority = priority;
389 0 : srxlThisDev.devEntry.rfu = 0;
390 0 : srxlThisDev.uid = uid;
391 0 : srxlThisDev.vtxProxy = false;
392 :
393 : #ifdef SRXL_INCLUDE_MASTER_CODE
394 : // If this device is a receiver, add to our receiver info
395 : if(deviceID < 0x30)
396 : {
397 : srxlInitReceiver(deviceID, info);
398 : }
399 : #endif
400 :
401 : #ifdef SRXL_INCLUDE_FWD_PGM_CODE
402 : // If this device is a receiver or flight controller that supports Forward Programming, set as default
403 : if((info & SRXL_DEVINFO_FWD_PROG_SUPPORT) && deviceID < 0x40)
404 : {
405 : srxlFwdPgmDevice.deviceID = deviceID;
406 : srxlFwdPgmDevice.busIndex = 0;
407 : }
408 : #endif
409 :
410 : #if(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_STM_HW)
411 : // Enable the peripheral clock for the HW CRC engine
412 : RCC->AHBENR |= RCC_AHBENR_CRCEN;
413 : #endif
414 :
415 0 : return true;
416 : }
417 :
418 : /**
419 : @brief Initialize bus settings for the given SRXL bus
420 :
421 : @param busIndex: Index into srxlBus array of bus entries
422 : @param uart: Number to identify UART to which this SRXL bus should be connected
423 : @param baudSupported: 0 = 115200 baud, 1 = 400000 baud
424 : @return bool: True if SRXL bus was successfully initialized
425 : */
426 0 : bool srxlInitBus(uint8_t busIndex, uint8_t uart, uint8_t baudSupported)
427 : {
428 0 : if(busIndex >= SRXL_NUM_OF_BUSES || !srxlThisDev.devEntry.deviceID)
429 0 : return false;
430 :
431 0 : SrxlBus* pBus = &srxlBus[busIndex];
432 0 : pBus->state = SrxlState_ListenOnStartup;
433 0 : pBus->fullID.deviceID = srxlThisDev.devEntry.deviceID;
434 0 : pBus->fullID.busIndex = busIndex;
435 0 : pBus->rxDevCount = 0;
436 0 : pBus->rxDevPrioritySum = 0;
437 0 : pBus->requestID = (srxlThisDev.devEntry.deviceID == 0x10) ? 0x11 : 0;
438 0 : pBus->baudSupported = baudSupported;
439 0 : pBus->baudRate = SRXL_BAUD_115200;
440 0 : pBus->frameErrCount = 0;
441 0 : pBus->uart = uart;
442 : // Default remote receiver is automatically master -- everyone else figures it out during handshake
443 0 : pBus->master = (srxlThisDev.devEntry.deviceID == 0x10);
444 0 : pBus->pMasterRcvr = (srxlThisDev.devEntry.deviceID == 0x10) ? &srxlRx.rcvr[0] : 0;
445 0 : pBus->initialized = true;
446 :
447 0 : return true;
448 : }
449 :
450 : /**
451 : @brief See if this device is the bus master on the given bus
452 :
453 : @param busIndex: Index into srxlBus array for the desired SRXL bus
454 : @return bool: True if this device is the bus master on the given SRXL bus
455 : */
456 0 : bool srxlIsBusMaster(uint8_t busIndex)
457 : {
458 0 : return (busIndex < SRXL_NUM_OF_BUSES && srxlBus[busIndex].master);
459 : }
460 :
461 : /**
462 : @brief Get the current SRXL state machine timeout count for the given bus
463 :
464 : @param busIndex: Index into srxlBus array for the desired SRXL bus
465 : @return uint16_t: Timeout count in ms for the given SRXL bus
466 : */
467 0 : uint16_t srxlGetTimeoutCount_ms(uint8_t busIndex)
468 : {
469 0 : return (busIndex < SRXL_NUM_OF_BUSES) ? srxlBus[busIndex].timeoutCount_ms : 0;
470 : }
471 :
472 : /**
473 : @brief Get the Device ID of this device on the given bus
474 :
475 : @param busIndex: Index into srxlBus array for the desired SRXL bus
476 : @return uint8_t: Device ID of this device on the given SRXL bus
477 : */
478 0 : uint8_t srxlGetDeviceID(uint8_t busIndex)
479 : {
480 0 : return (busIndex < SRXL_NUM_OF_BUSES) ? srxlBus[busIndex].fullID.deviceID : 0;
481 : }
482 :
483 : /**
484 : @brief Internal send function called by srxlRun() -- do not call in user code
485 :
486 : @param pBus: Pointer to SRXL bus entry for the desired SRXL bus
487 : @param srxlCmd: Specific type of packet to send
488 : @param replyID: Device ID of the device this Send command is targeting
489 : */
490 0 : static void srxlSend(SrxlBus* pBus, SRXL_CMD srxlCmd, uint8_t replyID)
491 : {
492 0 : if(!pBus || !pBus->initialized)
493 0 : return;
494 :
495 0 : memset(pBus->srxlOut.raw, 0, SRXL_MAX_BUFFER_SIZE);
496 0 : pBus->srxlOut.header.srxlID = SPEKTRUM_SRXL_ID;
497 :
498 : // VTX Data
499 0 : if(srxlCmd == SRXL_CMD_VTX)
500 : {
501 0 : pBus->srxlOut.header.packetType = SRXL_CTRL_ID;
502 0 : pBus->srxlOut.header.length = SRXL_CTRL_BASE_LENGTH + sizeof(SrxlVtxData);
503 0 : pBus->srxlOut.control.payload.cmd = SRXL_CTRL_CMD_VTX;
504 0 : pBus->srxlOut.control.payload.replyID = replyID;
505 0 : pBus->srxlOut.control.payload.vtxData = srxlVtxData;
506 : }
507 : #ifdef SRXL_INCLUDE_MASTER_CODE
508 : // Channel Data
509 : else if(srxlCmd == SRXL_CMD_CHANNEL || srxlCmd == SRXL_CMD_CHANNEL_FS)
510 : {
511 : pBus->srxlOut.header.packetType = SRXL_CTRL_ID;
512 : uint32_t channelMask;
513 : if(srxlCmd == SRXL_CMD_CHANNEL)
514 : {
515 : pBus->srxlOut.control.payload.cmd = SRXL_CTRL_CMD_CHANNEL;
516 : pBus->srxlOut.control.payload.replyID = replyID;
517 :
518 : channelMask = srxlChData.mask;
519 : }
520 : else // == SRXL_CMD_CHANNEL_FS
521 : {
522 : // In failsafe mode, we dont want a telemetry reply
523 : pBus->srxlOut.control.payload.cmd = SRXL_CTRL_CMD_CHANNEL_FS;
524 : pBus->srxlOut.control.payload.replyID = 0;
525 :
526 : channelMask = srxlFailsafeChMask;
527 : }
528 :
529 : // Set signal quality info (only a bus master sends this, so assume srxlChData contains the latest values)
530 : pBus->srxlOut.control.payload.channelData.rssi = srxlChData.rssi;
531 : #ifdef SRXL_IS_HUB
532 : pBus->srxlOut.control.payload.channelData.frameLosses = srxlRx.frameLosses;
533 : #else
534 : pBus->srxlOut.control.payload.channelData.frameLosses = srxlRx.rcvr[0].fades;
535 : #endif
536 :
537 : uint8_t channelIndex = 0;
538 : uint32_t channelMaskBit = 1;
539 : for(uint8_t i = 0; i < 32; ++i, channelMaskBit <<= 1)
540 : {
541 : if(channelMask & channelMaskBit)
542 : {
543 : pBus->srxlOut.control.payload.channelData.values[channelIndex++] = srxlChData.values[i];
544 : }
545 : }
546 :
547 : // Set bits in packet for channels we populated, and clear those mask bits if it was part of a normal channel data command
548 : pBus->srxlOut.control.payload.channelData.mask = channelMask;
549 : if(srxlCmd == SRXL_CMD_CHANNEL)
550 : srxlChData.mask &= ~channelMask;
551 :
552 : pBus->srxlOut.header.length = SRXL_CTRL_BASE_LENGTH + 7 + (2 * channelIndex);
553 : }
554 : #ifdef SRXL_INCLUDE_FWD_PGM_CODE
555 : // Forward Programming Pass-thru
556 : else if(srxlCmd == SRXL_CMD_FWDPGM)
557 : {
558 : pBus->srxlOut.header.packetType = SRXL_CTRL_ID;
559 : pBus->srxlOut.header.length = SRXL_CTRL_BASE_LENGTH + 3 + srxlFwdPgmBufferLength;
560 : pBus->srxlOut.control.payload.cmd = SRXL_CTRL_CMD_FWDPGM;
561 : pBus->srxlOut.control.payload.replyID = replyID;
562 : memcpy(pBus->srxlOut.control.payload.fpData.data, srxlFwdPgmBuffer, srxlFwdPgmBufferLength);
563 : }
564 : #endif // SRXL_INCLUDE_FWD_PGM_CODE
565 : #endif // SRXL_INCLUDE_MASTER_CODE
566 : // RSSI Data
567 0 : else if(srxlCmd == SRXL_CMD_RSSI)
568 : {
569 0 : pBus->srxlOut.header.packetType = SRXL_RSSI_ID;
570 0 : pBus->srxlOut.header.length = sizeof(SrxlRssiPacket);
571 0 : pBus->srxlOut.rssi.request = SRXL_RSSI_REQ_REQUEST; // TODO: Needs to handle both directions!
572 0 : pBus->srxlOut.rssi.antennaA = 0; // TODO: Fill in actual data later
573 0 : pBus->srxlOut.rssi.antennaB = 0;
574 0 : pBus->srxlOut.rssi.antennaC = 0;
575 0 : pBus->srxlOut.rssi.antennaD = 0;
576 : }
577 0 : else if(srxlCmd == SRXL_CMD_HANDSHAKE)
578 : {
579 0 : pBus->srxlOut.header.packetType = SRXL_HANDSHAKE_ID;
580 0 : pBus->srxlOut.header.length = sizeof(SrxlHandshakePacket);
581 0 : pBus->srxlOut.handshake.payload.srcDevID = srxlThisDev.devEntry.deviceID;
582 0 : pBus->srxlOut.handshake.payload.destDevID = replyID;
583 0 : pBus->srxlOut.handshake.payload.priority = srxlThisDev.devEntry.priority;
584 0 : pBus->srxlOut.handshake.payload.baudSupported = pBus->baudSupported;
585 0 : pBus->srxlOut.handshake.payload.info = srxlThisDev.devEntry.info;
586 0 : pBus->srxlOut.handshake.payload.uid = srxlThisDev.uid;
587 : }
588 0 : else if(srxlCmd == SRXL_CMD_TELEMETRY)
589 : {
590 0 : srxlFillTelemetry(&pBus->srxlOut.telemetry.payload);
591 0 : pBus->srxlOut.header.packetType = SRXL_TELEM_ID;
592 0 : pBus->srxlOut.header.length = sizeof(SrxlTelemetryPacket);
593 : // If we successfully received a handshake from the bus master
594 0 : if(pBus->pMasterRcvr)
595 : {
596 : // If we know that a device on this bus should send it, then target that device
597 0 : if(srxlRx.pTelemRcvr && (srxlRx.pTelemRcvr->busBits & (1u << pBus->fullID.busIndex)))
598 0 : pBus->srxlOut.telemetry.destDevID = srxlRx.pTelemRcvr->deviceID;
599 : else
600 0 : pBus->srxlOut.telemetry.destDevID = 0;
601 : }
602 : else
603 : {
604 : // Send 0xFF to tell bus master to re-send the handshake so we know where to direct telemetry in the future
605 0 : pBus->srxlOut.telemetry.destDevID = 0xFF;
606 : }
607 :
608 : #ifdef SRXL_INCLUDE_MASTER_CODE
609 : if(srxlRx.pTelemRcvr && (pBus->srxlOut.telemetry.destDevID == srxlRx.pTelemRcvr->deviceID))
610 : {
611 : // Don't mark telemetry as having been sent if we are sending it ourself over RF
612 : if(pBus->srxlOut.telemetry.destDevID != srxlThisDev.pRcvr->deviceID)
613 : {
614 : srxlTelemetrySent();
615 : // Clear telemetry buffer after sending so we don't repeatedly display old data
616 : // srxlTelemData.sensorID = srxlTelemData.secondaryID = 0;
617 : }
618 : }
619 : #endif
620 : }
621 0 : else if(srxlCmd == SRXL_CMD_ENTER_BIND)
622 : {
623 0 : pBus->srxlOut.header.packetType = SRXL_BIND_ID;
624 0 : pBus->srxlOut.header.length = sizeof(SrxlBindPacket);
625 0 : pBus->srxlOut.bind.request = SRXL_BIND_REQ_ENTER;
626 0 : pBus->srxlOut.bind.deviceID = replyID;
627 0 : pBus->srxlOut.bind.data.type = DSMX_11MS;
628 0 : pBus->srxlOut.bind.data.options = (replyID != 0xFF) ? SRXL_BIND_OPT_TELEM_TX_ENABLE | SRXL_BIND_OPT_BIND_TX_ENABLE : 0;
629 0 : pBus->srxlOut.bind.data.guid = 0;
630 0 : pBus->srxlOut.bind.data.uid = 0;
631 : }
632 0 : else if(srxlCmd == SRXL_CMD_REQ_BIND)
633 : {
634 0 : pBus->srxlOut.header.packetType = SRXL_BIND_ID;
635 0 : pBus->srxlOut.header.length = sizeof(SrxlBindPacket);
636 0 : pBus->srxlOut.bind.request = SRXL_BIND_REQ_STATUS;
637 0 : pBus->srxlOut.bind.deviceID = replyID;
638 0 : memset(&(pBus->srxlOut.bind.data), 0, sizeof(SrxlBindData));
639 : }
640 0 : else if(srxlCmd == SRXL_CMD_SET_BIND)
641 : {
642 0 : pBus->srxlOut.header.packetType = SRXL_BIND_ID;
643 0 : pBus->srxlOut.header.length = sizeof(SrxlBindPacket);
644 0 : pBus->srxlOut.bind.request = SRXL_BIND_REQ_SET_BIND;
645 0 : pBus->srxlOut.bind.deviceID = replyID;
646 0 : pBus->srxlOut.bind.data = srxlBindInfo;
647 : }
648 0 : else if(srxlCmd == SRXL_CMD_BIND_INFO)
649 : {
650 0 : pBus->srxlOut.header.packetType = SRXL_BIND_ID;
651 0 : pBus->srxlOut.header.length = sizeof(SrxlBindPacket);
652 0 : pBus->srxlOut.bind.request = SRXL_BIND_REQ_BOUND_DATA;
653 0 : pBus->srxlOut.bind.deviceID = replyID;
654 0 : pBus->srxlOut.bind.data = srxlBindInfo;
655 : }
656 :
657 : // Compute CRC over entire SRXL packet (excluding the 2 CRC bytes at the end)
658 0 : uint16_t crc = srxlCrc16(pBus->srxlOut.raw);
659 :
660 : // Add CRC to packet in big-endian byte order
661 0 : pBus->srxlOut.raw[pBus->srxlOut.header.length - 2] = (crc >> 8) & 0xFF;
662 0 : pBus->srxlOut.raw[pBus->srxlOut.header.length - 1] = crc & 0xFF;
663 :
664 : // Send the packet out over the assigned UART
665 0 : srxlSendOnUart(pBus->uart, pBus->srxlOut.raw, pBus->srxlOut.header.length);
666 : }
667 :
668 : /**
669 : @brief Parse an SRXL packet received on the given SRXL bus UART
670 :
671 : @param busIndex: Index of SRXL bus state information entry in the srxlBus array
672 : @param packet: Pointer to received packet data
673 : @param length: Length in bytes of received packet data
674 : @return bool: True if a valid packet was received, else false
675 : */
676 0 : bool srxlParsePacket(uint8_t busIndex, uint8_t* packet, uint8_t length)
677 : {
678 : // Validate parameters
679 0 : if(busIndex >= SRXL_NUM_OF_BUSES || !packet || length < 5 || length > SRXL_MAX_BUFFER_SIZE)
680 0 : return false;
681 :
682 : // Validate SRXL ID and length
683 0 : if(packet[0] != SPEKTRUM_SRXL_ID || packet[2] != length)
684 0 : return false;
685 :
686 : // Validate checksum
687 0 : uint16_t crc = srxlCrc16(packet);
688 0 : if((((uint16_t)packet[length - 2] << 8) | packet[length - 1]) != crc)
689 0 : return false;
690 :
691 : // Copy packet into our unioned buffer to avoid "strict aliasing" violations
692 0 : SrxlBus* pBus = &srxlBus[busIndex];
693 0 : SrxlPacket* pRx = &(pBus->srxlIn);
694 0 : memcpy(pRx, packet, length);
695 :
696 : // Handle restart with ongoing communications -- bump to run state
697 0 : pBus->timeoutCount_ms = 0; // TODO: Should we clear this even if packet isn't valid?
698 0 : if(pBus->state < SrxlState_Running && pRx->header.packetType != SRXL_HANDSHAKE_ID)
699 0 : pBus->state = SrxlState_Running;
700 :
701 : // Parse the specific data
702 0 : switch(pRx->header.packetType)
703 : {
704 0 : case SRXL_CTRL_ID: // 0xCD
705 : {
706 0 : SrxlControlData* pCtrlData = &(pRx->control.payload);
707 :
708 : // Validate command
709 0 : if(pCtrlData->cmd > SRXL_CTRL_CMD_FWDPGM)
710 0 : break;
711 :
712 : // VTX
713 0 : if(pCtrlData->cmd == SRXL_CTRL_CMD_VTX)
714 : {
715 0 : if(srxlSetVtxData(&pCtrlData->vtxData))
716 0 : srxlOnVtx(&srxlVtxData);
717 0 : if(pCtrlData->replyID == pBus->fullID.deviceID || pCtrlData->replyID == 0xFF || srxlThisDev.vtxProxy)
718 : {
719 : // TODO: Should we ack this somehow
720 : }
721 : }
722 : #ifdef SRXL_INCLUDE_FWD_PGM_CODE
723 : // Forward Programming
724 : else if(pCtrlData->cmd == SRXL_CTRL_CMD_FWDPGM)
725 : {
726 : if(pCtrlData->replyID == pBus->fullID.deviceID)
727 : {
728 : memcpy(srxlFwdPgmBuffer, pCtrlData->fpData.data, pRx->header.length - 3 - SRXL_CTRL_BASE_LENGTH);
729 : srxlFwdPgmBufferLength = length;
730 : if(pCtrlData->replyID == srxlFwdPgmDevice.deviceID)
731 : {
732 : // Handle Forward Programming command locally
733 : srxlOnFwdPgm(srxlFwdPgmBuffer, srxlFwdPgmBufferLength);
734 : }
735 : else if(srxlFwdPgmDevice.deviceID && srxlBus[srxlFwdPgmDevice.busIndex].master)
736 : {
737 : // Pass it on through to the next target
738 : srxlBus[srxlFwdPgmDevice.busIndex].txFlags.sendFwdPgmData = 1;
739 : }
740 : }
741 : }
742 : #endif
743 : // Channel Data or Failsafe Data
744 : else
745 : {
746 0 : bool isFailsafe = (pCtrlData->cmd == SRXL_CTRL_CMD_CHANNEL_FS);
747 0 : srxlChData.rssi = pCtrlData->channelData.rssi;
748 0 : srxlChData.frameLosses = pCtrlData->channelData.frameLosses;
749 0 : if(pBus->pMasterRcvr)
750 : {
751 0 : if(pCtrlData->channelData.rssi < 0)
752 : {
753 0 : pBus->pMasterRcvr->rssi_dBm = pCtrlData->channelData.rssi;
754 0 : pBus->pMasterRcvr->rssiRcvd |= RSSI_RCVD_DBM;
755 : }
756 : else
757 : {
758 0 : pBus->pMasterRcvr->rssi_Pct = pCtrlData->channelData.rssi;
759 0 : pBus->pMasterRcvr->rssiRcvd |= RSSI_RCVD_PCT;
760 : }
761 : // If the receiver is sending alternating dBm/%, then use that as phase
762 0 : if(pBus->pMasterRcvr->rssiRcvd == RSSI_RCVD_BOTH)
763 : {
764 0 : srxlTelemetryPhase = pCtrlData->channelData.rssi >= 0;
765 : }
766 0 : pBus->pMasterRcvr->fades = pCtrlData->channelData.frameLosses;
767 0 : pBus->pMasterRcvr->channelMask = isFailsafe ? 0 : pCtrlData->channelData.mask;
768 0 : srxlRx.rxBusBits |= pBus->pMasterRcvr->busBits;
769 : }
770 :
771 : // Only save received channel values to srxlChData if it's normal channel data or we're in a hold condition
772 0 : if(!isFailsafe || !srxlRx.lossCountdown)
773 : {
774 0 : uint8_t channelIndex = 0;
775 0 : uint32_t channelMaskBit = 1;
776 : uint8_t i;
777 0 : for(i = 0; i < 32 && channelMaskBit <= pCtrlData->channelData.mask; ++i, channelMaskBit <<= 1)
778 : {
779 0 : if(pCtrlData->channelData.mask & channelMaskBit)
780 : {
781 0 : srxlChData.values[i] = pCtrlData->channelData.values[channelIndex++];
782 0 : srxlChData.mask |= channelMaskBit;
783 : }
784 : }
785 : }
786 :
787 0 : srxlChDataIsFailsafe = isFailsafe; // TODO: Can we still assume this???
788 0 : srxlReceivedChannelData(&(pCtrlData->channelData), isFailsafe);
789 :
790 : // Figure out what type of reply packet to send, if any
791 0 : if(pCtrlData->replyID == 0)
792 : {
793 0 : if(pBus->txFlags.enterBind)
794 : {
795 0 : pBus->state = SrxlState_SendEnterBind;
796 0 : pBus->txFlags.enterBind = 0;
797 : }
798 0 : else if(pBus->txFlags.setBindInfo)
799 : {
800 0 : if(srxlRx.pBindRcvr) // TODO: Double-check this logic
801 : {
802 0 : pBus->requestID = srxlRx.pBindRcvr->deviceID;
803 0 : pBus->state = SrxlState_SendSetBindInfo;
804 : }
805 0 : pBus->txFlags.setBindInfo = 0;
806 : }
807 0 : else if(pBus->txFlags.broadcastBindInfo)
808 : {
809 0 : pBus->requestID = 0xFF;
810 0 : pBus->state = SrxlState_SendSetBindInfo;
811 0 : pBus->txFlags.broadcastBindInfo = 0;
812 : }
813 : }
814 0 : else if(pCtrlData->replyID == pBus->fullID.deviceID)
815 : {
816 0 : pBus->state = SrxlState_SendTelemetry;
817 : }
818 : }
819 0 : break;
820 : }
821 0 : case SRXL_HANDSHAKE_ID: // 0x21
822 : {
823 0 : if(length < sizeof(SrxlHandshakePacket))
824 0 : return false;
825 :
826 : // If this is an unprompted handshake (dest == 0) from a higher device ID, then we're the master
827 0 : SrxlHandshakeData* pHandshake = &(pRx->handshake.payload);
828 0 : if((pHandshake->destDevID == 0) && (pHandshake->srcDevID > pBus->fullID.deviceID))
829 : {
830 : // Send a reply immediately to get the slave to shut up
831 0 : pBus->state = SrxlState_SendHandshake;
832 0 : pBus->requestID = pHandshake->srcDevID;
833 0 : pBus->baudSupported = SRXL_SUPPORTED_BAUD_RATES;
834 0 : srxlRun(busIndex, 0);
835 0 : pBus->requestID = pBus->fullID.deviceID;
836 0 : pBus->state = SrxlState_SendHandshake;
837 0 : pBus->master = true;
838 0 : pBus->pMasterRcvr = &srxlRx.rcvr[0];
839 : }
840 :
841 : // Add this device to our list of discovered devices
842 0 : SrxlDevEntry newDev = {.deviceID = pHandshake->srcDevID, .priority = pHandshake->priority, .info = pHandshake->info};
843 0 : srxlAddDeviceEntry(pBus, newDev);
844 :
845 : // Bus master needs to track responses and poll next device
846 0 : if(pBus->master)
847 : {
848 : // Keep track of baud rates supported
849 0 : pBus->baudSupported &= pHandshake->baudSupported;
850 :
851 : // Make sure the state machine is set to poll -- this will eventually reach broadcast address (0xFF)
852 0 : pBus->state = SrxlState_SendHandshake;
853 : }
854 : // Broadcast handshake sets the agreed upon baud rate for this bus
855 0 : else if(pHandshake->destDevID == 0xFF)
856 : {
857 : // Get bus master receiver entry (if it's a flight controller, add it now as a receiver on this bus)
858 0 : if(newDev.deviceID >= 0x30 && newDev.deviceID < 0x40)
859 : {
860 0 : pBus->pMasterRcvr = srxlAddReceiverEntry(pBus, newDev);
861 : }
862 : else
863 : {
864 0 : pBus->pMasterRcvr = srxlGetReceiverEntry(busIndex, newDev.deviceID);
865 : }
866 :
867 : // Set baud rate and advance to run state
868 0 : pBus->baudRate = pHandshake->baudSupported;
869 0 : if(pBus->baudRate & SRXL_BAUD_400000)
870 : {
871 0 : srxlChangeBaudRate(pBus->uart, 400000); // Only alternate rate supported for now...
872 : }
873 0 : pBus->state = SrxlState_Running;
874 : }
875 : // Normal Handshake destined for this device should be replied to -- else ignore
876 : else
877 : {
878 0 : if(pHandshake->destDevID == pBus->fullID.deviceID && pBus->state != SrxlState_SendHandshake)
879 : {
880 0 : pBus->requestID = pHandshake->srcDevID;
881 0 : pBus->state = SrxlState_SendHandshake;
882 : }
883 : else
884 : {
885 0 : pBus->state = SrxlState_ListenForHandshake;
886 : }
887 : }
888 :
889 0 : break;
890 : }
891 0 : case SRXL_PARAM_ID: // 0x50
892 : {
893 : // TODO: Add later
894 0 : break;
895 : }
896 0 : case SRXL_RSSI_ID: // 0x55
897 : {
898 : // TODO: Add later
899 0 : break;
900 : }
901 0 : case SRXL_BIND_ID: // 0x41
902 : {
903 0 : if(length < sizeof(SrxlBindPacket))
904 0 : return false;
905 :
906 0 : SrxlBindPacket* pBindInfo = &(pRx->bind);
907 :
908 : // If this is a bound data report
909 0 : if(pBindInfo->request == SRXL_BIND_REQ_BOUND_DATA)
910 : {
911 : // Call the user-defined callback -- if returns true, bind all other receivers
912 : SrxlFullID boundID;
913 0 : boundID.deviceID = pBindInfo->deviceID;
914 0 : boundID.busIndex = busIndex;
915 0 : if(srxlOnBind(boundID, pBindInfo->data))
916 : {
917 : // Update the bind info
918 0 : srxlBindInfo.type = pBindInfo->data.type;
919 0 : if(pBindInfo->data.options & SRXL_BIND_OPT_TELEM_TX_ENABLE)
920 : {
921 0 : SrxlRcvrEntry* pNewTelem = srxlGetReceiverEntry(busIndex, pBindInfo->deviceID);
922 0 : if(pNewTelem || !srxlRx.pTelemRcvr)
923 0 : srxlRx.pTelemRcvr = pNewTelem;
924 : }
925 0 : srxlBindInfo.options = 0; // Disable telemetry and Discovery reply when setting other receivers
926 0 : srxlBindInfo.guid = pBindInfo->data.guid;
927 0 : srxlBindInfo.uid = pBindInfo->data.uid;
928 :
929 : // Try to set bind info for all other receivers on other buses to match it
930 : uint8_t b;
931 0 : for(b = 0; b < SRXL_NUM_OF_BUSES; ++b)
932 : {
933 0 : if(b == busIndex)
934 0 : continue;
935 0 : srxlBus[b].txFlags.broadcastBindInfo = 1;
936 : }
937 : }
938 : }
939 : // If this bind packet is directed at us
940 0 : else if(pBindInfo->deviceID == pBus->fullID.deviceID || pBindInfo->deviceID == 0xFF)
941 : {
942 : // Check for Enter Bind Mode (only valid if sent to a specific receiver)
943 0 : if(pBindInfo->request == SRXL_BIND_REQ_ENTER)
944 : {
945 : #ifdef SRXL_INCLUDE_MASTER_CODE
946 : srxlBindInfo.type = pBindInfo->data.type;
947 : srxlBindInfo.options = pBindInfo->data.options;
948 : srxlBindInfo.guid = 0;
949 : srxlBindInfo.uid = 0;
950 : srxlTryToBind(srxlBindInfo);
951 : #endif
952 : }
953 0 : else if(pBindInfo->request == SRXL_BIND_REQ_STATUS && srxlThisDev.pRcvr)
954 : {
955 : // TODO: Fill in data if we didn't just bind?
956 0 : pBus->txFlags.reportBindInfo = 1;
957 : }
958 : // Handle set bind info request
959 0 : else if(pBindInfo->request == SRXL_BIND_REQ_SET_BIND)
960 : {
961 0 : srxlBindInfo = pBindInfo->data;
962 : #ifdef SRXL_INCLUDE_MASTER_CODE
963 : if(pBus->fullID.deviceID < 0x30)
964 : srxlTryToBind(srxlBindInfo);
965 : #endif
966 : }
967 : }
968 :
969 0 : break;
970 : }
971 0 : case SRXL_TELEM_ID: // 0x80
972 : {
973 0 : if(length < sizeof(SrxlTelemetryPacket))
974 0 : return false;
975 :
976 : // NOTE: This data should be sent by exactly one telemetry device in response to a bus master request,
977 : // so it is safe to update the global pTelemRcvr here even though this is a bus-specific function.
978 :
979 0 : SrxlTelemetryPacket* pTelem = &(pRx->telemetry);
980 0 : memcpy(&srxlTelemData, &pTelem->payload, sizeof(srxlTelemData));
981 : // If the telemetry destination is set to broadcast, that indicates a request to re-handshake
982 0 : if(pBus->master && pTelem->destDevID == 0xFF)
983 : {
984 : // If the master only found one device, don't poll again -- just tell the requesting device who we are
985 0 : pBus->requestID = pBus->rxDevCount > 1 ? pBus->fullID.deviceID : 0xFF;
986 0 : pBus->state = SrxlState_SendHandshake;
987 : }
988 : // If the incoming telemetry is destined for us, then we need to figure out who should send it over RF
989 0 : else if(pTelem->destDevID == pBus->fullID.deviceID)
990 : {
991 : // This needs different logic for hubs versus endpoints
992 : #ifdef SRXL_IS_HUB
993 : if(srxlRx.pTelemRcvr == 0)
994 : {
995 : srxlRx.pTelemRcvr = srxlChooseTelemRcvr();
996 : }
997 : #else
998 0 : srxlRx.pTelemRcvr = srxlThisDev.pRcvr;
999 : #endif
1000 : // Enable this device's telemetry tx based on whether we are the chosen telemetry receiver
1001 : #ifdef SRXL_INCLUDE_MASTER_CODE
1002 : srxlSetTelemetryTxEnable(srxlRx.pTelemRcvr && (srxlRx.pTelemRcvr == srxlThisDev.pRcvr));
1003 : #endif
1004 : }
1005 : // Else turn off our telemetry and that of any receivers we might reply to via our own telemetry
1006 : else
1007 : {
1008 0 : srxlRx.pTelemRcvr = 0;
1009 : #ifdef SRXL_INCLUDE_MASTER_CODE
1010 : srxlSetTelemetryTxEnable(false);
1011 : #endif
1012 : }
1013 :
1014 0 : srxlTelemSuppressCount = 0;
1015 : #ifdef SRXL_INCLUDE_MASTER_CODE
1016 : srxlSuppressInternalTelemetry(&pTelem->payload);
1017 : #endif
1018 0 : break;
1019 : }
1020 0 : default:
1021 : {
1022 0 : break;
1023 : }
1024 : }
1025 :
1026 : // Run state machine for slave devices after each received packet
1027 0 : if(!pBus->master)
1028 : {
1029 0 : srxlRun(busIndex, 0);
1030 : }
1031 :
1032 0 : return true;
1033 : }
1034 :
1035 : /**
1036 : @brief Run the SRXL state machine after each receive or rx timeout
1037 :
1038 : @param busIndex: Index of SRXL bus state information entry in the srxlBus array
1039 : @param timeoutDelta_ms: Number of milliseconds to increment receive timeout if a timeout
1040 : occured, or <= 0 to clear timeout count upon packet receive.
1041 : */
1042 0 : void srxlRun(uint8_t busIndex, int16_t timeoutDelta_ms)
1043 : {
1044 0 : SrxlBus* pBus = &srxlBus[busIndex];
1045 0 : if(busIndex >= SRXL_NUM_OF_BUSES || !pBus->initialized || pBus->state == SrxlState_Disabled)
1046 0 : return;
1047 :
1048 : // Check receive timeout and advance state if needed
1049 0 : if(timeoutDelta_ms > 0)
1050 : {
1051 0 : pBus->timeoutCount_ms += timeoutDelta_ms;
1052 0 : if(pBus->timeoutCount_ms >= 30000)
1053 0 : pBus->timeoutCount_ms = 30000;
1054 :
1055 0 : if(pBus->timeoutCount_ms >= 50)
1056 : {
1057 : // After startup delay of 50ms, switch to handshake send or listen based on device unit ID
1058 0 : if(pBus->state == SrxlState_ListenOnStartup)
1059 : {
1060 0 : pBus->state = (pBus->fullID.deviceID & 0x0F) ? SrxlState_ListenForHandshake : SrxlState_SendHandshake;
1061 : }
1062 : // Reset non-master device back to startup conditions if 50ms elapses with no communications
1063 0 : else if(!pBus->master && pBus->state >= SrxlState_Running)
1064 : {
1065 0 : srxlChangeBaudRate(pBus->uart, 115200);
1066 0 : pBus->baudRate = SRXL_BAUD_115200;
1067 0 : pBus->timeoutCount_ms = 0;
1068 0 : pBus->requestID = 0; // Change back to 0 to indicate unprompted handshake from slave device
1069 0 : if(pBus->pMasterRcvr)
1070 : {
1071 0 : pBus->pMasterRcvr->channelMask = 0;
1072 0 : pBus->pMasterRcvr->fades = 0xFFFF;
1073 0 : pBus->pMasterRcvr->rssi_Pct = 0;
1074 0 : pBus->pMasterRcvr->rssi_dBm = -1;
1075 : }
1076 0 : pBus->state = SrxlState_ListenOnStartup;
1077 : }
1078 : }
1079 : }
1080 : else
1081 : {
1082 0 : pBus->timeoutCount_ms = 0;
1083 : }
1084 :
1085 0 : if(!pBus->master)
1086 : {
1087 : // Non-master actions for the given state
1088 0 : switch(pBus->state)
1089 : {
1090 0 : case SrxlState_SendHandshake:
1091 : {
1092 0 : srxlSend(pBus, SRXL_CMD_HANDSHAKE, pBus->requestID);
1093 0 : break;
1094 : }
1095 0 : case SrxlState_SendTelemetry:
1096 : {
1097 0 : srxlSend(pBus, SRXL_CMD_TELEMETRY, pBus->requestID);
1098 0 : pBus->state = SrxlState_Running;
1099 0 : break;
1100 : }
1101 0 : case SrxlState_SendVTX:
1102 : {
1103 0 : srxlSend(pBus, SRXL_CMD_VTX, pBus->requestID);
1104 0 : pBus->state = SrxlState_Running;
1105 0 : break;
1106 : }
1107 0 : case SrxlState_SendEnterBind:
1108 : {
1109 0 : if(srxlRx.pBindRcvr && (srxlRx.pBindRcvr != srxlThisDev.pRcvr))
1110 : {
1111 0 : srxlSend(pBus, SRXL_CMD_ENTER_BIND, srxlRx.pBindRcvr->deviceID);
1112 : }
1113 : else
1114 : {
1115 0 : srxlBindInfo.options = 0;
1116 0 : srxlSend(pBus, SRXL_CMD_ENTER_BIND, 0xFF);
1117 : }
1118 0 : pBus->state = SrxlState_Running;
1119 0 : break;
1120 : }
1121 0 : case SrxlState_SendSetBindInfo:
1122 : {
1123 0 : srxlSend(pBus, SRXL_CMD_SET_BIND, pBus->requestID);
1124 0 : pBus->state = SrxlState_Running;
1125 0 : break;
1126 : }
1127 0 : case SrxlState_SendBoundDataReport:
1128 : {
1129 0 : srxlSend(pBus, SRXL_CMD_BIND_INFO, pBus->fullID.deviceID);
1130 0 : pBus->state = SrxlState_Running;
1131 0 : break;
1132 : }
1133 0 : case SrxlState_Running:
1134 : default:
1135 : {
1136 0 : return;
1137 : }
1138 : }
1139 : }
1140 : #ifdef SRXL_INCLUDE_MASTER_CODE
1141 : else
1142 : {
1143 : srxlRunMaster(pBus);
1144 : }
1145 : #endif // SRXL_INCLUDE_MASTER_CODE
1146 : }
1147 :
1148 : /**
1149 : @brief Tell the "best" receiver to enter bind mode, either locally or via SRXL command
1150 :
1151 : @param bindType: One of the possible bind status types to use when binding -- NOTE: The transmitter may ignore this
1152 : @param broadcast: True if this is a local request that should tell all connected receivers to enter bind
1153 : @return bool: True if a receiver was told to enter bind mode; else false
1154 : */
1155 0 : bool srxlEnterBind(uint8_t bindType, bool broadcast)
1156 : {
1157 0 : srxlRx.pBindRcvr = 0;
1158 0 : if(broadcast && srxlThisDev.pRcvr)
1159 : {
1160 0 : srxlRx.pBindRcvr = srxlThisDev.pRcvr;
1161 : }
1162 0 : else if(srxlRx.pTelemRcvr)
1163 : {
1164 0 : srxlRx.pBindRcvr = srxlRx.pTelemRcvr;
1165 : }
1166 0 : else if(srxlRx.rcvrCount > 0 && srxlRx.rcvrSorted[0]->deviceID < 0x30)
1167 : {
1168 0 : srxlRx.pBindRcvr = srxlRx.rcvrSorted[0];
1169 : }
1170 :
1171 0 : if(srxlRx.pBindRcvr)
1172 : {
1173 : #ifdef SRXL_INCLUDE_MASTER_CODE
1174 : // Local bind
1175 : if(srxlRx.pBindRcvr == srxlThisDev.pRcvr)
1176 : {
1177 : srxlBindInfo.type = bindType;
1178 : srxlBindInfo.options = SRXL_BIND_OPT_BIND_TX_ENABLE;
1179 : srxlBindInfo.guid = 0;
1180 : srxlBindInfo.uid = 0;
1181 : if(srxlRx.pBindRcvr == srxlRx.pTelemRcvr)
1182 : srxlBindInfo.options |= SRXL_BIND_OPT_TELEM_TX_ENABLE;
1183 : srxlTryToBind(srxlBindInfo);
1184 : if(broadcast)
1185 : {
1186 : uint8_t b;
1187 : for(b = 0; b < SRXL_NUM_OF_BUSES; ++b)
1188 : {
1189 : srxlBus[b].txFlags.enterBind = 1;
1190 : }
1191 : }
1192 : return true;
1193 : }
1194 : #endif // SRXL_INCLUDE_MASTER_CODE
1195 :
1196 : // Remote bind
1197 : uint8_t i;
1198 0 : for(i = 0; i < SRXL_NUM_OF_BUSES; ++i)
1199 : {
1200 0 : if((1u << i) & srxlRx.pBindRcvr->busBits)
1201 : {
1202 0 : srxlBus[i].txFlags.enterBind = 1;
1203 0 : return true;
1204 : }
1205 : }
1206 : }
1207 :
1208 0 : return false;
1209 : }
1210 :
1211 : /**
1212 : @brief Public function to set bind info for the system, either locally or via SRXL commands
1213 :
1214 : @param bindType: Type of bind requested for this receiver or all receivers
1215 : @param guid: Transmitter GUID to bind the receiver to
1216 : @param uid: Unique ID provided by transmitter upon initial bind (can be 0 if unknown)
1217 : @return bool: True if bind info was successfully set for the destination device; else false
1218 : */
1219 0 : bool srxlSetBindInfo(uint8_t bindType, uint64_t guid, uint32_t uid)
1220 : {
1221 0 : if(guid == 0)
1222 0 : return false;
1223 :
1224 : // Set bind info, with options defaulted to 0
1225 0 : srxlBindInfo.type = bindType;
1226 0 : srxlBindInfo.options = SRXL_BIND_OPT_US_POWER; // Request US power, with no guarantee it's supported
1227 0 : srxlBindInfo.guid = guid;
1228 0 : srxlBindInfo.uid = uid ? uid : srxlThisDev.uid;
1229 :
1230 : #ifdef SRXL_INCLUDE_MASTER_CODE
1231 : // If we are a receiver
1232 : if(srxlThisDev.pRcvr)
1233 : {
1234 : // Bind locally, which will result in no further packets since options == 0
1235 : srxlTryToBind(srxlBindInfo);
1236 : }
1237 : #endif
1238 :
1239 : // Broadcast this bind info on all SRXL buses
1240 : uint8_t b;
1241 0 : for(b = 0; b < SRXL_NUM_OF_BUSES; ++b)
1242 : {
1243 0 : srxlBus[b].txFlags.broadcastBindInfo = 1;
1244 : }
1245 :
1246 0 : return true;
1247 : }
1248 :
1249 : /**
1250 : @brief Public function to call from the user UART code when a UART frame error occurs
1251 :
1252 : @param busIndex: Index of SRXL bus state information entry in the srxlBus array
1253 : */
1254 0 : void srxlOnFrameError(uint8_t busIndex)
1255 : {
1256 0 : SrxlBus* pBus = &srxlBus[busIndex];
1257 0 : if(busIndex >= SRXL_NUM_OF_BUSES || !pBus->initialized)
1258 0 : return;
1259 :
1260 0 : ++(pBus->frameErrCount);
1261 0 : if(pBus->master)
1262 : {
1263 : // TODO: If master (i.e. remote receiver 0x10), cause break condition to force reset?
1264 0 : return;
1265 : }
1266 :
1267 0 : if(pBus->state == SrxlState_ListenOnStartup || pBus->state == SrxlState_ListenForHandshake)
1268 : {
1269 : // Wait for multiple frame breaks before trying to change baud rate
1270 0 : if(pBus->frameErrCount < 3)
1271 0 : return;
1272 :
1273 : // Try the next higher baud rate
1274 0 : switch(pBus->baudRate)
1275 : {
1276 0 : case SRXL_BAUD_115200:
1277 : {
1278 0 : if(pBus->baudSupported & SRXL_BAUD_400000)
1279 : {
1280 0 : srxlChangeBaudRate(pBus->uart, 400000);
1281 0 : pBus->baudRate = SRXL_BAUD_400000;
1282 0 : break;
1283 : }
1284 : FALLTHROUGH;
1285 : // else fall thru...
1286 : }
1287 : case SRXL_BAUD_400000:
1288 : default:
1289 : {
1290 : // TODO: Cause break condition to force reset of everyone? Or just keep cycling?
1291 :
1292 0 : srxlChangeBaudRate(pBus->uart, 115200);
1293 0 : pBus->baudRate = SRXL_BAUD_115200;
1294 0 : break;
1295 : }
1296 : }
1297 0 : pBus->frameErrCount = 0;
1298 : }
1299 : else
1300 : {
1301 : // TODO: Handle frame error during normal comm -- probably caused by collision on bus?
1302 : }
1303 : }
1304 :
1305 0 : SrxlFullID srxlGetTelemetryEndpoint(void)
1306 : {
1307 0 : SrxlFullID retVal = {{0}};
1308 0 : if(srxlRx.pTelemRcvr)
1309 : {
1310 0 : retVal.deviceID = srxlRx.pTelemRcvr->deviceID;
1311 0 : retVal.busIndex = srxlRx.pTelemRcvr->busBits;
1312 : }
1313 0 : return retVal;
1314 : }
1315 :
1316 0 : bool srxlSetVtxData(SrxlVtxData* pVtxData)
1317 : {
1318 0 : if(!pVtxData)
1319 0 : return false;
1320 :
1321 : // Update VTX data, ignoring values marked as unchanged
1322 0 : if(pVtxData->band != 0xFF)
1323 0 : srxlVtxData.band = pVtxData->band;
1324 0 : if(pVtxData->channel != 0xFF)
1325 0 : srxlVtxData.channel = pVtxData->channel;
1326 0 : if(pVtxData->pit != 0xFF)
1327 0 : srxlVtxData.pit = pVtxData->pit;
1328 0 : if(pVtxData->power != 0xFF || pVtxData->powerDec != 0xFFFF)
1329 0 : srxlVtxData.power = pVtxData->power;
1330 0 : if(pVtxData->powerDec != 0xFFFF)
1331 0 : srxlVtxData.powerDec = pVtxData->powerDec;
1332 0 : if(pVtxData->region != 0xFF)
1333 0 : srxlVtxData.region = pVtxData->region;
1334 :
1335 : uint8_t b;
1336 0 : for(b = 0; b < SRXL_NUM_OF_BUSES; ++b)
1337 : {
1338 0 : srxlBus[b].txFlags.sendVtxData = 1;
1339 : }
1340 :
1341 0 : return true;
1342 : }
1343 :
1344 0 : void srxlSetHoldThreshold(uint8_t countdownReset)
1345 : {
1346 0 : srxlRx.lossHoldCount = (countdownReset > 1) ? countdownReset : 45;
1347 0 : }
1348 :
1349 0 : void srxlClearCommStats(void)
1350 : {
1351 0 : srxlRx.holds = 0;
1352 0 : srxlRx.frameLosses = 0;
1353 0 : srxlRx.lossCountdown = srxlRx.lossHoldCount + 1;
1354 0 : }
1355 :
1356 : // Return true on failsafe hold
1357 0 : bool srxlUpdateCommStats(bool isFade)
1358 : {
1359 0 : srxlRx.rxBusBits = 0;
1360 0 : if(srxlTelemetryPhase)
1361 : {
1362 0 : srxlRx.bestRssi_dBm = -128;
1363 0 : srxlRx.bestRssi_Pct = 0;
1364 : }
1365 :
1366 : uint8_t i;
1367 0 : for(i = 0; i < srxlRx.rcvrCount; ++i)
1368 : {
1369 0 : if(srxlRx.rcvr[i].channelMask)
1370 : {
1371 0 : srxlRx.lossCountdown = srxlRx.lossHoldCount + 1;
1372 :
1373 0 : if((srxlRx.rcvr[i].rssiRcvd & RSSI_RCVD_DBM) && srxlRx.bestRssi_dBm < srxlRx.rcvr[i].rssi_dBm)
1374 0 : srxlRx.bestRssi_dBm = srxlRx.rcvr[i].rssi_dBm;
1375 0 : if((srxlRx.rcvr[i].rssiRcvd & RSSI_RCVD_PCT) && srxlRx.bestRssi_dBm < srxlRx.rcvr[i].rssi_Pct)
1376 0 : srxlRx.bestRssi_Pct = srxlRx.rcvr[i].rssi_Pct;
1377 : }
1378 : }
1379 :
1380 : // Set RSSI based on telemetry phase and type of telemetry received
1381 0 : srxlChData.rssi = (srxlTelemetryPhase || srxlChDataIsFailsafe) ? srxlRx.bestRssi_Pct : srxlRx.bestRssi_dBm;
1382 :
1383 : // Update flight log frame losses and holds
1384 0 : if(isFade && srxlRx.lossCountdown)
1385 : {
1386 0 : if(--srxlRx.lossCountdown == 0)
1387 : {
1388 0 : ++srxlRx.holds;
1389 0 : srxlRx.frameLosses -= srxlRx.lossHoldCount;
1390 : }
1391 : else
1392 : {
1393 0 : ++srxlRx.frameLosses;
1394 : }
1395 : }
1396 :
1397 : static uint8_t telemFadeCount = 0;
1398 : // If we are allowed to send telemetry by the device downstream (i.e. any slave device)
1399 0 : if(srxlRx.pTelemRcvr)
1400 : {
1401 0 : if(srxlRx.pTelemRcvr->channelMask == 0 || (srxlRx.pTelemRcvr->info & SRXL_DEVINFO_TELEM_FULL_RANGE) == 0)
1402 : {
1403 : // If our telemetry receiver missed channel data 3 frames in a row, switch
1404 0 : if(++telemFadeCount > 3)
1405 : {
1406 0 : srxlRx.pTelemRcvr = srxlChooseTelemRcvr();
1407 : #ifdef SRXL_INCLUDE_MASTER_CODE
1408 : srxlSetTelemetryTxEnable(srxlRx.pTelemRcvr && (srxlRx.pTelemRcvr == srxlThisDev.pRcvr));
1409 : #endif
1410 0 : telemFadeCount = 0;
1411 : }
1412 : }
1413 : else
1414 : {
1415 0 : telemFadeCount = 0;
1416 : }
1417 : }
1418 : #ifdef SRXL_INCLUDE_MASTER_CODE
1419 : // Else check to make sure we're still supposed to suppress telemetry (reset countdown when slave tells us not to send again)
1420 : else if(++srxlTelemSuppressCount > SRXL_TELEM_SUPPRESS_MAX)
1421 : {
1422 : // Enable this device's telemetry tx since we stopped being told not to
1423 : srxlRx.pTelemRcvr = srxlThisDev.pRcvr;
1424 : srxlSetTelemetryTxEnable(srxlRx.pTelemRcvr);
1425 : }
1426 : #endif
1427 :
1428 : // Return true while we're in hold condition (failsafe)
1429 0 : return srxlRx.lossCountdown == 0;
1430 : }
|
