diff --git a/docs/Serial.md b/docs/Serial.md index ca039910f5..da3ffacb34 100644 --- a/docs/Serial.md +++ b/docs/Serial.md @@ -76,21 +76,21 @@ Note: for Identifier see serialPortIdentifier_e in the source; for Function bitm | Identifier | Value | | -------------------------- | ----- | -| SERIAL_PORT_NONE | -1 | -| SERIAL_PORT_USART1 | 0 | -| SERIAL_PORT_USART2 | 1 | -| SERIAL_PORT_USART3 | 2 | -| SERIAL_PORT_UART4 | 3 | -| SERIAL_PORT_UART5 | 4 | -| SERIAL_PORT_USART6 | 5 | -| SERIAL_PORT_USART7 | 6 | -| SERIAL_PORT_USART8 | 7 | -| SERIAL_PORT_UART9 | 8 | -| SERIAL_PORT_USART10 | 9 | -| SERIAL_PORT_USB_VCP | 20 | -| SERIAL_PORT_SOFTSERIAL1 | 30 | -| SERIAL_PORT_SOFTSERIAL2 | 31 | -| SERIAL_PORT_LPUART1 | 40 | +| SERIAL\_PORT\_NONE | -1 | +| SERIAL\_PORT\_USART1 | 0 | +| SERIAL\_PORT\_USART2 | 1 | +| SERIAL\_PORT\_USART3 | 2 | +| SERIAL\_PORT\_UART4 | 3 | +| SERIAL\_PORT\_UART5 | 4 | +| SERIAL\_PORT\_USART6 | 5 | +| SERIAL\_PORT\_USART7 | 6 | +| SERIAL\_PORT\_USART8 | 7 | +| SERIAL\_PORT\_UART9 | 8 | +| SERIAL\_PORT\_USART10 | 9 | +| SERIAL\_PORT\_USB\_VCP | 20 | +| SERIAL\_PORT\_SOFTSERIAL1 | 30 | +| SERIAL\_PORT\_SOFTSERIAL2 | 31 | +| SERIAL\_PORT\_LPUART1 | 40 | ID's 0-19 reserved for UARTS 1-20 ID's 20-29 reserved for USB 1-10 @@ -102,25 +102,26 @@ Other devices can be added starting from id 50. | Function | Value | Bit | | ---------------------------- | ------ | --- | -| FUNCTION_NONE | 0 | 0 | -| FUNCTION_MSP | 1 | 1 << 0 | -| FUNCTION_GPS | 2 | 1 << 1 | -| FUNCTION_TELEMETRY_FRSKY_HUB | 4 | 1 << 2 | -| FUNCTION_TELEMETRY_HOTT | 8 | 1 << 3 | -| FUNCTION_TELEMETRY_LTM | 16 | 1 << 4 | -| FUNCTION_TELEMETRY_SMARTPORT | 32 | 1 << 5 | -| FUNCTION_RX_SERIAL | 64 | 1 << 6 | -| FUNCTION_BLACKBOX | 128 | 1 << 7 | -| NOT USED | 256 | 1 << 8 | -| FUNCTION_TELEMETRY_MAVLINK | 512 | 1 << 9 | -| FUNCTION_ESC_SENSOR | 1024 | 1 << 10 | -| FUNCTION_VTX_SMARTAUDIO | 2048 | 1 << 11 | -| FUNCTION_TELEMETRY_IBUS | 4096 | 1 << 12 | -| FUNCTION_VTX_TRAMP | 8192 | 1 << 13 | -| FUNCTION_RCDEVICE | 16384 | 1 << 14 | -| FUNCTION_LIDAR_TF | 32768 | 1 << 15 | -| FUNCTION_FRSKY_OSD | 65536 | 1 << 16 | -| FUNCTION_VTX_MSP | 131072 | 1 << 17 | +| FUNCTION\_NONE | 0 | 0 | +| FUNCTION\_MSP | 1 | 1 << 0 | +| FUNCTION\_GPS | 2 | 1 << 1 | +| FUNCTION\_TELEMETRY\_FRSKY_HUB | 4 | 1 << 2 | +| FUNCTION\_TELEMETRY\_HOTT | 8 | 1 << 3 | +| FUNCTION\_TELEMETRY\_LTM | 16 | 1 << 4 | +| FUNCTION\_TELEMETRY\_SMARTPORT | 32 | 1 << 5 | +| FUNCTION\_RX_SERIAL | 64 | 1 << 6 | +| FUNCTION\_BLACKBOX | 128 | 1 << 7 | +| NOT USED | 256 | 1 << 8 | +| FUNCTION\_TELEMETRY\_MAVLINK | 512 | 1 << 9 | +| FUNCTION\_ESC\_SENSOR | 1024 | 1 << 10 | +| FUNCTION\_VTX\_SMARTAUDIO | 2048 | 1 << 11 | +| FUNCTION\_TELEMETRY\_IBUS | 4096 | 1 << 12 | +| FUNCTION\_VTX\_TRAMP | 8192 | 1 << 13 | +| FUNCTION\_RCDEVICE | 16384 | 1 << 14 | +| FUNCTION\_LIDAR\_TF | 32768 | 1 << 15 | +| FUNCTION\_FRSKY\_OSD | 65536 | 1 << 16 | +| FUNCTION\_VTX\_MSP | 131072 | 1 << 17 | +| FUNCTION\_MSP\_DISPLAYPORT | 262145 | (1 << 18) \| FUNCTION\_MSP | Note: `FUNCTION_FRSKY_OSD` = `(1<<16)` requires 17 bits. Note2: We can use up to 32 bits (1<<32) here. @@ -204,7 +205,6 @@ The Serial Port baudrates are defined as follows: | 14 | 2000000 | | 15 | 2470000 | - ### Passthrough Betaflight can enter a special passthrough mode whereby it passes serial data through to a device connected to a UART/SoftSerial port. This is useful to change the configuration of a Betaflight peripheral such as an OSD, bluetooth dongle, serial RX etc. diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index faa6e0e020..16dbd78d07 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1509,7 +1509,6 @@ const clivalue_t valueTable[] = { #ifdef USE_MSP_DISPLAYPORT { "displayport_msp_col_adjust", VAR_INT8 | MASTER_VALUE, .config.minmax = { -6, 0 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, colAdjust) }, { "displayport_msp_row_adjust", VAR_INT8 | MASTER_VALUE, .config.minmax = { -3, 0 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, rowAdjust) }, - { "displayport_msp_serial", VAR_INT8 | MASTER_VALUE, .config.minmax = { SERIAL_PORT_NONE, SERIAL_PORT_IDENTIFIER_MAX }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, displayPortSerial) }, { "displayport_msp_attrs", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 4, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, attrValues) }, { "displayport_msp_use_device_blink", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, useDeviceBlink) }, #endif diff --git a/src/main/config/config.c b/src/main/config/config.c index d7fb90c66e..8f589bfbc3 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -57,6 +57,7 @@ #include "flight/position.h" #include "io/beeper.h" +#include "io/displayport_msp.h" #include "io/gps.h" #include "io/ledstrip.h" #include "io/serial.h" @@ -577,15 +578,17 @@ static void validateAndFixConfig(void) } #ifdef USE_MSP_DISPLAYPORT - // validate that displayport_msp_serial is referencing a valid UART that actually has MSP enabled - if (displayPortProfileMsp()->displayPortSerial != SERIAL_PORT_NONE) { - const serialPortConfig_t *portConfig = serialFindPortConfiguration(displayPortProfileMsp()->displayPortSerial); - if (!portConfig || !(portConfig->functionMask & FUNCTION_MSP) -#ifndef USE_MSP_PUSH_OVER_VCP - || (portConfig->identifier == SERIAL_PORT_USB_VCP) -#endif - ) { - displayPortProfileMspMutable()->displayPortSerial = SERIAL_PORT_NONE; + // Find the first serial port on which MSP Displayport is enabled + displayPortMspSetSerial(SERIAL_PORT_NONE); + + for (uint8_t serialPort = 0; serialPort < SERIAL_PORT_COUNT; serialPort++) { + const serialPortConfig_t *portConfig = &serialConfig()->portConfigs[serialPort]; + + if (portConfig && + (portConfig->identifier != SERIAL_PORT_USB_VCP) && + ((portConfig->functionMask & FUNCTION_MSP_DISPLAYPORT) == FUNCTION_MSP_DISPLAYPORT)) { + displayPortMspSetSerial(portConfig->identifier); + break; } } #endif diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c index 590b435ca6..768d19a0d7 100644 --- a/src/main/io/displayport_msp.c +++ b/src/main/io/displayport_msp.c @@ -43,6 +43,7 @@ #include "pg/vcd.h" static displayPort_t mspDisplayPort; +static serialPortIdentifier_e displayPortSerial; static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *buf, int len) { @@ -54,7 +55,7 @@ static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *buf, int len return 0; } #endif - return mspSerialPush(displayPortProfileMsp()->displayPortSerial, cmd, buf, len, MSP_DIRECTION_REPLY, MSP_V1); + return mspSerialPush(displayPortSerial, cmd, buf, len, MSP_DIRECTION_REPLY, MSP_V1); } static int heartbeat(displayPort_t *displayPort) @@ -191,4 +192,8 @@ displayPort_t *displayPortMspInit(void) redraw(&mspDisplayPort); return &mspDisplayPort; } + +void displayPortMspSetSerial(serialPortIdentifier_e serialPort) { + displayPortSerial = serialPort; +} #endif // USE_MSP_DISPLAYPORT diff --git a/src/main/io/displayport_msp.h b/src/main/io/displayport_msp.h index 12893386c9..a358da3b37 100644 --- a/src/main/io/displayport_msp.h +++ b/src/main/io/displayport_msp.h @@ -22,6 +22,8 @@ #include "drivers/display.h" +#include "io/serial.h" + #include "pg/displayport_profiles.h" // MSP Display Port commands @@ -36,3 +38,5 @@ #define DISPLAYPORT_MSP_ATTR_MASK (~(DISPLAYPORT_MSP_ATTR_VERSION|DISPLAYPORT_MSP_ATTR_BLINK)) struct displayPort_s *displayPortMspInit(void); +void displayPortMspSetSerial(serialPortIdentifier_e serialPort); + diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 1948f38175..1f6eefa2f4 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -272,9 +272,9 @@ serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e s } #ifdef USE_TELEMETRY -#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | TELEMETRY_PORT_FUNCTIONS_MASK | FUNCTION_VTX_MSP) +#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | TELEMETRY_PORT_FUNCTIONS_MASK | FUNCTION_VTX_MSP | FUNCTION_MSP_DISPLAYPORT) #else -#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | FUNCTION_VTX_MSP) +#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | FUNCTION_VTX_MSP | FUNCTION_MSP_DISPLAYPORT) #endif bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck) diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 61dd6bab46..3702f4b6ce 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -51,6 +51,7 @@ typedef enum { FUNCTION_LIDAR_TF = (1 << 15), // 32768 FUNCTION_FRSKY_OSD = (1 << 16), // 65536 FUNCTION_VTX_MSP = (1 << 17), // 131072 + FUNCTION_MSP_DISPLAYPORT = (1 << 18) | FUNCTION_MSP, // 262145 } serialPortFunction_e; #define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY_HUB | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK) diff --git a/src/main/pg/displayport_profiles.c b/src/main/pg/displayport_profiles.c index 2e702b6b54..6009c42a51 100644 --- a/src/main/pg/displayport_profiles.c +++ b/src/main/pg/displayport_profiles.c @@ -30,12 +30,7 @@ #if defined(USE_MSP_DISPLAYPORT) -PG_REGISTER_WITH_RESET_FN(displayPortProfile_t, displayPortProfileMsp, PG_DISPLAY_PORT_MSP_CONFIG, 0); - -void pgResetFn_displayPortProfileMsp(displayPortProfile_t *displayPortProfile) -{ - displayPortProfile->displayPortSerial = SERIAL_PORT_NONE; -} +PG_REGISTER(displayPortProfile_t, displayPortProfileMsp, PG_DISPLAY_PORT_MSP_CONFIG, 1); #endif diff --git a/src/main/pg/displayport_profiles.h b/src/main/pg/displayport_profiles.h index d7642c3655..23a4818395 100644 --- a/src/main/pg/displayport_profiles.h +++ b/src/main/pg/displayport_profiles.h @@ -28,7 +28,6 @@ typedef struct displayPortProfile_s { bool invert; uint8_t blackBrightness; uint8_t whiteBrightness; - int8_t displayPortSerial; // serialPortIdentifier_e // For attribute-rich OSDs