serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction) { while (findSharedSerialPortState.lastIndex < SERIAL_PORT_COUNT) { serialPortConfig_t *candidate = &serialConfig->portConfigs[findSharedSerialPortState.lastIndex++]; if (isSerialPortShared(candidate, functionMask, sharedWithFunction)) { serialPortUsage_t *serialPortUsage = findSerialPortUsageByIdentifier(candidate->identifier); if (!serialPortUsage) { continue; } return serialPortUsage->serialPort; } } return NULL; }
void mspSerialAllocatePorts(void) { uint8_t portIndex = 0; serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP); while (portConfig && portIndex < MAX_MSP_PORT_COUNT) { mspPort_t *mspPort = &mspPorts[portIndex]; if (mspPort->port) { portIndex++; continue; } serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); if (serialPort) { bool sharedWithTelemetry = isSerialPortShared(portConfig, FUNCTION_MSP, TELEMETRY_PORT_FUNCTIONS_MASK); resetMspPort(mspPort, serialPort, sharedWithTelemetry); portIndex++; } portConfig = findNextSerialPortConfig(FUNCTION_MSP); } }
void configureIbusTelemetryPort(void) { if (!ibusSerialPortConfig) { return; } if (isSerialPortShared(ibusSerialPortConfig, FUNCTION_RX_SERIAL, FUNCTION_TELEMETRY_IBUS)) { // serialRx will open port and handle telemetry return; } ibusSerialPort = openSerialPort(ibusSerialPortConfig->identifier, FUNCTION_TELEMETRY_IBUS, NULL, NULL, IBUS_BAUDRATE, IBUS_UART_MODE, SERIAL_BIDIR | (telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED)); if (!ibusSerialPort) { return; } initSharedIbusTelemetry(ibusSerialPort); ibusTelemetryEnabled = true; outboundBytesToIgnoreOnRxCount = 0; }
bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxConfig); ibusSyncByte = 0; rxRuntimeConfig->channelCount = IBUS_MAX_CHANNEL; rxRuntimeConfig->rxRefreshRate = 20000; // TODO - Verify speed rxRuntimeConfig->rcReadRawFn = ibusReadRawRC; rxRuntimeConfig->rcFrameStatusFn = ibusFrameStatus; const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { return false; } #ifdef TELEMETRY bool portShared = isSerialPortShared(portConfig, FUNCTION_RX_SERIAL, FUNCTION_TELEMETRY_IBUS); #else bool portShared = false; #endif rxBytesToIgnore = 0; serialPort_t *ibusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, ibusDataReceive, IBUS_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED | (rxConfig->halfDuplex || portShared ? SERIAL_BIDIR : 0) ); #if defined(TELEMETRY) && defined(TELEMETRY_IBUS) if (portShared) { initSharedIbusTelemetry(ibusPort); } #endif return ibusPort != NULL; }