diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp index 75e84ade9a701..3c34085cb27e3 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp @@ -29,7 +29,10 @@ // for video TX configuration: #if AP_VIDEOTX_ENABLED #include +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wc99-designator" #include "spm_srxl.h" +#pragma clang diagnostic pop #endif diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp index f0f7cafdea6af..3cef04f9de671 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp @@ -29,7 +29,10 @@ #include #include +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wc99-designator" #include "spm_srxl.h" +#pragma clang diagnostic pop extern const AP_HAL::HAL& hal; //#define SRXL2_DEBUG diff --git a/libraries/AP_RCProtocol/spm_srxl.cpp b/libraries/AP_RCProtocol/spm_srxl.cpp index 8dfcf495b58d2..eb6f5af506a9b 100644 --- a/libraries/AP_RCProtocol/spm_srxl.cpp +++ b/libraries/AP_RCProtocol/spm_srxl.cpp @@ -1,1430 +1,7 @@ -/* -MIT License - -Copyright (c) 2019 Horizon Hobby, LLC - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -*/ - -#include -#include - -#include "spm_srxl.h" - -/// LOCAL TYPES AND CONSTANTS /// - -#if(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_SPEED) -const uint16_t srxlCRCTable[] = -{ - 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, - 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, - 0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, - 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE, - 0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, - 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D, - 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, - 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC, - - 0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, - 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B, - 0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, - 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, - 0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, - 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49, - 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, - 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78, - - 0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, - 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067, - 0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, - 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256, - 0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, - 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, - 0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, - 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634, - - 0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, - 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3, - 0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, - 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92, - 0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, - 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1, - 0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, - 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0 -}; -#endif - -#define SRXL_TELEM_SUPPRESS_MAX (100) - -/// PUBLIC VARIABLES /// - -SrxlChannelData srxlChData = {0, 0, 0, {0}}; -SrxlTelemetryData srxlTelemData = {0}; -SrxlVtxData srxlVtxData = {0, 0, 1, 0, 0, 1}; - -/// LOCAL VARIABLES /// - -static SrxlDevice srxlThisDev = {0}; -static SrxlBus srxlBus[SRXL_NUM_OF_BUSES]; -static bool srxlChDataIsFailsafe = false; -static bool srxlTelemetryPhase = false; -#ifdef SRXL_INCLUDE_MASTER_CODE -static uint32_t srxlFailsafeChMask = 0; // Tracks all active channels for use during failsafe transmission -#endif -static SrxlBindData srxlBindInfo = {0, 0, 0, 0}; -static SrxlReceiverStats srxlRx = {0}; -static uint16_t srxlTelemSuppressCount = 0; - -#ifdef SRXL_INCLUDE_FWD_PGM_CODE -static SrxlFullID srxlFwdPgmDevice = {0, 0}; // Device that should accept Forward Programming connection by default -static uint8_t srxlFwdPgmBuffer[FWD_PGM_MAX_DATA_SIZE] = {0}; -static uint8_t srxlFwdPgmBufferLength = 0; -#endif - -// Include additional header and externs if using STM32 hardware acceleration -#if(SRXL_CRC_OPTIMIZE_MODE > SRXL_CRC_OPTIMIZE_SIZE) -#ifdef __cplusplus -extern "C" -{ -#endif -#if(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_STM_HAL) -#if(SRXL_STM_TARGET_FAMILY == SRXL_STM_TARGET_F7) -#include "stm32f7xx_hal.h" -#else -#include "stm32f3xx_hal.h" // Default to F3 if not given -#endif - extern CRC_HandleTypeDef hcrc; -#else -#if(SRXL_STM_TARGET_FAMILY == SRXL_STM_TARGET_F7) -#error "STM32F7 targets not yet supported for register-based STM HW optimization" -#else -#include "stm32f30x.h" // Default to F3 if not given -#endif -#endif -#ifdef __cplusplus -} -#endif -#endif - -/// LOCAL HELPER FUNCTIONS /// - -// Compute SRXL CRC over packet buffer (assumes length is correctly set) -static uint16_t srxlCrc16(uint8_t* packet) -{ - uint16_t crc = 0; // Seed with 0 - uint8_t length = packet[2] - 2; // Exclude 2 CRC bytes at end of packet from the length - - if(length <= SRXL_MAX_BUFFER_SIZE - 2) - { -#if(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_SIZE) - // Use bitwise method - for(uint8_t i = 0; i < length; ++i) - { - crc = crc ^ ((uint16_t)packet[i] << 8); - for(int b = 0; b < 8; b++) - { - if(crc & 0x8000) - crc = (crc << 1) ^ 0x1021; - else - crc = crc << 1; - } - } -#elif(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_STM_HW) - // Use direct STM32 HW CRC register access - uint8_t* pEnd = &packet[length]; - srxlEnterCriticalSection(); -#ifdef SRXL_SAVE_HW_CRC_CONTEXT - uint32 savedPOL = CRC->POL; - uint32 savedINIT = CRC->INIT; - uint32 savedCR = CRC->CR; - uint32 savedDR = CRC->DR; -#endif - CRC->POL = 0x1021; - CRC->INIT = 0; - CRC->CR = 0x09; // 16-bit polynomial, no input or output reversal, reset to init of 0 - while(packet < pEnd) - *(uint8_t*)(CRC_BASE) = *(packet++); - crc = (uint16_t)CRC->DR; -#ifdef SRXL_SAVE_HW_CRC_CONTEXT - // We have to use this convoluted method to restore things because writing INIT sets DR too - CRC->CR = 0; - CRC->POL = 0x04C11DB7; - CRC->INIT = savedINIT; - CRC->DR = savedINIT; - CRC->POL = savedDR; - CRC->DR = 1; - CRC->CR = savedCR; - CRC->POL = savedPOL; -#endif - srxlExitCriticalSection(); -#elif(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_STM_HAL) - // STM32f3/f7 hardware optimization using the STM32Cube libraries in HAL mode requires the following - // configuration, set in the STM32CubeMX "Pinout & Configuration" tab under "Computing > CRC": - // Basic Parameters - // Default Polynomial State = Disable - // CRC Length = 16-bit - // CRC Generating Polynomial = X12+X5+X0 - // Default Init Value State = Disable - // Init Value For CRC Computation = 0 - // Advanced Parameters - // Input Data Inversion Mode = None - // Output Data Inversion Mode = Disable - // Input Data Format = Bytes - crc = (uint16_t)HAL_CRC_Calculate(&hcrc, (uint32_t*)packet, length); -#elif(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_EXTERNAL) - crc = SRXL_CRC_CALCULATE(packet, length, crc); -#else - // Default to table-lookup method - uint8_t i; - for(i = 0; i < length; ++i) - { - // Get indexed position in lookup table using XOR of current CRC hi byte - uint8_t pos = (uint8_t)((crc >> 8) ^ packet[i]); - // Shift LSB up and XOR with the resulting lookup table entry - crc = (uint16_t)((crc << 8) ^ (uint16_t)(srxlCRCTable[pos])); - } -#endif - } - return crc; -} - -// Get the receiver entry for the requested bus and device ID -static inline SrxlRcvrEntry* srxlGetReceiverEntry(uint8_t busIndex, uint8_t deviceID) -{ - SrxlRcvrEntry* pRcvr = 0; - uint8_t i; - for(i = 0; i < srxlRx.rcvrCount; ++i) - { - if((srxlRx.rcvr[i].busBits & (1u << busIndex)) && (srxlRx.rcvr[i].deviceID == deviceID)) - { - pRcvr = &srxlRx.rcvr[i]; - break; - } - } - return pRcvr; -} - -// Add a new receiver entry for the given device -static inline SrxlRcvrEntry* srxlAddReceiverEntry(SrxlBus* pBus, SrxlDevEntry devEntry) -{ - // Only allow receivers (or flight controllers in certain circumstances) to be added - if(!pBus || devEntry.deviceID < 0x10 || devEntry.deviceID >= 0x40) - return 0; - - // If we didn't previously add this receiver, add it now if we have room - SrxlRcvrEntry* pRcvr = srxlGetReceiverEntry(pBus->fullID.busIndex, devEntry.deviceID); - if(!pRcvr) - { - if(srxlRx.rcvrCount >= SRXL_MAX_RCVRS) - return 0; - - uint8_t i = srxlRx.rcvrCount++; - pRcvr = &srxlRx.rcvr[i]; - pRcvr->deviceID = devEntry.deviceID; - pRcvr->busBits = (1u << pBus->fullID.busIndex); - pRcvr->info = devEntry.info; - - // If this receiver is full-range, insert into our sorted list after the other full-range telemetry receivers - if(pRcvr->info & SRXL_DEVINFO_TELEM_FULL_RANGE) - { - uint8_t n; - for(n = i; n > srxlRx.rcvrSortInsert; --n) - srxlRx.rcvrSorted[n] = srxlRx.rcvrSorted[n - 1]; - srxlRx.rcvrSorted[(srxlRx.rcvrSortInsert)++] = pRcvr; - } - // Else just tack onto the end - else - { - srxlRx.rcvrSorted[i] = pRcvr; - } - } - - // If this new receiver is a base receiver that supports telemetry or we haven't set a default active telemetry receiver, set it - if(!srxlRx.pTelemRcvr || (pRcvr->deviceID >= 0x20 && pRcvr->deviceID < 0x30 && (pRcvr->info & SRXL_DEVINFO_TELEM_TX_ENABLED))) - { - srxlRx.pTelemRcvr = pRcvr; - } - - return pRcvr; -} - -// Pick the best receiver to send telemetry on -static inline SrxlRcvrEntry* srxlChooseTelemRcvr(void) -{ - // If we only know about one receiver, set it to that - if(srxlRx.rcvrCount == 1) - return srxlRx.rcvrSorted[0]; - - // If we were previously sending telemetry - if(srxlRx.pTelemRcvr && srxlRx.pTelemRcvr->channelMask) - { - // If the current choice is not full-range - if((srxlRx.pTelemRcvr->info & SRXL_DEVINFO_TELEM_FULL_RANGE) == 0) - { - // Then see if there is a full-range choice that received channel data to switch to - uint8_t i; - for(i = 0; i < srxlRx.rcvrSortInsert; ++i) - { - if(srxlRx.rcvrSorted[i]->channelMask) - return srxlRx.rcvrSorted[i]; - } - } - - // Else keep using the current receiver - return srxlRx.pTelemRcvr; - } - // Else just pick the first one that got channel data this past frame - else - { - uint8_t i; - for(i = 0; i < srxlRx.rcvrCount; ++i) - { - if(srxlRx.rcvrSorted[i]->channelMask) - return srxlRx.rcvrSorted[i]; - } - } - - return 0; -} - -// Return pointer to device entry matching the given ID, or NULL if not found -static SrxlDevEntry* srxlGetDeviceEntry(SrxlBus* pBus, uint8_t deviceID) -{ - if(pBus) - { - uint8_t i; - for(i = 0; i < pBus->rxDevCount; ++i) - { - if(pBus->rxDev[i].deviceID == deviceID) - return &(pBus->rxDev[i]); - } - } - return 0; -} - -// Add an entry to our list of devices found on the SRXL bus (or update an entry if it already exists) -static SrxlDevEntry* srxlAddDeviceEntry(SrxlBus* pBus, SrxlDevEntry devEntry) -{ - // Don't allow broadcast or unknown device types to be added - if(!pBus || devEntry.deviceID < 0x10 || devEntry.deviceID > 0xEF) - return 0; - - // Limit device priority - if(devEntry.priority > 100) - devEntry.priority = 100; - - // Update device entry if it already exists - SrxlDevEntry* retVal = srxlGetDeviceEntry(pBus, devEntry.deviceID); - if(retVal) - { - pBus->rxDevPrioritySum -= retVal->priority; - *retVal = devEntry; - pBus->rxDevPrioritySum += retVal->priority; - } - // Else add to the list if we have room - else if(pBus->rxDevCount < SRXL_MAX_DEVICES) - { - retVal = &(pBus->rxDev[pBus->rxDevCount++]); - *retVal = devEntry; - pBus->rxDevPrioritySum += retVal->priority; - -#ifdef SRXL_INCLUDE_FWD_PGM_CODE - // If the new device supports Forward Programming and is a base receiver or flight controller, choose as the default - if(devEntry.info & SRXL_DEVINFO_FWD_PROG_SUPPORT) - { - uint8_t devType = devEntry.deviceID >> 4; - if(devType > (srxlFwdPgmDevice.deviceID >> 4) && devType < SrxlDevType_ESC) - { - srxlFwdPgmDevice.deviceID = devEntry.deviceID; - srxlFwdPgmDevice.busIndex = pBus->fullID.busIndex; - } - } -#endif - - // If the new device is a receiver, add to our receiver list - if(retVal->deviceID < 0x30) - { - srxlAddReceiverEntry(pBus, *retVal); - } - } - - return retVal; -} - -/// PUBLIC FUNCTIONS /// - -/** - @brief Initialize common SRXL info for this device - - @param deviceID: SRXL Device ID (see section 7.1.1 of SRXL2 Spec) - @param priority: Requested telemetry priority (1-100; typical is 10 per unique message type) - @param info: Device info bits (see SRXL_DEVINFO_XXX bits in spm_srxl.h) - @param uid: Unique 32-bit id to avoid device ID collision during handshake - @return bool: True if device info was successfully initialized -*/ -bool srxlInitDevice(uint8_t deviceID, uint8_t priority, uint8_t info, uint32_t uid) -{ - if(deviceID < 0x10 || deviceID > 0xEF) - return false; - - srxlThisDev.devEntry.deviceID = deviceID; - srxlThisDev.devEntry.info = info; - srxlThisDev.devEntry.priority = priority; - srxlThisDev.devEntry.rfu = 0; - srxlThisDev.uid = uid; - srxlThisDev.vtxProxy = false; - -#ifdef SRXL_INCLUDE_MASTER_CODE - // If this device is a receiver, add to our receiver info - if(deviceID < 0x30) - { - srxlInitReceiver(deviceID, info); - } -#endif - -#ifdef SRXL_INCLUDE_FWD_PGM_CODE - // If this device is a receiver or flight controller that supports Forward Programming, set as default - if((info & SRXL_DEVINFO_FWD_PROG_SUPPORT) && deviceID < 0x40) - { - srxlFwdPgmDevice.deviceID = deviceID; - srxlFwdPgmDevice.busIndex = 0; - } -#endif - -#if(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_STM_HW) - // Enable the peripheral clock for the HW CRC engine - RCC->AHBENR |= RCC_AHBENR_CRCEN; -#endif - - return true; -} - -/** - @brief Initialize bus settings for the given SRXL bus - - @param busIndex: Index into srxlBus array of bus entries - @param uart: Number to identify UART to which this SRXL bus should be connected - @param baudSupported: 0 = 115200 baud, 1 = 400000 baud - @return bool: True if SRXL bus was successfully initialized -*/ -bool srxlInitBus(uint8_t busIndex, uint8_t uart, uint8_t baudSupported) -{ - if(busIndex >= SRXL_NUM_OF_BUSES || !srxlThisDev.devEntry.deviceID) - return false; - - SrxlBus* pBus = &srxlBus[busIndex]; - pBus->state = SrxlState_ListenOnStartup; - pBus->fullID.deviceID = srxlThisDev.devEntry.deviceID; - pBus->fullID.busIndex = busIndex; - pBus->rxDevCount = 0; - pBus->rxDevPrioritySum = 0; - pBus->requestID = (srxlThisDev.devEntry.deviceID == 0x10) ? 0x11 : 0; - pBus->baudSupported = baudSupported; - pBus->baudRate = SRXL_BAUD_115200; - pBus->frameErrCount = 0; - pBus->uart = uart; - // Default remote receiver is automatically master -- everyone else figures it out during handshake - pBus->master = (srxlThisDev.devEntry.deviceID == 0x10); - pBus->pMasterRcvr = (srxlThisDev.devEntry.deviceID == 0x10) ? &srxlRx.rcvr[0] : 0; - pBus->initialized = true; - - return true; -} - -/** - @brief See if this device is the bus master on the given bus - - @param busIndex: Index into srxlBus array for the desired SRXL bus - @return bool: True if this device is the bus master on the given SRXL bus -*/ -bool srxlIsBusMaster(uint8_t busIndex) -{ - return (busIndex < SRXL_NUM_OF_BUSES && srxlBus[busIndex].master); -} - -/** - @brief Get the current SRXL state machine timeout count for the given bus - - @param busIndex: Index into srxlBus array for the desired SRXL bus - @return uint16_t: Timeout count in ms for the given SRXL bus -*/ -uint16_t srxlGetTimeoutCount_ms(uint8_t busIndex) -{ - return (busIndex < SRXL_NUM_OF_BUSES) ? srxlBus[busIndex].timeoutCount_ms : 0; -} - -/** - @brief Get the Device ID of this device on the given bus - - @param busIndex: Index into srxlBus array for the desired SRXL bus - @return uint8_t: Device ID of this device on the given SRXL bus -*/ -uint8_t srxlGetDeviceID(uint8_t busIndex) -{ - return (busIndex < SRXL_NUM_OF_BUSES) ? srxlBus[busIndex].fullID.deviceID : 0; -} - -/** - @brief Internal send function called by srxlRun() -- do not call in user code - - @param pBus: Pointer to SRXL bus entry for the desired SRXL bus - @param srxlCmd: Specific type of packet to send - @param replyID: Device ID of the device this Send command is targeting -*/ -static void srxlSend(SrxlBus* pBus, SRXL_CMD srxlCmd, uint8_t replyID) -{ - if(!pBus || !pBus->initialized) - return; - - memset(pBus->srxlOut.raw, 0, SRXL_MAX_BUFFER_SIZE); - pBus->srxlOut.header.srxlID = SPEKTRUM_SRXL_ID; - - // VTX Data - if(srxlCmd == SRXL_CMD_VTX) - { - pBus->srxlOut.header.packetType = SRXL_CTRL_ID; - pBus->srxlOut.header.length = SRXL_CTRL_BASE_LENGTH + sizeof(SrxlVtxData); - pBus->srxlOut.control.payload.cmd = SRXL_CTRL_CMD_VTX; - pBus->srxlOut.control.payload.replyID = replyID; - pBus->srxlOut.control.payload.vtxData = srxlVtxData; - } -#ifdef SRXL_INCLUDE_MASTER_CODE - // Channel Data - else if(srxlCmd == SRXL_CMD_CHANNEL || srxlCmd == SRXL_CMD_CHANNEL_FS) - { - pBus->srxlOut.header.packetType = SRXL_CTRL_ID; - uint32_t channelMask; - if(srxlCmd == SRXL_CMD_CHANNEL) - { - pBus->srxlOut.control.payload.cmd = SRXL_CTRL_CMD_CHANNEL; - pBus->srxlOut.control.payload.replyID = replyID; - - channelMask = srxlChData.mask; - } - else // == SRXL_CMD_CHANNEL_FS - { - // In failsafe mode, we dont want a telemetry reply - pBus->srxlOut.control.payload.cmd = SRXL_CTRL_CMD_CHANNEL_FS; - pBus->srxlOut.control.payload.replyID = 0; - - channelMask = srxlFailsafeChMask; - } - - // Set signal quality info (only a bus master sends this, so assume srxlChData contains the latest values) - pBus->srxlOut.control.payload.channelData.rssi = srxlChData.rssi; -#ifdef SRXL_IS_HUB - pBus->srxlOut.control.payload.channelData.frameLosses = srxlRx.frameLosses; -#else - pBus->srxlOut.control.payload.channelData.frameLosses = srxlRx.rcvr[0].fades; -#endif - - uint8_t channelIndex = 0; - uint32_t channelMaskBit = 1; - for(uint8_t i = 0; i < 32; ++i, channelMaskBit <<= 1) - { - if(channelMask & channelMaskBit) - { - pBus->srxlOut.control.payload.channelData.values[channelIndex++] = srxlChData.values[i]; - } - } - - // Set bits in packet for channels we populated, and clear those mask bits if it was part of a normal channel data command - pBus->srxlOut.control.payload.channelData.mask = channelMask; - if(srxlCmd == SRXL_CMD_CHANNEL) - srxlChData.mask &= ~channelMask; - - pBus->srxlOut.header.length = SRXL_CTRL_BASE_LENGTH + 7 + (2 * channelIndex); - } -#ifdef SRXL_INCLUDE_FWD_PGM_CODE - // Forward Programming Pass-thru - else if(srxlCmd == SRXL_CMD_FWDPGM) - { - pBus->srxlOut.header.packetType = SRXL_CTRL_ID; - pBus->srxlOut.header.length = SRXL_CTRL_BASE_LENGTH + 3 + srxlFwdPgmBufferLength; - pBus->srxlOut.control.payload.cmd = SRXL_CTRL_CMD_FWDPGM; - pBus->srxlOut.control.payload.replyID = replyID; - memcpy(pBus->srxlOut.control.payload.fpData.data, srxlFwdPgmBuffer, srxlFwdPgmBufferLength); - } -#endif // SRXL_INCLUDE_FWD_PGM_CODE -#endif // SRXL_INCLUDE_MASTER_CODE - // RSSI Data - else if(srxlCmd == SRXL_CMD_RSSI) - { - pBus->srxlOut.header.packetType = SRXL_RSSI_ID; - pBus->srxlOut.header.length = sizeof(SrxlRssiPacket); - pBus->srxlOut.rssi.request = SRXL_RSSI_REQ_REQUEST; // TODO: Needs to handle both directions! - pBus->srxlOut.rssi.antennaA = 0; // TODO: Fill in actual data later - pBus->srxlOut.rssi.antennaB = 0; - pBus->srxlOut.rssi.antennaC = 0; - pBus->srxlOut.rssi.antennaD = 0; - } - else if(srxlCmd == SRXL_CMD_HANDSHAKE) - { - pBus->srxlOut.header.packetType = SRXL_HANDSHAKE_ID; - pBus->srxlOut.header.length = sizeof(SrxlHandshakePacket); - pBus->srxlOut.handshake.payload.srcDevID = srxlThisDev.devEntry.deviceID; - pBus->srxlOut.handshake.payload.destDevID = replyID; - pBus->srxlOut.handshake.payload.priority = srxlThisDev.devEntry.priority; - pBus->srxlOut.handshake.payload.baudSupported = pBus->baudSupported; - pBus->srxlOut.handshake.payload.info = srxlThisDev.devEntry.info; - pBus->srxlOut.handshake.payload.uid = srxlThisDev.uid; - } - else if(srxlCmd == SRXL_CMD_TELEMETRY) - { - srxlFillTelemetry(&pBus->srxlOut.telemetry.payload); - pBus->srxlOut.header.packetType = SRXL_TELEM_ID; - pBus->srxlOut.header.length = sizeof(SrxlTelemetryPacket); - // If we successfully received a handshake from the bus master - if(pBus->pMasterRcvr) - { - // If we know that a device on this bus should send it, then target that device - if(srxlRx.pTelemRcvr && (srxlRx.pTelemRcvr->busBits & (1u << pBus->fullID.busIndex))) - pBus->srxlOut.telemetry.destDevID = srxlRx.pTelemRcvr->deviceID; - else - pBus->srxlOut.telemetry.destDevID = 0; - } - else - { - // Send 0xFF to tell bus master to re-send the handshake so we know where to direct telemetry in the future - pBus->srxlOut.telemetry.destDevID = 0xFF; - } - -#ifdef SRXL_INCLUDE_MASTER_CODE - if(srxlRx.pTelemRcvr && (pBus->srxlOut.telemetry.destDevID == srxlRx.pTelemRcvr->deviceID)) - { - // Don't mark telemetry as having been sent if we are sending it ourself over RF - if(pBus->srxlOut.telemetry.destDevID != srxlThisDev.pRcvr->deviceID) - { - srxlTelemetrySent(); - // Clear telemetry buffer after sending so we don't repeatedly display old data -// srxlTelemData.sensorID = srxlTelemData.secondaryID = 0; - } - } -#endif - } - else if(srxlCmd == SRXL_CMD_ENTER_BIND) - { - pBus->srxlOut.header.packetType = SRXL_BIND_ID; - pBus->srxlOut.header.length = sizeof(SrxlBindPacket); - pBus->srxlOut.bind.request = SRXL_BIND_REQ_ENTER; - pBus->srxlOut.bind.deviceID = replyID; - pBus->srxlOut.bind.data.type = DSMX_11MS; - pBus->srxlOut.bind.data.options = (replyID != 0xFF) ? SRXL_BIND_OPT_TELEM_TX_ENABLE | SRXL_BIND_OPT_BIND_TX_ENABLE : 0; - pBus->srxlOut.bind.data.guid = 0; - pBus->srxlOut.bind.data.uid = 0; - } - else if(srxlCmd == SRXL_CMD_REQ_BIND) - { - pBus->srxlOut.header.packetType = SRXL_BIND_ID; - pBus->srxlOut.header.length = sizeof(SrxlBindPacket); - pBus->srxlOut.bind.request = SRXL_BIND_REQ_STATUS; - pBus->srxlOut.bind.deviceID = replyID; - memset(&(pBus->srxlOut.bind.data), 0, sizeof(SrxlBindData)); - } - else if(srxlCmd == SRXL_CMD_SET_BIND) - { - pBus->srxlOut.header.packetType = SRXL_BIND_ID; - pBus->srxlOut.header.length = sizeof(SrxlBindPacket); - pBus->srxlOut.bind.request = SRXL_BIND_REQ_SET_BIND; - pBus->srxlOut.bind.deviceID = replyID; - pBus->srxlOut.bind.data = srxlBindInfo; - } - else if(srxlCmd == SRXL_CMD_BIND_INFO) - { - pBus->srxlOut.header.packetType = SRXL_BIND_ID; - pBus->srxlOut.header.length = sizeof(SrxlBindPacket); - pBus->srxlOut.bind.request = SRXL_BIND_REQ_BOUND_DATA; - pBus->srxlOut.bind.deviceID = replyID; - pBus->srxlOut.bind.data = srxlBindInfo; - } - - // Compute CRC over entire SRXL packet (excluding the 2 CRC bytes at the end) - uint16_t crc = srxlCrc16(pBus->srxlOut.raw); - - // Add CRC to packet in big-endian byte order - pBus->srxlOut.raw[pBus->srxlOut.header.length - 2] = (crc >> 8) & 0xFF; - pBus->srxlOut.raw[pBus->srxlOut.header.length - 1] = crc & 0xFF; - - // Send the packet out over the assigned UART - srxlSendOnUart(pBus->uart, pBus->srxlOut.raw, pBus->srxlOut.header.length); -} - -/** - @brief Parse an SRXL packet received on the given SRXL bus UART - - @param busIndex: Index of SRXL bus state information entry in the srxlBus array - @param packet: Pointer to received packet data - @param length: Length in bytes of received packet data - @return bool: True if a valid packet was received, else false -*/ -bool srxlParsePacket(uint8_t busIndex, uint8_t* packet, uint8_t length) -{ - // Validate parameters - if(busIndex >= SRXL_NUM_OF_BUSES || !packet || length < 5 || length > SRXL_MAX_BUFFER_SIZE) - return false; - - // Validate SRXL ID and length - if(packet[0] != SPEKTRUM_SRXL_ID || packet[2] != length) - return false; - - // Validate checksum - uint16_t crc = srxlCrc16(packet); - if((((uint16_t)packet[length - 2] << 8) | packet[length - 1]) != crc) - return false; - - // Copy packet into our unioned buffer to avoid "strict aliasing" violations - SrxlBus* pBus = &srxlBus[busIndex]; - SrxlPacket* pRx = &(pBus->srxlIn); - memcpy(pRx, packet, length); - - // Handle restart with ongoing communications -- bump to run state - pBus->timeoutCount_ms = 0; // TODO: Should we clear this even if packet isn't valid? - if(pBus->state < SrxlState_Running && pRx->header.packetType != SRXL_HANDSHAKE_ID) - pBus->state = SrxlState_Running; - - // Parse the specific data - switch(pRx->header.packetType) - { - case SRXL_CTRL_ID: // 0xCD - { - SrxlControlData* pCtrlData = &(pRx->control.payload); - - // Validate command - if(pCtrlData->cmd > SRXL_CTRL_CMD_FWDPGM) - break; - - // VTX - if(pCtrlData->cmd == SRXL_CTRL_CMD_VTX) - { - if(srxlSetVtxData(&pCtrlData->vtxData)) - srxlOnVtx(&srxlVtxData); - if(pCtrlData->replyID == pBus->fullID.deviceID || pCtrlData->replyID == 0xFF || srxlThisDev.vtxProxy) - { - // TODO: Should we ack this somehow - } - } -#ifdef SRXL_INCLUDE_FWD_PGM_CODE - // Forward Programming - else if(pCtrlData->cmd == SRXL_CTRL_CMD_FWDPGM) - { - if(pCtrlData->replyID == pBus->fullID.deviceID) - { - memcpy(srxlFwdPgmBuffer, pCtrlData->fpData.data, pRx->header.length - 3 - SRXL_CTRL_BASE_LENGTH); - srxlFwdPgmBufferLength = length; - if(pCtrlData->replyID == srxlFwdPgmDevice.deviceID) - { - // Handle Forward Programming command locally - srxlOnFwdPgm(srxlFwdPgmBuffer, srxlFwdPgmBufferLength); - } - else if(srxlFwdPgmDevice.deviceID && srxlBus[srxlFwdPgmDevice.busIndex].master) - { - // Pass it on through to the next target - srxlBus[srxlFwdPgmDevice.busIndex].txFlags.sendFwdPgmData = 1; - } - } - } -#endif - // Channel Data or Failsafe Data - else - { - bool isFailsafe = (pCtrlData->cmd == SRXL_CTRL_CMD_CHANNEL_FS); - srxlChData.rssi = pCtrlData->channelData.rssi; - srxlChData.frameLosses = pCtrlData->channelData.frameLosses; - if(pBus->pMasterRcvr) - { - if(pCtrlData->channelData.rssi < 0) - { - pBus->pMasterRcvr->rssi_dBm = pCtrlData->channelData.rssi; - pBus->pMasterRcvr->rssiRcvd |= RSSI_RCVD_DBM; - } - else - { - pBus->pMasterRcvr->rssi_Pct = pCtrlData->channelData.rssi; - pBus->pMasterRcvr->rssiRcvd |= RSSI_RCVD_PCT; - } - // If the receiver is sending alternating dBm/%, then use that as phase - if(pBus->pMasterRcvr->rssiRcvd == RSSI_RCVD_BOTH) - { - srxlTelemetryPhase = pCtrlData->channelData.rssi >= 0; - } - pBus->pMasterRcvr->fades = pCtrlData->channelData.frameLosses; - pBus->pMasterRcvr->channelMask = isFailsafe ? 0 : pCtrlData->channelData.mask; - srxlRx.rxBusBits |= pBus->pMasterRcvr->busBits; - } - - // Only save received channel values to srxlChData if it's normal channel data or we're in a hold condition - if(!isFailsafe || !srxlRx.lossCountdown) - { - uint8_t channelIndex = 0; - uint32_t channelMaskBit = 1; - uint8_t i; - for(i = 0; i < 32 && channelMaskBit <= pCtrlData->channelData.mask; ++i, channelMaskBit <<= 1) - { - if(pCtrlData->channelData.mask & channelMaskBit) - { - srxlChData.values[i] = pCtrlData->channelData.values[channelIndex++]; - srxlChData.mask |= channelMaskBit; - } - } - } - - srxlChDataIsFailsafe = isFailsafe; // TODO: Can we still assume this??? - srxlReceivedChannelData(&(pCtrlData->channelData), isFailsafe); - - // Figure out what type of reply packet to send, if any - if(pCtrlData->replyID == 0) - { - if(pBus->txFlags.enterBind) - { - pBus->state = SrxlState_SendEnterBind; - pBus->txFlags.enterBind = 0; - } - else if(pBus->txFlags.setBindInfo) - { - if(srxlRx.pBindRcvr) // TODO: Double-check this logic - { - pBus->requestID = srxlRx.pBindRcvr->deviceID; - pBus->state = SrxlState_SendSetBindInfo; - } - pBus->txFlags.setBindInfo = 0; - } - else if(pBus->txFlags.broadcastBindInfo) - { - pBus->requestID = 0xFF; - pBus->state = SrxlState_SendSetBindInfo; - pBus->txFlags.broadcastBindInfo = 0; - } - } - else if(pCtrlData->replyID == pBus->fullID.deviceID) - { - pBus->state = SrxlState_SendTelemetry; - } - } - break; - } - case SRXL_HANDSHAKE_ID: // 0x21 - { - if(length < sizeof(SrxlHandshakePacket)) - return false; - - // If this is an unprompted handshake (dest == 0) from a higher device ID, then we're the master - SrxlHandshakeData* pHandshake = &(pRx->handshake.payload); - if((pHandshake->destDevID == 0) && (pHandshake->srcDevID > pBus->fullID.deviceID)) - { - // Send a reply immediately to get the slave to shut up - pBus->state = SrxlState_SendHandshake; - pBus->requestID = pHandshake->srcDevID; - pBus->baudSupported = SRXL_SUPPORTED_BAUD_RATES; - srxlRun(busIndex, 0); - pBus->requestID = pBus->fullID.deviceID; - pBus->state = SrxlState_SendHandshake; - pBus->master = true; - pBus->pMasterRcvr = &srxlRx.rcvr[0]; - } - - // Add this device to our list of discovered devices - SrxlDevEntry newDev = {.deviceID = pHandshake->srcDevID, .priority = pHandshake->priority, .info = pHandshake->info}; - srxlAddDeviceEntry(pBus, newDev); - - // Bus master needs to track responses and poll next device - if(pBus->master) - { - // Keep track of baud rates supported - pBus->baudSupported &= pHandshake->baudSupported; - - // Make sure the state machine is set to poll -- this will eventually reach broadcast address (0xFF) - pBus->state = SrxlState_SendHandshake; - } - // Broadcast handshake sets the agreed upon baud rate for this bus - else if(pHandshake->destDevID == 0xFF) - { - // Get bus master receiver entry (if it's a flight controller, add it now as a receiver on this bus) - if(newDev.deviceID >= 0x30 && newDev.deviceID < 0x40) - { - pBus->pMasterRcvr = srxlAddReceiverEntry(pBus, newDev); - } - else - { - pBus->pMasterRcvr = srxlGetReceiverEntry(busIndex, newDev.deviceID); - } - - // Set baud rate and advance to run state - pBus->baudRate = pHandshake->baudSupported; - if(pBus->baudRate & SRXL_BAUD_400000) - { - srxlChangeBaudRate(pBus->uart, 400000); // Only alternate rate supported for now... - } - pBus->state = SrxlState_Running; - } - // Normal Handshake destined for this device should be replied to -- else ignore - else - { - if(pHandshake->destDevID == pBus->fullID.deviceID && pBus->state != SrxlState_SendHandshake) - { - pBus->requestID = pHandshake->srcDevID; - pBus->state = SrxlState_SendHandshake; - } - else - { - pBus->state = SrxlState_ListenForHandshake; - } - } - - break; - } - case SRXL_PARAM_ID: // 0x50 - { - // TODO: Add later - break; - } - case SRXL_RSSI_ID: // 0x55 - { - // TODO: Add later - break; - } - case SRXL_BIND_ID: // 0x41 - { - if(length < sizeof(SrxlBindPacket)) - return false; - - SrxlBindPacket* pBindInfo = &(pRx->bind); - - // If this is a bound data report - if(pBindInfo->request == SRXL_BIND_REQ_BOUND_DATA) - { - // Call the user-defined callback -- if returns true, bind all other receivers - SrxlFullID boundID; - boundID.deviceID = pBindInfo->deviceID; - boundID.busIndex = busIndex; - if(srxlOnBind(boundID, pBindInfo->data)) - { - // Update the bind info - srxlBindInfo.type = pBindInfo->data.type; - if(pBindInfo->data.options & SRXL_BIND_OPT_TELEM_TX_ENABLE) - { - SrxlRcvrEntry* pNewTelem = srxlGetReceiverEntry(busIndex, pBindInfo->deviceID); - if(pNewTelem || !srxlRx.pTelemRcvr) - srxlRx.pTelemRcvr = pNewTelem; - } - srxlBindInfo.options = 0; // Disable telemetry and Discovery reply when setting other receivers - srxlBindInfo.guid = pBindInfo->data.guid; - srxlBindInfo.uid = pBindInfo->data.uid; - - // Try to set bind info for all other receivers on other buses to match it - uint8_t b; - for(b = 0; b < SRXL_NUM_OF_BUSES; ++b) - { - if(b == busIndex) - continue; - srxlBus[b].txFlags.broadcastBindInfo = 1; - } - } - } - // If this bind packet is directed at us - else if(pBindInfo->deviceID == pBus->fullID.deviceID || pBindInfo->deviceID == 0xFF) - { - // Check for Enter Bind Mode (only valid if sent to a specific receiver) - if(pBindInfo->request == SRXL_BIND_REQ_ENTER) - { -#ifdef SRXL_INCLUDE_MASTER_CODE - srxlBindInfo.type = pBindInfo->data.type; - srxlBindInfo.options = pBindInfo->data.options; - srxlBindInfo.guid = 0; - srxlBindInfo.uid = 0; - srxlTryToBind(srxlBindInfo); -#endif - } - else if(pBindInfo->request == SRXL_BIND_REQ_STATUS && srxlThisDev.pRcvr) - { - // TODO: Fill in data if we didn't just bind? - pBus->txFlags.reportBindInfo = 1; - } - // Handle set bind info request - else if(pBindInfo->request == SRXL_BIND_REQ_SET_BIND) - { - srxlBindInfo = pBindInfo->data; -#ifdef SRXL_INCLUDE_MASTER_CODE - if(pBus->fullID.deviceID < 0x30) - srxlTryToBind(srxlBindInfo); -#endif - } - } - - break; - } - case SRXL_TELEM_ID: // 0x80 - { - if(length < sizeof(SrxlTelemetryPacket)) - return false; - - // NOTE: This data should be sent by exactly one telemetry device in response to a bus master request, - // so it is safe to update the global pTelemRcvr here even though this is a bus-specific function. - - SrxlTelemetryPacket* pTelem = &(pRx->telemetry); - memcpy(&srxlTelemData, &pTelem->payload, sizeof(srxlTelemData)); - // If the telemetry destination is set to broadcast, that indicates a request to re-handshake - if(pBus->master && pTelem->destDevID == 0xFF) - { - // If the master only found one device, don't poll again -- just tell the requesting device who we are - pBus->requestID = pBus->rxDevCount > 1 ? pBus->fullID.deviceID : 0xFF; - pBus->state = SrxlState_SendHandshake; - } - // If the incoming telemetry is destined for us, then we need to figure out who should send it over RF - else if(pTelem->destDevID == pBus->fullID.deviceID) - { - // This needs different logic for hubs versus endpoints -#ifdef SRXL_IS_HUB - if(srxlRx.pTelemRcvr == 0) - { - srxlRx.pTelemRcvr = srxlChooseTelemRcvr(); - } -#else - srxlRx.pTelemRcvr = srxlThisDev.pRcvr; -#endif - // Enable this device's telemetry tx based on whether we are the chosen telemetry receiver -#ifdef SRXL_INCLUDE_MASTER_CODE - srxlSetTelemetryTxEnable(srxlRx.pTelemRcvr && (srxlRx.pTelemRcvr == srxlThisDev.pRcvr)); -#endif - } - // Else turn off our telemetry and that of any receivers we might reply to via our own telemetry - else - { - srxlRx.pTelemRcvr = 0; -#ifdef SRXL_INCLUDE_MASTER_CODE - srxlSetTelemetryTxEnable(false); -#endif - } - - srxlTelemSuppressCount = 0; -#ifdef SRXL_INCLUDE_MASTER_CODE - srxlSuppressInternalTelemetry(&pTelem->payload); -#endif - break; - } - default: - { - break; - } - } - - // Run state machine for slave devices after each received packet - if(!pBus->master) - { - srxlRun(busIndex, 0); - } - - return true; -} - -/** - @brief Run the SRXL state machine after each receive or rx timeout - - @param busIndex: Index of SRXL bus state information entry in the srxlBus array - @param timeoutDelta_ms: Number of milliseconds to increment receive timeout if a timeout - occured, or <= 0 to clear timeout count upon packet receive. -*/ -void srxlRun(uint8_t busIndex, int16_t timeoutDelta_ms) -{ - SrxlBus* pBus = &srxlBus[busIndex]; - if(busIndex >= SRXL_NUM_OF_BUSES || !pBus->initialized || pBus->state == SrxlState_Disabled) - return; - - // Check receive timeout and advance state if needed - if(timeoutDelta_ms > 0) - { - pBus->timeoutCount_ms += timeoutDelta_ms; - if(pBus->timeoutCount_ms >= 30000) - pBus->timeoutCount_ms = 30000; - - if(pBus->timeoutCount_ms >= 50) - { - // After startup delay of 50ms, switch to handshake send or listen based on device unit ID - if(pBus->state == SrxlState_ListenOnStartup) - { - pBus->state = (pBus->fullID.deviceID & 0x0F) ? SrxlState_ListenForHandshake : SrxlState_SendHandshake; - } - // Reset non-master device back to startup conditions if 50ms elapses with no communications - else if(!pBus->master && pBus->state >= SrxlState_Running) - { - srxlChangeBaudRate(pBus->uart, 115200); - pBus->baudRate = SRXL_BAUD_115200; - pBus->timeoutCount_ms = 0; - pBus->requestID = 0; // Change back to 0 to indicate unprompted handshake from slave device - if(pBus->pMasterRcvr) - { - pBus->pMasterRcvr->channelMask = 0; - pBus->pMasterRcvr->fades = 0xFFFF; - pBus->pMasterRcvr->rssi_Pct = 0; - pBus->pMasterRcvr->rssi_dBm = -1; - } - pBus->state = SrxlState_ListenOnStartup; - } - } - } - else - { - pBus->timeoutCount_ms = 0; - } - - if(!pBus->master) - { - // Non-master actions for the given state - switch(pBus->state) - { - case SrxlState_SendHandshake: - { - srxlSend(pBus, SRXL_CMD_HANDSHAKE, pBus->requestID); - break; - } - case SrxlState_SendTelemetry: - { - srxlSend(pBus, SRXL_CMD_TELEMETRY, pBus->requestID); - pBus->state = SrxlState_Running; - break; - } - case SrxlState_SendVTX: - { - srxlSend(pBus, SRXL_CMD_VTX, pBus->requestID); - pBus->state = SrxlState_Running; - break; - } - case SrxlState_SendEnterBind: - { - if(srxlRx.pBindRcvr && (srxlRx.pBindRcvr != srxlThisDev.pRcvr)) - { - srxlSend(pBus, SRXL_CMD_ENTER_BIND, srxlRx.pBindRcvr->deviceID); - } - else - { - srxlBindInfo.options = 0; - srxlSend(pBus, SRXL_CMD_ENTER_BIND, 0xFF); - } - pBus->state = SrxlState_Running; - break; - } - case SrxlState_SendSetBindInfo: - { - srxlSend(pBus, SRXL_CMD_SET_BIND, pBus->requestID); - pBus->state = SrxlState_Running; - break; - } - case SrxlState_SendBoundDataReport: - { - srxlSend(pBus, SRXL_CMD_BIND_INFO, pBus->fullID.deviceID); - pBus->state = SrxlState_Running; - break; - } - case SrxlState_Running: - default: - { - return; - } - } - } -#ifdef SRXL_INCLUDE_MASTER_CODE - else - { - srxlRunMaster(pBus); - } -#endif // SRXL_INCLUDE_MASTER_CODE -} - -/** - @brief Tell the "best" receiver to enter bind mode, either locally or via SRXL command - - @param bindType: One of the possible bind status types to use when binding -- NOTE: The transmitter may ignore this - @param broadcast: True if this is a local request that should tell all connected receivers to enter bind - @return bool: True if a receiver was told to enter bind mode; else false -*/ -bool srxlEnterBind(uint8_t bindType, bool broadcast) -{ - srxlRx.pBindRcvr = 0; - if(broadcast && srxlThisDev.pRcvr) - { - srxlRx.pBindRcvr = srxlThisDev.pRcvr; - } - else if(srxlRx.pTelemRcvr) - { - srxlRx.pBindRcvr = srxlRx.pTelemRcvr; - } - else if(srxlRx.rcvrCount > 0 && srxlRx.rcvrSorted[0]->deviceID < 0x30) - { - srxlRx.pBindRcvr = srxlRx.rcvrSorted[0]; - } - - if(srxlRx.pBindRcvr) - { -#ifdef SRXL_INCLUDE_MASTER_CODE - // Local bind - if(srxlRx.pBindRcvr == srxlThisDev.pRcvr) - { - srxlBindInfo.type = bindType; - srxlBindInfo.options = SRXL_BIND_OPT_BIND_TX_ENABLE; - srxlBindInfo.guid = 0; - srxlBindInfo.uid = 0; - if(srxlRx.pBindRcvr == srxlRx.pTelemRcvr) - srxlBindInfo.options |= SRXL_BIND_OPT_TELEM_TX_ENABLE; - srxlTryToBind(srxlBindInfo); - if(broadcast) - { - uint8_t b; - for(b = 0; b < SRXL_NUM_OF_BUSES; ++b) - { - srxlBus[b].txFlags.enterBind = 1; - } - } - return true; - } -#endif // SRXL_INCLUDE_MASTER_CODE - - // Remote bind - uint8_t i; - for(i = 0; i < SRXL_NUM_OF_BUSES; ++i) - { - if((1u << i) & srxlRx.pBindRcvr->busBits) - { - srxlBus[i].txFlags.enterBind = 1; - return true; - } - } - } - - return false; -} - -/** - @brief Public function to set bind info for the system, either locally or via SRXL commands - - @param bindType: Type of bind requested for this receiver or all receivers - @param guid: Transmitter GUID to bind the receiver to - @param uid: Unique ID provided by transmitter upon initial bind (can be 0 if unknown) - @return bool: True if bind info was successfully set for the destination device; else false -*/ -bool srxlSetBindInfo(uint8_t bindType, uint64_t guid, uint32_t uid) -{ - if(guid == 0) - return false; - - // Set bind info, with options defaulted to 0 - srxlBindInfo.type = bindType; - srxlBindInfo.options = SRXL_BIND_OPT_US_POWER; // Request US power, with no guarantee it's supported - srxlBindInfo.guid = guid; - srxlBindInfo.uid = uid ? uid : srxlThisDev.uid; - -#ifdef SRXL_INCLUDE_MASTER_CODE - // If we are a receiver - if(srxlThisDev.pRcvr) - { - // Bind locally, which will result in no further packets since options == 0 - srxlTryToBind(srxlBindInfo); - } -#endif - - // Broadcast this bind info on all SRXL buses - uint8_t b; - for(b = 0; b < SRXL_NUM_OF_BUSES; ++b) - { - srxlBus[b].txFlags.broadcastBindInfo = 1; - } - - return true; -} - -/** - @brief Public function to call from the user UART code when a UART frame error occurs - - @param busIndex: Index of SRXL bus state information entry in the srxlBus array -*/ -void srxlOnFrameError(uint8_t busIndex) -{ - SrxlBus* pBus = &srxlBus[busIndex]; - if(busIndex >= SRXL_NUM_OF_BUSES || !pBus->initialized) - return; - - ++(pBus->frameErrCount); - if(pBus->master) - { - // TODO: If master (i.e. remote receiver 0x10), cause break condition to force reset? - return; - } - - if(pBus->state == SrxlState_ListenOnStartup || pBus->state == SrxlState_ListenForHandshake) - { - // Wait for multiple frame breaks before trying to change baud rate - if(pBus->frameErrCount < 3) - return; - - // Try the next higher baud rate - switch(pBus->baudRate) - { - case SRXL_BAUD_115200: - { - if(pBus->baudSupported & SRXL_BAUD_400000) - { - srxlChangeBaudRate(pBus->uart, 400000); - pBus->baudRate = SRXL_BAUD_400000; - break; - } - FALLTHROUGH; - // else fall thru... - } - case SRXL_BAUD_400000: - default: - { - // TODO: Cause break condition to force reset of everyone? Or just keep cycling? - - srxlChangeBaudRate(pBus->uart, 115200); - pBus->baudRate = SRXL_BAUD_115200; - break; - } - } - pBus->frameErrCount = 0; - } - else - { - // TODO: Handle frame error during normal comm -- probably caused by collision on bus? - } -} - -SrxlFullID srxlGetTelemetryEndpoint(void) -{ - SrxlFullID retVal = {0}; - if(srxlRx.pTelemRcvr) - { - retVal.deviceID = srxlRx.pTelemRcvr->deviceID; - retVal.busIndex = srxlRx.pTelemRcvr->busBits; - } - return retVal; -} - -bool srxlSetVtxData(SrxlVtxData* pVtxData) -{ - if(!pVtxData) - return false; - - // Update VTX data, ignoring values marked as unchanged - if(pVtxData->band != 0xFF) - srxlVtxData.band = pVtxData->band; - if(pVtxData->channel != 0xFF) - srxlVtxData.channel = pVtxData->channel; - if(pVtxData->pit != 0xFF) - srxlVtxData.pit = pVtxData->pit; - if(pVtxData->power != 0xFF || pVtxData->powerDec != 0xFFFF) - srxlVtxData.power = pVtxData->power; - if(pVtxData->powerDec != 0xFFFF) - srxlVtxData.powerDec = pVtxData->powerDec; - if(pVtxData->region != 0xFF) - srxlVtxData.region = pVtxData->region; - - uint8_t b; - for(b = 0; b < SRXL_NUM_OF_BUSES; ++b) - { - srxlBus[b].txFlags.sendVtxData = 1; - } - - return true; -} - -void srxlSetHoldThreshold(uint8_t countdownReset) -{ - srxlRx.lossHoldCount = (countdownReset > 1) ? countdownReset : 45; -} - -void srxlClearCommStats(void) -{ - srxlRx.holds = 0; - srxlRx.frameLosses = 0; - srxlRx.lossCountdown = srxlRx.lossHoldCount + 1; -} - -// Return true on failsafe hold -bool srxlUpdateCommStats(bool isFade) -{ - srxlRx.rxBusBits = 0; - if(srxlTelemetryPhase) - { - srxlRx.bestRssi_dBm = -128; - srxlRx.bestRssi_Pct = 0; - } - - uint8_t i; - for(i = 0; i < srxlRx.rcvrCount; ++i) - { - if(srxlRx.rcvr[i].channelMask) - { - srxlRx.lossCountdown = srxlRx.lossHoldCount + 1; - - if((srxlRx.rcvr[i].rssiRcvd & RSSI_RCVD_DBM) && srxlRx.bestRssi_dBm < srxlRx.rcvr[i].rssi_dBm) - srxlRx.bestRssi_dBm = srxlRx.rcvr[i].rssi_dBm; - if((srxlRx.rcvr[i].rssiRcvd & RSSI_RCVD_PCT) && srxlRx.bestRssi_dBm < srxlRx.rcvr[i].rssi_Pct) - srxlRx.bestRssi_Pct = srxlRx.rcvr[i].rssi_Pct; - } - } - - // Set RSSI based on telemetry phase and type of telemetry received - srxlChData.rssi = (srxlTelemetryPhase || srxlChDataIsFailsafe) ? srxlRx.bestRssi_Pct : srxlRx.bestRssi_dBm; - - // Update flight log frame losses and holds - if(isFade && srxlRx.lossCountdown) - { - if(--srxlRx.lossCountdown == 0) - { - ++srxlRx.holds; - srxlRx.frameLosses -= srxlRx.lossHoldCount; - } - else - { - ++srxlRx.frameLosses; - } - } - - static uint8_t telemFadeCount = 0; - // If we are allowed to send telemetry by the device downstream (i.e. any slave device) - if(srxlRx.pTelemRcvr) - { - if(srxlRx.pTelemRcvr->channelMask == 0 || (srxlRx.pTelemRcvr->info & SRXL_DEVINFO_TELEM_FULL_RANGE) == 0) - { - // If our telemetry receiver missed channel data 3 frames in a row, switch - if(++telemFadeCount > 3) - { - srxlRx.pTelemRcvr = srxlChooseTelemRcvr(); -#ifdef SRXL_INCLUDE_MASTER_CODE - srxlSetTelemetryTxEnable(srxlRx.pTelemRcvr && (srxlRx.pTelemRcvr == srxlThisDev.pRcvr)); -#endif - telemFadeCount = 0; - } - } - else - { - telemFadeCount = 0; - } - } -#ifdef SRXL_INCLUDE_MASTER_CODE - // Else check to make sure we're still supposed to suppress telemetry (reset countdown when slave tells us not to send again) - else if(++srxlTelemSuppressCount > SRXL_TELEM_SUPPRESS_MAX) - { - // Enable this device's telemetry tx since we stopped being told not to - srxlRx.pTelemRcvr = srxlThisDev.pRcvr; - srxlSetTelemetryTxEnable(srxlRx.pTelemRcvr); - } -#endif - - // Return true while we're in hold condition (failsafe) - return srxlRx.lossCountdown == 0; -} +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wmissing-braces" +#pragma clang diagnostic ignored "-Wc99-designator" +// Since original is C and we want to compile it as C++ wrap it here +// so that we can disable some C++ warnings +#include "spm_srxl.library" +#pragma clang diagnostic pop \ No newline at end of file diff --git a/libraries/AP_RCProtocol/spm_srxl.library b/libraries/AP_RCProtocol/spm_srxl.library new file mode 100644 index 0000000000000..8dfcf495b58d2 --- /dev/null +++ b/libraries/AP_RCProtocol/spm_srxl.library @@ -0,0 +1,1430 @@ +/* +MIT License + +Copyright (c) 2019 Horizon Hobby, LLC + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +#include +#include + +#include "spm_srxl.h" + +/// LOCAL TYPES AND CONSTANTS /// + +#if(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_SPEED) +const uint16_t srxlCRCTable[] = +{ + 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, + 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, + 0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, + 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE, + 0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, + 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D, + 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, + 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC, + + 0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, + 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B, + 0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, + 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, + 0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, + 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49, + 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, + 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78, + + 0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, + 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067, + 0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, + 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256, + 0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, + 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, + 0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, + 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634, + + 0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, + 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3, + 0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, + 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92, + 0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, + 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1, + 0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, + 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0 +}; +#endif + +#define SRXL_TELEM_SUPPRESS_MAX (100) + +/// PUBLIC VARIABLES /// + +SrxlChannelData srxlChData = {0, 0, 0, {0}}; +SrxlTelemetryData srxlTelemData = {0}; +SrxlVtxData srxlVtxData = {0, 0, 1, 0, 0, 1}; + +/// LOCAL VARIABLES /// + +static SrxlDevice srxlThisDev = {0}; +static SrxlBus srxlBus[SRXL_NUM_OF_BUSES]; +static bool srxlChDataIsFailsafe = false; +static bool srxlTelemetryPhase = false; +#ifdef SRXL_INCLUDE_MASTER_CODE +static uint32_t srxlFailsafeChMask = 0; // Tracks all active channels for use during failsafe transmission +#endif +static SrxlBindData srxlBindInfo = {0, 0, 0, 0}; +static SrxlReceiverStats srxlRx = {0}; +static uint16_t srxlTelemSuppressCount = 0; + +#ifdef SRXL_INCLUDE_FWD_PGM_CODE +static SrxlFullID srxlFwdPgmDevice = {0, 0}; // Device that should accept Forward Programming connection by default +static uint8_t srxlFwdPgmBuffer[FWD_PGM_MAX_DATA_SIZE] = {0}; +static uint8_t srxlFwdPgmBufferLength = 0; +#endif + +// Include additional header and externs if using STM32 hardware acceleration +#if(SRXL_CRC_OPTIMIZE_MODE > SRXL_CRC_OPTIMIZE_SIZE) +#ifdef __cplusplus +extern "C" +{ +#endif +#if(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_STM_HAL) +#if(SRXL_STM_TARGET_FAMILY == SRXL_STM_TARGET_F7) +#include "stm32f7xx_hal.h" +#else +#include "stm32f3xx_hal.h" // Default to F3 if not given +#endif + extern CRC_HandleTypeDef hcrc; +#else +#if(SRXL_STM_TARGET_FAMILY == SRXL_STM_TARGET_F7) +#error "STM32F7 targets not yet supported for register-based STM HW optimization" +#else +#include "stm32f30x.h" // Default to F3 if not given +#endif +#endif +#ifdef __cplusplus +} +#endif +#endif + +/// LOCAL HELPER FUNCTIONS /// + +// Compute SRXL CRC over packet buffer (assumes length is correctly set) +static uint16_t srxlCrc16(uint8_t* packet) +{ + uint16_t crc = 0; // Seed with 0 + uint8_t length = packet[2] - 2; // Exclude 2 CRC bytes at end of packet from the length + + if(length <= SRXL_MAX_BUFFER_SIZE - 2) + { +#if(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_SIZE) + // Use bitwise method + for(uint8_t i = 0; i < length; ++i) + { + crc = crc ^ ((uint16_t)packet[i] << 8); + for(int b = 0; b < 8; b++) + { + if(crc & 0x8000) + crc = (crc << 1) ^ 0x1021; + else + crc = crc << 1; + } + } +#elif(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_STM_HW) + // Use direct STM32 HW CRC register access + uint8_t* pEnd = &packet[length]; + srxlEnterCriticalSection(); +#ifdef SRXL_SAVE_HW_CRC_CONTEXT + uint32 savedPOL = CRC->POL; + uint32 savedINIT = CRC->INIT; + uint32 savedCR = CRC->CR; + uint32 savedDR = CRC->DR; +#endif + CRC->POL = 0x1021; + CRC->INIT = 0; + CRC->CR = 0x09; // 16-bit polynomial, no input or output reversal, reset to init of 0 + while(packet < pEnd) + *(uint8_t*)(CRC_BASE) = *(packet++); + crc = (uint16_t)CRC->DR; +#ifdef SRXL_SAVE_HW_CRC_CONTEXT + // We have to use this convoluted method to restore things because writing INIT sets DR too + CRC->CR = 0; + CRC->POL = 0x04C11DB7; + CRC->INIT = savedINIT; + CRC->DR = savedINIT; + CRC->POL = savedDR; + CRC->DR = 1; + CRC->CR = savedCR; + CRC->POL = savedPOL; +#endif + srxlExitCriticalSection(); +#elif(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_STM_HAL) + // STM32f3/f7 hardware optimization using the STM32Cube libraries in HAL mode requires the following + // configuration, set in the STM32CubeMX "Pinout & Configuration" tab under "Computing > CRC": + // Basic Parameters + // Default Polynomial State = Disable + // CRC Length = 16-bit + // CRC Generating Polynomial = X12+X5+X0 + // Default Init Value State = Disable + // Init Value For CRC Computation = 0 + // Advanced Parameters + // Input Data Inversion Mode = None + // Output Data Inversion Mode = Disable + // Input Data Format = Bytes + crc = (uint16_t)HAL_CRC_Calculate(&hcrc, (uint32_t*)packet, length); +#elif(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_EXTERNAL) + crc = SRXL_CRC_CALCULATE(packet, length, crc); +#else + // Default to table-lookup method + uint8_t i; + for(i = 0; i < length; ++i) + { + // Get indexed position in lookup table using XOR of current CRC hi byte + uint8_t pos = (uint8_t)((crc >> 8) ^ packet[i]); + // Shift LSB up and XOR with the resulting lookup table entry + crc = (uint16_t)((crc << 8) ^ (uint16_t)(srxlCRCTable[pos])); + } +#endif + } + return crc; +} + +// Get the receiver entry for the requested bus and device ID +static inline SrxlRcvrEntry* srxlGetReceiverEntry(uint8_t busIndex, uint8_t deviceID) +{ + SrxlRcvrEntry* pRcvr = 0; + uint8_t i; + for(i = 0; i < srxlRx.rcvrCount; ++i) + { + if((srxlRx.rcvr[i].busBits & (1u << busIndex)) && (srxlRx.rcvr[i].deviceID == deviceID)) + { + pRcvr = &srxlRx.rcvr[i]; + break; + } + } + return pRcvr; +} + +// Add a new receiver entry for the given device +static inline SrxlRcvrEntry* srxlAddReceiverEntry(SrxlBus* pBus, SrxlDevEntry devEntry) +{ + // Only allow receivers (or flight controllers in certain circumstances) to be added + if(!pBus || devEntry.deviceID < 0x10 || devEntry.deviceID >= 0x40) + return 0; + + // If we didn't previously add this receiver, add it now if we have room + SrxlRcvrEntry* pRcvr = srxlGetReceiverEntry(pBus->fullID.busIndex, devEntry.deviceID); + if(!pRcvr) + { + if(srxlRx.rcvrCount >= SRXL_MAX_RCVRS) + return 0; + + uint8_t i = srxlRx.rcvrCount++; + pRcvr = &srxlRx.rcvr[i]; + pRcvr->deviceID = devEntry.deviceID; + pRcvr->busBits = (1u << pBus->fullID.busIndex); + pRcvr->info = devEntry.info; + + // If this receiver is full-range, insert into our sorted list after the other full-range telemetry receivers + if(pRcvr->info & SRXL_DEVINFO_TELEM_FULL_RANGE) + { + uint8_t n; + for(n = i; n > srxlRx.rcvrSortInsert; --n) + srxlRx.rcvrSorted[n] = srxlRx.rcvrSorted[n - 1]; + srxlRx.rcvrSorted[(srxlRx.rcvrSortInsert)++] = pRcvr; + } + // Else just tack onto the end + else + { + srxlRx.rcvrSorted[i] = pRcvr; + } + } + + // If this new receiver is a base receiver that supports telemetry or we haven't set a default active telemetry receiver, set it + if(!srxlRx.pTelemRcvr || (pRcvr->deviceID >= 0x20 && pRcvr->deviceID < 0x30 && (pRcvr->info & SRXL_DEVINFO_TELEM_TX_ENABLED))) + { + srxlRx.pTelemRcvr = pRcvr; + } + + return pRcvr; +} + +// Pick the best receiver to send telemetry on +static inline SrxlRcvrEntry* srxlChooseTelemRcvr(void) +{ + // If we only know about one receiver, set it to that + if(srxlRx.rcvrCount == 1) + return srxlRx.rcvrSorted[0]; + + // If we were previously sending telemetry + if(srxlRx.pTelemRcvr && srxlRx.pTelemRcvr->channelMask) + { + // If the current choice is not full-range + if((srxlRx.pTelemRcvr->info & SRXL_DEVINFO_TELEM_FULL_RANGE) == 0) + { + // Then see if there is a full-range choice that received channel data to switch to + uint8_t i; + for(i = 0; i < srxlRx.rcvrSortInsert; ++i) + { + if(srxlRx.rcvrSorted[i]->channelMask) + return srxlRx.rcvrSorted[i]; + } + } + + // Else keep using the current receiver + return srxlRx.pTelemRcvr; + } + // Else just pick the first one that got channel data this past frame + else + { + uint8_t i; + for(i = 0; i < srxlRx.rcvrCount; ++i) + { + if(srxlRx.rcvrSorted[i]->channelMask) + return srxlRx.rcvrSorted[i]; + } + } + + return 0; +} + +// Return pointer to device entry matching the given ID, or NULL if not found +static SrxlDevEntry* srxlGetDeviceEntry(SrxlBus* pBus, uint8_t deviceID) +{ + if(pBus) + { + uint8_t i; + for(i = 0; i < pBus->rxDevCount; ++i) + { + if(pBus->rxDev[i].deviceID == deviceID) + return &(pBus->rxDev[i]); + } + } + return 0; +} + +// Add an entry to our list of devices found on the SRXL bus (or update an entry if it already exists) +static SrxlDevEntry* srxlAddDeviceEntry(SrxlBus* pBus, SrxlDevEntry devEntry) +{ + // Don't allow broadcast or unknown device types to be added + if(!pBus || devEntry.deviceID < 0x10 || devEntry.deviceID > 0xEF) + return 0; + + // Limit device priority + if(devEntry.priority > 100) + devEntry.priority = 100; + + // Update device entry if it already exists + SrxlDevEntry* retVal = srxlGetDeviceEntry(pBus, devEntry.deviceID); + if(retVal) + { + pBus->rxDevPrioritySum -= retVal->priority; + *retVal = devEntry; + pBus->rxDevPrioritySum += retVal->priority; + } + // Else add to the list if we have room + else if(pBus->rxDevCount < SRXL_MAX_DEVICES) + { + retVal = &(pBus->rxDev[pBus->rxDevCount++]); + *retVal = devEntry; + pBus->rxDevPrioritySum += retVal->priority; + +#ifdef SRXL_INCLUDE_FWD_PGM_CODE + // If the new device supports Forward Programming and is a base receiver or flight controller, choose as the default + if(devEntry.info & SRXL_DEVINFO_FWD_PROG_SUPPORT) + { + uint8_t devType = devEntry.deviceID >> 4; + if(devType > (srxlFwdPgmDevice.deviceID >> 4) && devType < SrxlDevType_ESC) + { + srxlFwdPgmDevice.deviceID = devEntry.deviceID; + srxlFwdPgmDevice.busIndex = pBus->fullID.busIndex; + } + } +#endif + + // If the new device is a receiver, add to our receiver list + if(retVal->deviceID < 0x30) + { + srxlAddReceiverEntry(pBus, *retVal); + } + } + + return retVal; +} + +/// PUBLIC FUNCTIONS /// + +/** + @brief Initialize common SRXL info for this device + + @param deviceID: SRXL Device ID (see section 7.1.1 of SRXL2 Spec) + @param priority: Requested telemetry priority (1-100; typical is 10 per unique message type) + @param info: Device info bits (see SRXL_DEVINFO_XXX bits in spm_srxl.h) + @param uid: Unique 32-bit id to avoid device ID collision during handshake + @return bool: True if device info was successfully initialized +*/ +bool srxlInitDevice(uint8_t deviceID, uint8_t priority, uint8_t info, uint32_t uid) +{ + if(deviceID < 0x10 || deviceID > 0xEF) + return false; + + srxlThisDev.devEntry.deviceID = deviceID; + srxlThisDev.devEntry.info = info; + srxlThisDev.devEntry.priority = priority; + srxlThisDev.devEntry.rfu = 0; + srxlThisDev.uid = uid; + srxlThisDev.vtxProxy = false; + +#ifdef SRXL_INCLUDE_MASTER_CODE + // If this device is a receiver, add to our receiver info + if(deviceID < 0x30) + { + srxlInitReceiver(deviceID, info); + } +#endif + +#ifdef SRXL_INCLUDE_FWD_PGM_CODE + // If this device is a receiver or flight controller that supports Forward Programming, set as default + if((info & SRXL_DEVINFO_FWD_PROG_SUPPORT) && deviceID < 0x40) + { + srxlFwdPgmDevice.deviceID = deviceID; + srxlFwdPgmDevice.busIndex = 0; + } +#endif + +#if(SRXL_CRC_OPTIMIZE_MODE == SRXL_CRC_OPTIMIZE_STM_HW) + // Enable the peripheral clock for the HW CRC engine + RCC->AHBENR |= RCC_AHBENR_CRCEN; +#endif + + return true; +} + +/** + @brief Initialize bus settings for the given SRXL bus + + @param busIndex: Index into srxlBus array of bus entries + @param uart: Number to identify UART to which this SRXL bus should be connected + @param baudSupported: 0 = 115200 baud, 1 = 400000 baud + @return bool: True if SRXL bus was successfully initialized +*/ +bool srxlInitBus(uint8_t busIndex, uint8_t uart, uint8_t baudSupported) +{ + if(busIndex >= SRXL_NUM_OF_BUSES || !srxlThisDev.devEntry.deviceID) + return false; + + SrxlBus* pBus = &srxlBus[busIndex]; + pBus->state = SrxlState_ListenOnStartup; + pBus->fullID.deviceID = srxlThisDev.devEntry.deviceID; + pBus->fullID.busIndex = busIndex; + pBus->rxDevCount = 0; + pBus->rxDevPrioritySum = 0; + pBus->requestID = (srxlThisDev.devEntry.deviceID == 0x10) ? 0x11 : 0; + pBus->baudSupported = baudSupported; + pBus->baudRate = SRXL_BAUD_115200; + pBus->frameErrCount = 0; + pBus->uart = uart; + // Default remote receiver is automatically master -- everyone else figures it out during handshake + pBus->master = (srxlThisDev.devEntry.deviceID == 0x10); + pBus->pMasterRcvr = (srxlThisDev.devEntry.deviceID == 0x10) ? &srxlRx.rcvr[0] : 0; + pBus->initialized = true; + + return true; +} + +/** + @brief See if this device is the bus master on the given bus + + @param busIndex: Index into srxlBus array for the desired SRXL bus + @return bool: True if this device is the bus master on the given SRXL bus +*/ +bool srxlIsBusMaster(uint8_t busIndex) +{ + return (busIndex < SRXL_NUM_OF_BUSES && srxlBus[busIndex].master); +} + +/** + @brief Get the current SRXL state machine timeout count for the given bus + + @param busIndex: Index into srxlBus array for the desired SRXL bus + @return uint16_t: Timeout count in ms for the given SRXL bus +*/ +uint16_t srxlGetTimeoutCount_ms(uint8_t busIndex) +{ + return (busIndex < SRXL_NUM_OF_BUSES) ? srxlBus[busIndex].timeoutCount_ms : 0; +} + +/** + @brief Get the Device ID of this device on the given bus + + @param busIndex: Index into srxlBus array for the desired SRXL bus + @return uint8_t: Device ID of this device on the given SRXL bus +*/ +uint8_t srxlGetDeviceID(uint8_t busIndex) +{ + return (busIndex < SRXL_NUM_OF_BUSES) ? srxlBus[busIndex].fullID.deviceID : 0; +} + +/** + @brief Internal send function called by srxlRun() -- do not call in user code + + @param pBus: Pointer to SRXL bus entry for the desired SRXL bus + @param srxlCmd: Specific type of packet to send + @param replyID: Device ID of the device this Send command is targeting +*/ +static void srxlSend(SrxlBus* pBus, SRXL_CMD srxlCmd, uint8_t replyID) +{ + if(!pBus || !pBus->initialized) + return; + + memset(pBus->srxlOut.raw, 0, SRXL_MAX_BUFFER_SIZE); + pBus->srxlOut.header.srxlID = SPEKTRUM_SRXL_ID; + + // VTX Data + if(srxlCmd == SRXL_CMD_VTX) + { + pBus->srxlOut.header.packetType = SRXL_CTRL_ID; + pBus->srxlOut.header.length = SRXL_CTRL_BASE_LENGTH + sizeof(SrxlVtxData); + pBus->srxlOut.control.payload.cmd = SRXL_CTRL_CMD_VTX; + pBus->srxlOut.control.payload.replyID = replyID; + pBus->srxlOut.control.payload.vtxData = srxlVtxData; + } +#ifdef SRXL_INCLUDE_MASTER_CODE + // Channel Data + else if(srxlCmd == SRXL_CMD_CHANNEL || srxlCmd == SRXL_CMD_CHANNEL_FS) + { + pBus->srxlOut.header.packetType = SRXL_CTRL_ID; + uint32_t channelMask; + if(srxlCmd == SRXL_CMD_CHANNEL) + { + pBus->srxlOut.control.payload.cmd = SRXL_CTRL_CMD_CHANNEL; + pBus->srxlOut.control.payload.replyID = replyID; + + channelMask = srxlChData.mask; + } + else // == SRXL_CMD_CHANNEL_FS + { + // In failsafe mode, we dont want a telemetry reply + pBus->srxlOut.control.payload.cmd = SRXL_CTRL_CMD_CHANNEL_FS; + pBus->srxlOut.control.payload.replyID = 0; + + channelMask = srxlFailsafeChMask; + } + + // Set signal quality info (only a bus master sends this, so assume srxlChData contains the latest values) + pBus->srxlOut.control.payload.channelData.rssi = srxlChData.rssi; +#ifdef SRXL_IS_HUB + pBus->srxlOut.control.payload.channelData.frameLosses = srxlRx.frameLosses; +#else + pBus->srxlOut.control.payload.channelData.frameLosses = srxlRx.rcvr[0].fades; +#endif + + uint8_t channelIndex = 0; + uint32_t channelMaskBit = 1; + for(uint8_t i = 0; i < 32; ++i, channelMaskBit <<= 1) + { + if(channelMask & channelMaskBit) + { + pBus->srxlOut.control.payload.channelData.values[channelIndex++] = srxlChData.values[i]; + } + } + + // Set bits in packet for channels we populated, and clear those mask bits if it was part of a normal channel data command + pBus->srxlOut.control.payload.channelData.mask = channelMask; + if(srxlCmd == SRXL_CMD_CHANNEL) + srxlChData.mask &= ~channelMask; + + pBus->srxlOut.header.length = SRXL_CTRL_BASE_LENGTH + 7 + (2 * channelIndex); + } +#ifdef SRXL_INCLUDE_FWD_PGM_CODE + // Forward Programming Pass-thru + else if(srxlCmd == SRXL_CMD_FWDPGM) + { + pBus->srxlOut.header.packetType = SRXL_CTRL_ID; + pBus->srxlOut.header.length = SRXL_CTRL_BASE_LENGTH + 3 + srxlFwdPgmBufferLength; + pBus->srxlOut.control.payload.cmd = SRXL_CTRL_CMD_FWDPGM; + pBus->srxlOut.control.payload.replyID = replyID; + memcpy(pBus->srxlOut.control.payload.fpData.data, srxlFwdPgmBuffer, srxlFwdPgmBufferLength); + } +#endif // SRXL_INCLUDE_FWD_PGM_CODE +#endif // SRXL_INCLUDE_MASTER_CODE + // RSSI Data + else if(srxlCmd == SRXL_CMD_RSSI) + { + pBus->srxlOut.header.packetType = SRXL_RSSI_ID; + pBus->srxlOut.header.length = sizeof(SrxlRssiPacket); + pBus->srxlOut.rssi.request = SRXL_RSSI_REQ_REQUEST; // TODO: Needs to handle both directions! + pBus->srxlOut.rssi.antennaA = 0; // TODO: Fill in actual data later + pBus->srxlOut.rssi.antennaB = 0; + pBus->srxlOut.rssi.antennaC = 0; + pBus->srxlOut.rssi.antennaD = 0; + } + else if(srxlCmd == SRXL_CMD_HANDSHAKE) + { + pBus->srxlOut.header.packetType = SRXL_HANDSHAKE_ID; + pBus->srxlOut.header.length = sizeof(SrxlHandshakePacket); + pBus->srxlOut.handshake.payload.srcDevID = srxlThisDev.devEntry.deviceID; + pBus->srxlOut.handshake.payload.destDevID = replyID; + pBus->srxlOut.handshake.payload.priority = srxlThisDev.devEntry.priority; + pBus->srxlOut.handshake.payload.baudSupported = pBus->baudSupported; + pBus->srxlOut.handshake.payload.info = srxlThisDev.devEntry.info; + pBus->srxlOut.handshake.payload.uid = srxlThisDev.uid; + } + else if(srxlCmd == SRXL_CMD_TELEMETRY) + { + srxlFillTelemetry(&pBus->srxlOut.telemetry.payload); + pBus->srxlOut.header.packetType = SRXL_TELEM_ID; + pBus->srxlOut.header.length = sizeof(SrxlTelemetryPacket); + // If we successfully received a handshake from the bus master + if(pBus->pMasterRcvr) + { + // If we know that a device on this bus should send it, then target that device + if(srxlRx.pTelemRcvr && (srxlRx.pTelemRcvr->busBits & (1u << pBus->fullID.busIndex))) + pBus->srxlOut.telemetry.destDevID = srxlRx.pTelemRcvr->deviceID; + else + pBus->srxlOut.telemetry.destDevID = 0; + } + else + { + // Send 0xFF to tell bus master to re-send the handshake so we know where to direct telemetry in the future + pBus->srxlOut.telemetry.destDevID = 0xFF; + } + +#ifdef SRXL_INCLUDE_MASTER_CODE + if(srxlRx.pTelemRcvr && (pBus->srxlOut.telemetry.destDevID == srxlRx.pTelemRcvr->deviceID)) + { + // Don't mark telemetry as having been sent if we are sending it ourself over RF + if(pBus->srxlOut.telemetry.destDevID != srxlThisDev.pRcvr->deviceID) + { + srxlTelemetrySent(); + // Clear telemetry buffer after sending so we don't repeatedly display old data +// srxlTelemData.sensorID = srxlTelemData.secondaryID = 0; + } + } +#endif + } + else if(srxlCmd == SRXL_CMD_ENTER_BIND) + { + pBus->srxlOut.header.packetType = SRXL_BIND_ID; + pBus->srxlOut.header.length = sizeof(SrxlBindPacket); + pBus->srxlOut.bind.request = SRXL_BIND_REQ_ENTER; + pBus->srxlOut.bind.deviceID = replyID; + pBus->srxlOut.bind.data.type = DSMX_11MS; + pBus->srxlOut.bind.data.options = (replyID != 0xFF) ? SRXL_BIND_OPT_TELEM_TX_ENABLE | SRXL_BIND_OPT_BIND_TX_ENABLE : 0; + pBus->srxlOut.bind.data.guid = 0; + pBus->srxlOut.bind.data.uid = 0; + } + else if(srxlCmd == SRXL_CMD_REQ_BIND) + { + pBus->srxlOut.header.packetType = SRXL_BIND_ID; + pBus->srxlOut.header.length = sizeof(SrxlBindPacket); + pBus->srxlOut.bind.request = SRXL_BIND_REQ_STATUS; + pBus->srxlOut.bind.deviceID = replyID; + memset(&(pBus->srxlOut.bind.data), 0, sizeof(SrxlBindData)); + } + else if(srxlCmd == SRXL_CMD_SET_BIND) + { + pBus->srxlOut.header.packetType = SRXL_BIND_ID; + pBus->srxlOut.header.length = sizeof(SrxlBindPacket); + pBus->srxlOut.bind.request = SRXL_BIND_REQ_SET_BIND; + pBus->srxlOut.bind.deviceID = replyID; + pBus->srxlOut.bind.data = srxlBindInfo; + } + else if(srxlCmd == SRXL_CMD_BIND_INFO) + { + pBus->srxlOut.header.packetType = SRXL_BIND_ID; + pBus->srxlOut.header.length = sizeof(SrxlBindPacket); + pBus->srxlOut.bind.request = SRXL_BIND_REQ_BOUND_DATA; + pBus->srxlOut.bind.deviceID = replyID; + pBus->srxlOut.bind.data = srxlBindInfo; + } + + // Compute CRC over entire SRXL packet (excluding the 2 CRC bytes at the end) + uint16_t crc = srxlCrc16(pBus->srxlOut.raw); + + // Add CRC to packet in big-endian byte order + pBus->srxlOut.raw[pBus->srxlOut.header.length - 2] = (crc >> 8) & 0xFF; + pBus->srxlOut.raw[pBus->srxlOut.header.length - 1] = crc & 0xFF; + + // Send the packet out over the assigned UART + srxlSendOnUart(pBus->uart, pBus->srxlOut.raw, pBus->srxlOut.header.length); +} + +/** + @brief Parse an SRXL packet received on the given SRXL bus UART + + @param busIndex: Index of SRXL bus state information entry in the srxlBus array + @param packet: Pointer to received packet data + @param length: Length in bytes of received packet data + @return bool: True if a valid packet was received, else false +*/ +bool srxlParsePacket(uint8_t busIndex, uint8_t* packet, uint8_t length) +{ + // Validate parameters + if(busIndex >= SRXL_NUM_OF_BUSES || !packet || length < 5 || length > SRXL_MAX_BUFFER_SIZE) + return false; + + // Validate SRXL ID and length + if(packet[0] != SPEKTRUM_SRXL_ID || packet[2] != length) + return false; + + // Validate checksum + uint16_t crc = srxlCrc16(packet); + if((((uint16_t)packet[length - 2] << 8) | packet[length - 1]) != crc) + return false; + + // Copy packet into our unioned buffer to avoid "strict aliasing" violations + SrxlBus* pBus = &srxlBus[busIndex]; + SrxlPacket* pRx = &(pBus->srxlIn); + memcpy(pRx, packet, length); + + // Handle restart with ongoing communications -- bump to run state + pBus->timeoutCount_ms = 0; // TODO: Should we clear this even if packet isn't valid? + if(pBus->state < SrxlState_Running && pRx->header.packetType != SRXL_HANDSHAKE_ID) + pBus->state = SrxlState_Running; + + // Parse the specific data + switch(pRx->header.packetType) + { + case SRXL_CTRL_ID: // 0xCD + { + SrxlControlData* pCtrlData = &(pRx->control.payload); + + // Validate command + if(pCtrlData->cmd > SRXL_CTRL_CMD_FWDPGM) + break; + + // VTX + if(pCtrlData->cmd == SRXL_CTRL_CMD_VTX) + { + if(srxlSetVtxData(&pCtrlData->vtxData)) + srxlOnVtx(&srxlVtxData); + if(pCtrlData->replyID == pBus->fullID.deviceID || pCtrlData->replyID == 0xFF || srxlThisDev.vtxProxy) + { + // TODO: Should we ack this somehow + } + } +#ifdef SRXL_INCLUDE_FWD_PGM_CODE + // Forward Programming + else if(pCtrlData->cmd == SRXL_CTRL_CMD_FWDPGM) + { + if(pCtrlData->replyID == pBus->fullID.deviceID) + { + memcpy(srxlFwdPgmBuffer, pCtrlData->fpData.data, pRx->header.length - 3 - SRXL_CTRL_BASE_LENGTH); + srxlFwdPgmBufferLength = length; + if(pCtrlData->replyID == srxlFwdPgmDevice.deviceID) + { + // Handle Forward Programming command locally + srxlOnFwdPgm(srxlFwdPgmBuffer, srxlFwdPgmBufferLength); + } + else if(srxlFwdPgmDevice.deviceID && srxlBus[srxlFwdPgmDevice.busIndex].master) + { + // Pass it on through to the next target + srxlBus[srxlFwdPgmDevice.busIndex].txFlags.sendFwdPgmData = 1; + } + } + } +#endif + // Channel Data or Failsafe Data + else + { + bool isFailsafe = (pCtrlData->cmd == SRXL_CTRL_CMD_CHANNEL_FS); + srxlChData.rssi = pCtrlData->channelData.rssi; + srxlChData.frameLosses = pCtrlData->channelData.frameLosses; + if(pBus->pMasterRcvr) + { + if(pCtrlData->channelData.rssi < 0) + { + pBus->pMasterRcvr->rssi_dBm = pCtrlData->channelData.rssi; + pBus->pMasterRcvr->rssiRcvd |= RSSI_RCVD_DBM; + } + else + { + pBus->pMasterRcvr->rssi_Pct = pCtrlData->channelData.rssi; + pBus->pMasterRcvr->rssiRcvd |= RSSI_RCVD_PCT; + } + // If the receiver is sending alternating dBm/%, then use that as phase + if(pBus->pMasterRcvr->rssiRcvd == RSSI_RCVD_BOTH) + { + srxlTelemetryPhase = pCtrlData->channelData.rssi >= 0; + } + pBus->pMasterRcvr->fades = pCtrlData->channelData.frameLosses; + pBus->pMasterRcvr->channelMask = isFailsafe ? 0 : pCtrlData->channelData.mask; + srxlRx.rxBusBits |= pBus->pMasterRcvr->busBits; + } + + // Only save received channel values to srxlChData if it's normal channel data or we're in a hold condition + if(!isFailsafe || !srxlRx.lossCountdown) + { + uint8_t channelIndex = 0; + uint32_t channelMaskBit = 1; + uint8_t i; + for(i = 0; i < 32 && channelMaskBit <= pCtrlData->channelData.mask; ++i, channelMaskBit <<= 1) + { + if(pCtrlData->channelData.mask & channelMaskBit) + { + srxlChData.values[i] = pCtrlData->channelData.values[channelIndex++]; + srxlChData.mask |= channelMaskBit; + } + } + } + + srxlChDataIsFailsafe = isFailsafe; // TODO: Can we still assume this??? + srxlReceivedChannelData(&(pCtrlData->channelData), isFailsafe); + + // Figure out what type of reply packet to send, if any + if(pCtrlData->replyID == 0) + { + if(pBus->txFlags.enterBind) + { + pBus->state = SrxlState_SendEnterBind; + pBus->txFlags.enterBind = 0; + } + else if(pBus->txFlags.setBindInfo) + { + if(srxlRx.pBindRcvr) // TODO: Double-check this logic + { + pBus->requestID = srxlRx.pBindRcvr->deviceID; + pBus->state = SrxlState_SendSetBindInfo; + } + pBus->txFlags.setBindInfo = 0; + } + else if(pBus->txFlags.broadcastBindInfo) + { + pBus->requestID = 0xFF; + pBus->state = SrxlState_SendSetBindInfo; + pBus->txFlags.broadcastBindInfo = 0; + } + } + else if(pCtrlData->replyID == pBus->fullID.deviceID) + { + pBus->state = SrxlState_SendTelemetry; + } + } + break; + } + case SRXL_HANDSHAKE_ID: // 0x21 + { + if(length < sizeof(SrxlHandshakePacket)) + return false; + + // If this is an unprompted handshake (dest == 0) from a higher device ID, then we're the master + SrxlHandshakeData* pHandshake = &(pRx->handshake.payload); + if((pHandshake->destDevID == 0) && (pHandshake->srcDevID > pBus->fullID.deviceID)) + { + // Send a reply immediately to get the slave to shut up + pBus->state = SrxlState_SendHandshake; + pBus->requestID = pHandshake->srcDevID; + pBus->baudSupported = SRXL_SUPPORTED_BAUD_RATES; + srxlRun(busIndex, 0); + pBus->requestID = pBus->fullID.deviceID; + pBus->state = SrxlState_SendHandshake; + pBus->master = true; + pBus->pMasterRcvr = &srxlRx.rcvr[0]; + } + + // Add this device to our list of discovered devices + SrxlDevEntry newDev = {.deviceID = pHandshake->srcDevID, .priority = pHandshake->priority, .info = pHandshake->info}; + srxlAddDeviceEntry(pBus, newDev); + + // Bus master needs to track responses and poll next device + if(pBus->master) + { + // Keep track of baud rates supported + pBus->baudSupported &= pHandshake->baudSupported; + + // Make sure the state machine is set to poll -- this will eventually reach broadcast address (0xFF) + pBus->state = SrxlState_SendHandshake; + } + // Broadcast handshake sets the agreed upon baud rate for this bus + else if(pHandshake->destDevID == 0xFF) + { + // Get bus master receiver entry (if it's a flight controller, add it now as a receiver on this bus) + if(newDev.deviceID >= 0x30 && newDev.deviceID < 0x40) + { + pBus->pMasterRcvr = srxlAddReceiverEntry(pBus, newDev); + } + else + { + pBus->pMasterRcvr = srxlGetReceiverEntry(busIndex, newDev.deviceID); + } + + // Set baud rate and advance to run state + pBus->baudRate = pHandshake->baudSupported; + if(pBus->baudRate & SRXL_BAUD_400000) + { + srxlChangeBaudRate(pBus->uart, 400000); // Only alternate rate supported for now... + } + pBus->state = SrxlState_Running; + } + // Normal Handshake destined for this device should be replied to -- else ignore + else + { + if(pHandshake->destDevID == pBus->fullID.deviceID && pBus->state != SrxlState_SendHandshake) + { + pBus->requestID = pHandshake->srcDevID; + pBus->state = SrxlState_SendHandshake; + } + else + { + pBus->state = SrxlState_ListenForHandshake; + } + } + + break; + } + case SRXL_PARAM_ID: // 0x50 + { + // TODO: Add later + break; + } + case SRXL_RSSI_ID: // 0x55 + { + // TODO: Add later + break; + } + case SRXL_BIND_ID: // 0x41 + { + if(length < sizeof(SrxlBindPacket)) + return false; + + SrxlBindPacket* pBindInfo = &(pRx->bind); + + // If this is a bound data report + if(pBindInfo->request == SRXL_BIND_REQ_BOUND_DATA) + { + // Call the user-defined callback -- if returns true, bind all other receivers + SrxlFullID boundID; + boundID.deviceID = pBindInfo->deviceID; + boundID.busIndex = busIndex; + if(srxlOnBind(boundID, pBindInfo->data)) + { + // Update the bind info + srxlBindInfo.type = pBindInfo->data.type; + if(pBindInfo->data.options & SRXL_BIND_OPT_TELEM_TX_ENABLE) + { + SrxlRcvrEntry* pNewTelem = srxlGetReceiverEntry(busIndex, pBindInfo->deviceID); + if(pNewTelem || !srxlRx.pTelemRcvr) + srxlRx.pTelemRcvr = pNewTelem; + } + srxlBindInfo.options = 0; // Disable telemetry and Discovery reply when setting other receivers + srxlBindInfo.guid = pBindInfo->data.guid; + srxlBindInfo.uid = pBindInfo->data.uid; + + // Try to set bind info for all other receivers on other buses to match it + uint8_t b; + for(b = 0; b < SRXL_NUM_OF_BUSES; ++b) + { + if(b == busIndex) + continue; + srxlBus[b].txFlags.broadcastBindInfo = 1; + } + } + } + // If this bind packet is directed at us + else if(pBindInfo->deviceID == pBus->fullID.deviceID || pBindInfo->deviceID == 0xFF) + { + // Check for Enter Bind Mode (only valid if sent to a specific receiver) + if(pBindInfo->request == SRXL_BIND_REQ_ENTER) + { +#ifdef SRXL_INCLUDE_MASTER_CODE + srxlBindInfo.type = pBindInfo->data.type; + srxlBindInfo.options = pBindInfo->data.options; + srxlBindInfo.guid = 0; + srxlBindInfo.uid = 0; + srxlTryToBind(srxlBindInfo); +#endif + } + else if(pBindInfo->request == SRXL_BIND_REQ_STATUS && srxlThisDev.pRcvr) + { + // TODO: Fill in data if we didn't just bind? + pBus->txFlags.reportBindInfo = 1; + } + // Handle set bind info request + else if(pBindInfo->request == SRXL_BIND_REQ_SET_BIND) + { + srxlBindInfo = pBindInfo->data; +#ifdef SRXL_INCLUDE_MASTER_CODE + if(pBus->fullID.deviceID < 0x30) + srxlTryToBind(srxlBindInfo); +#endif + } + } + + break; + } + case SRXL_TELEM_ID: // 0x80 + { + if(length < sizeof(SrxlTelemetryPacket)) + return false; + + // NOTE: This data should be sent by exactly one telemetry device in response to a bus master request, + // so it is safe to update the global pTelemRcvr here even though this is a bus-specific function. + + SrxlTelemetryPacket* pTelem = &(pRx->telemetry); + memcpy(&srxlTelemData, &pTelem->payload, sizeof(srxlTelemData)); + // If the telemetry destination is set to broadcast, that indicates a request to re-handshake + if(pBus->master && pTelem->destDevID == 0xFF) + { + // If the master only found one device, don't poll again -- just tell the requesting device who we are + pBus->requestID = pBus->rxDevCount > 1 ? pBus->fullID.deviceID : 0xFF; + pBus->state = SrxlState_SendHandshake; + } + // If the incoming telemetry is destined for us, then we need to figure out who should send it over RF + else if(pTelem->destDevID == pBus->fullID.deviceID) + { + // This needs different logic for hubs versus endpoints +#ifdef SRXL_IS_HUB + if(srxlRx.pTelemRcvr == 0) + { + srxlRx.pTelemRcvr = srxlChooseTelemRcvr(); + } +#else + srxlRx.pTelemRcvr = srxlThisDev.pRcvr; +#endif + // Enable this device's telemetry tx based on whether we are the chosen telemetry receiver +#ifdef SRXL_INCLUDE_MASTER_CODE + srxlSetTelemetryTxEnable(srxlRx.pTelemRcvr && (srxlRx.pTelemRcvr == srxlThisDev.pRcvr)); +#endif + } + // Else turn off our telemetry and that of any receivers we might reply to via our own telemetry + else + { + srxlRx.pTelemRcvr = 0; +#ifdef SRXL_INCLUDE_MASTER_CODE + srxlSetTelemetryTxEnable(false); +#endif + } + + srxlTelemSuppressCount = 0; +#ifdef SRXL_INCLUDE_MASTER_CODE + srxlSuppressInternalTelemetry(&pTelem->payload); +#endif + break; + } + default: + { + break; + } + } + + // Run state machine for slave devices after each received packet + if(!pBus->master) + { + srxlRun(busIndex, 0); + } + + return true; +} + +/** + @brief Run the SRXL state machine after each receive or rx timeout + + @param busIndex: Index of SRXL bus state information entry in the srxlBus array + @param timeoutDelta_ms: Number of milliseconds to increment receive timeout if a timeout + occured, or <= 0 to clear timeout count upon packet receive. +*/ +void srxlRun(uint8_t busIndex, int16_t timeoutDelta_ms) +{ + SrxlBus* pBus = &srxlBus[busIndex]; + if(busIndex >= SRXL_NUM_OF_BUSES || !pBus->initialized || pBus->state == SrxlState_Disabled) + return; + + // Check receive timeout and advance state if needed + if(timeoutDelta_ms > 0) + { + pBus->timeoutCount_ms += timeoutDelta_ms; + if(pBus->timeoutCount_ms >= 30000) + pBus->timeoutCount_ms = 30000; + + if(pBus->timeoutCount_ms >= 50) + { + // After startup delay of 50ms, switch to handshake send or listen based on device unit ID + if(pBus->state == SrxlState_ListenOnStartup) + { + pBus->state = (pBus->fullID.deviceID & 0x0F) ? SrxlState_ListenForHandshake : SrxlState_SendHandshake; + } + // Reset non-master device back to startup conditions if 50ms elapses with no communications + else if(!pBus->master && pBus->state >= SrxlState_Running) + { + srxlChangeBaudRate(pBus->uart, 115200); + pBus->baudRate = SRXL_BAUD_115200; + pBus->timeoutCount_ms = 0; + pBus->requestID = 0; // Change back to 0 to indicate unprompted handshake from slave device + if(pBus->pMasterRcvr) + { + pBus->pMasterRcvr->channelMask = 0; + pBus->pMasterRcvr->fades = 0xFFFF; + pBus->pMasterRcvr->rssi_Pct = 0; + pBus->pMasterRcvr->rssi_dBm = -1; + } + pBus->state = SrxlState_ListenOnStartup; + } + } + } + else + { + pBus->timeoutCount_ms = 0; + } + + if(!pBus->master) + { + // Non-master actions for the given state + switch(pBus->state) + { + case SrxlState_SendHandshake: + { + srxlSend(pBus, SRXL_CMD_HANDSHAKE, pBus->requestID); + break; + } + case SrxlState_SendTelemetry: + { + srxlSend(pBus, SRXL_CMD_TELEMETRY, pBus->requestID); + pBus->state = SrxlState_Running; + break; + } + case SrxlState_SendVTX: + { + srxlSend(pBus, SRXL_CMD_VTX, pBus->requestID); + pBus->state = SrxlState_Running; + break; + } + case SrxlState_SendEnterBind: + { + if(srxlRx.pBindRcvr && (srxlRx.pBindRcvr != srxlThisDev.pRcvr)) + { + srxlSend(pBus, SRXL_CMD_ENTER_BIND, srxlRx.pBindRcvr->deviceID); + } + else + { + srxlBindInfo.options = 0; + srxlSend(pBus, SRXL_CMD_ENTER_BIND, 0xFF); + } + pBus->state = SrxlState_Running; + break; + } + case SrxlState_SendSetBindInfo: + { + srxlSend(pBus, SRXL_CMD_SET_BIND, pBus->requestID); + pBus->state = SrxlState_Running; + break; + } + case SrxlState_SendBoundDataReport: + { + srxlSend(pBus, SRXL_CMD_BIND_INFO, pBus->fullID.deviceID); + pBus->state = SrxlState_Running; + break; + } + case SrxlState_Running: + default: + { + return; + } + } + } +#ifdef SRXL_INCLUDE_MASTER_CODE + else + { + srxlRunMaster(pBus); + } +#endif // SRXL_INCLUDE_MASTER_CODE +} + +/** + @brief Tell the "best" receiver to enter bind mode, either locally or via SRXL command + + @param bindType: One of the possible bind status types to use when binding -- NOTE: The transmitter may ignore this + @param broadcast: True if this is a local request that should tell all connected receivers to enter bind + @return bool: True if a receiver was told to enter bind mode; else false +*/ +bool srxlEnterBind(uint8_t bindType, bool broadcast) +{ + srxlRx.pBindRcvr = 0; + if(broadcast && srxlThisDev.pRcvr) + { + srxlRx.pBindRcvr = srxlThisDev.pRcvr; + } + else if(srxlRx.pTelemRcvr) + { + srxlRx.pBindRcvr = srxlRx.pTelemRcvr; + } + else if(srxlRx.rcvrCount > 0 && srxlRx.rcvrSorted[0]->deviceID < 0x30) + { + srxlRx.pBindRcvr = srxlRx.rcvrSorted[0]; + } + + if(srxlRx.pBindRcvr) + { +#ifdef SRXL_INCLUDE_MASTER_CODE + // Local bind + if(srxlRx.pBindRcvr == srxlThisDev.pRcvr) + { + srxlBindInfo.type = bindType; + srxlBindInfo.options = SRXL_BIND_OPT_BIND_TX_ENABLE; + srxlBindInfo.guid = 0; + srxlBindInfo.uid = 0; + if(srxlRx.pBindRcvr == srxlRx.pTelemRcvr) + srxlBindInfo.options |= SRXL_BIND_OPT_TELEM_TX_ENABLE; + srxlTryToBind(srxlBindInfo); + if(broadcast) + { + uint8_t b; + for(b = 0; b < SRXL_NUM_OF_BUSES; ++b) + { + srxlBus[b].txFlags.enterBind = 1; + } + } + return true; + } +#endif // SRXL_INCLUDE_MASTER_CODE + + // Remote bind + uint8_t i; + for(i = 0; i < SRXL_NUM_OF_BUSES; ++i) + { + if((1u << i) & srxlRx.pBindRcvr->busBits) + { + srxlBus[i].txFlags.enterBind = 1; + return true; + } + } + } + + return false; +} + +/** + @brief Public function to set bind info for the system, either locally or via SRXL commands + + @param bindType: Type of bind requested for this receiver or all receivers + @param guid: Transmitter GUID to bind the receiver to + @param uid: Unique ID provided by transmitter upon initial bind (can be 0 if unknown) + @return bool: True if bind info was successfully set for the destination device; else false +*/ +bool srxlSetBindInfo(uint8_t bindType, uint64_t guid, uint32_t uid) +{ + if(guid == 0) + return false; + + // Set bind info, with options defaulted to 0 + srxlBindInfo.type = bindType; + srxlBindInfo.options = SRXL_BIND_OPT_US_POWER; // Request US power, with no guarantee it's supported + srxlBindInfo.guid = guid; + srxlBindInfo.uid = uid ? uid : srxlThisDev.uid; + +#ifdef SRXL_INCLUDE_MASTER_CODE + // If we are a receiver + if(srxlThisDev.pRcvr) + { + // Bind locally, which will result in no further packets since options == 0 + srxlTryToBind(srxlBindInfo); + } +#endif + + // Broadcast this bind info on all SRXL buses + uint8_t b; + for(b = 0; b < SRXL_NUM_OF_BUSES; ++b) + { + srxlBus[b].txFlags.broadcastBindInfo = 1; + } + + return true; +} + +/** + @brief Public function to call from the user UART code when a UART frame error occurs + + @param busIndex: Index of SRXL bus state information entry in the srxlBus array +*/ +void srxlOnFrameError(uint8_t busIndex) +{ + SrxlBus* pBus = &srxlBus[busIndex]; + if(busIndex >= SRXL_NUM_OF_BUSES || !pBus->initialized) + return; + + ++(pBus->frameErrCount); + if(pBus->master) + { + // TODO: If master (i.e. remote receiver 0x10), cause break condition to force reset? + return; + } + + if(pBus->state == SrxlState_ListenOnStartup || pBus->state == SrxlState_ListenForHandshake) + { + // Wait for multiple frame breaks before trying to change baud rate + if(pBus->frameErrCount < 3) + return; + + // Try the next higher baud rate + switch(pBus->baudRate) + { + case SRXL_BAUD_115200: + { + if(pBus->baudSupported & SRXL_BAUD_400000) + { + srxlChangeBaudRate(pBus->uart, 400000); + pBus->baudRate = SRXL_BAUD_400000; + break; + } + FALLTHROUGH; + // else fall thru... + } + case SRXL_BAUD_400000: + default: + { + // TODO: Cause break condition to force reset of everyone? Or just keep cycling? + + srxlChangeBaudRate(pBus->uart, 115200); + pBus->baudRate = SRXL_BAUD_115200; + break; + } + } + pBus->frameErrCount = 0; + } + else + { + // TODO: Handle frame error during normal comm -- probably caused by collision on bus? + } +} + +SrxlFullID srxlGetTelemetryEndpoint(void) +{ + SrxlFullID retVal = {0}; + if(srxlRx.pTelemRcvr) + { + retVal.deviceID = srxlRx.pTelemRcvr->deviceID; + retVal.busIndex = srxlRx.pTelemRcvr->busBits; + } + return retVal; +} + +bool srxlSetVtxData(SrxlVtxData* pVtxData) +{ + if(!pVtxData) + return false; + + // Update VTX data, ignoring values marked as unchanged + if(pVtxData->band != 0xFF) + srxlVtxData.band = pVtxData->band; + if(pVtxData->channel != 0xFF) + srxlVtxData.channel = pVtxData->channel; + if(pVtxData->pit != 0xFF) + srxlVtxData.pit = pVtxData->pit; + if(pVtxData->power != 0xFF || pVtxData->powerDec != 0xFFFF) + srxlVtxData.power = pVtxData->power; + if(pVtxData->powerDec != 0xFFFF) + srxlVtxData.powerDec = pVtxData->powerDec; + if(pVtxData->region != 0xFF) + srxlVtxData.region = pVtxData->region; + + uint8_t b; + for(b = 0; b < SRXL_NUM_OF_BUSES; ++b) + { + srxlBus[b].txFlags.sendVtxData = 1; + } + + return true; +} + +void srxlSetHoldThreshold(uint8_t countdownReset) +{ + srxlRx.lossHoldCount = (countdownReset > 1) ? countdownReset : 45; +} + +void srxlClearCommStats(void) +{ + srxlRx.holds = 0; + srxlRx.frameLosses = 0; + srxlRx.lossCountdown = srxlRx.lossHoldCount + 1; +} + +// Return true on failsafe hold +bool srxlUpdateCommStats(bool isFade) +{ + srxlRx.rxBusBits = 0; + if(srxlTelemetryPhase) + { + srxlRx.bestRssi_dBm = -128; + srxlRx.bestRssi_Pct = 0; + } + + uint8_t i; + for(i = 0; i < srxlRx.rcvrCount; ++i) + { + if(srxlRx.rcvr[i].channelMask) + { + srxlRx.lossCountdown = srxlRx.lossHoldCount + 1; + + if((srxlRx.rcvr[i].rssiRcvd & RSSI_RCVD_DBM) && srxlRx.bestRssi_dBm < srxlRx.rcvr[i].rssi_dBm) + srxlRx.bestRssi_dBm = srxlRx.rcvr[i].rssi_dBm; + if((srxlRx.rcvr[i].rssiRcvd & RSSI_RCVD_PCT) && srxlRx.bestRssi_dBm < srxlRx.rcvr[i].rssi_Pct) + srxlRx.bestRssi_Pct = srxlRx.rcvr[i].rssi_Pct; + } + } + + // Set RSSI based on telemetry phase and type of telemetry received + srxlChData.rssi = (srxlTelemetryPhase || srxlChDataIsFailsafe) ? srxlRx.bestRssi_Pct : srxlRx.bestRssi_dBm; + + // Update flight log frame losses and holds + if(isFade && srxlRx.lossCountdown) + { + if(--srxlRx.lossCountdown == 0) + { + ++srxlRx.holds; + srxlRx.frameLosses -= srxlRx.lossHoldCount; + } + else + { + ++srxlRx.frameLosses; + } + } + + static uint8_t telemFadeCount = 0; + // If we are allowed to send telemetry by the device downstream (i.e. any slave device) + if(srxlRx.pTelemRcvr) + { + if(srxlRx.pTelemRcvr->channelMask == 0 || (srxlRx.pTelemRcvr->info & SRXL_DEVINFO_TELEM_FULL_RANGE) == 0) + { + // If our telemetry receiver missed channel data 3 frames in a row, switch + if(++telemFadeCount > 3) + { + srxlRx.pTelemRcvr = srxlChooseTelemRcvr(); +#ifdef SRXL_INCLUDE_MASTER_CODE + srxlSetTelemetryTxEnable(srxlRx.pTelemRcvr && (srxlRx.pTelemRcvr == srxlThisDev.pRcvr)); +#endif + telemFadeCount = 0; + } + } + else + { + telemFadeCount = 0; + } + } +#ifdef SRXL_INCLUDE_MASTER_CODE + // Else check to make sure we're still supposed to suppress telemetry (reset countdown when slave tells us not to send again) + else if(++srxlTelemSuppressCount > SRXL_TELEM_SUPPRESS_MAX) + { + // Enable this device's telemetry tx since we stopped being told not to + srxlRx.pTelemRcvr = srxlThisDev.pRcvr; + srxlSetTelemetryTxEnable(srxlRx.pTelemRcvr); + } +#endif + + // Return true while we're in hold condition (failsafe) + return srxlRx.lossCountdown == 0; +}