diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 377d32622..a2f936c0f 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -241,7 +241,7 @@ serialPort_t *openSerialPort( portOptions_t options) { serialPortUsage_t *serialPortUsage = findSerialPortUsageByIdentifier(identifier); - if (serialPortUsage->function != FUNCTION_NONE) { + if (!serialPortUsage->enabled || serialPortUsage->function != FUNCTION_NONE) { // already in use return NULL; } @@ -312,7 +312,7 @@ void closeSerialPort(serialPort_t *serialPort) { serialPortUsage->serialPort = NULL; } -void serialInit(serialConfig_t *initialSerialConfig) +void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled) { uint8_t index; @@ -322,6 +322,20 @@ void serialInit(serialConfig_t *initialSerialConfig) for (index = 0; index < SERIAL_PORT_COUNT; index++) { serialPortUsageList[index].identifier = serialPortIdentifiers[index]; + + serialPortUsageList[index].enabled = true; + +#ifdef USE_SOFTSERIAL1 + if (serialPortUsageList[index].identifier == SERIAL_PORT_SOFTSERIAL1) { + serialPortUsageList[index].enabled = softserialEnabled; + } +#endif + +#ifdef USE_SOFTSERIAL2 + if (serialPortUsageList[index].identifier == SERIAL_PORT_SOFTSERIAL2) { + serialPortUsageList[index].enabled = softserialEnabled; + } +#endif } } diff --git a/src/main/io/serial.h b/src/main/io/serial.h index e02bf8318..193dac556 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -69,6 +69,7 @@ typedef struct serialPortUsage_s { serialPortIdentifier_e identifier; serialPort_t *serialPort; serialPortFunction_e function; + bool enabled; } serialPortUsage_t; serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction); diff --git a/src/main/main.c b/src/main/main.c index 5a3126724..33de38dd1 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -100,7 +100,7 @@ serialPort_t *loopbackPort; void printfSupportInit(void); void timerInit(void); void telemetryInit(void); -void serialInit(serialConfig_t *initialSerialConfig); +void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled); void mspInit(serialConfig_t *serialConfig); void cliInit(serialConfig_t *serialConfig); void failsafeInit(rxConfig_t *intialRxConfig); @@ -191,7 +191,7 @@ void init(void) timerInit(); // timer must be initialized before any channel is allocated - serialInit(&masterConfig.serialConfig); + serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL)); mixerInit(masterConfig.mixerMode, masterConfig.customMixer);