diff --git a/src/main/fc/config.c b/src/main/fc/config.c index eaa40a7ed6..d597fc128a 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -407,6 +407,7 @@ void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) void resetTelemetryConfig(telemetryConfig_t *telemetryConfig) { telemetryConfig->telemetry_inversion = 1; + telemetryConfig->sportHalfDuplex = 1; telemetryConfig->telemetry_switch = 0; telemetryConfig->gpsNoFixLatitude = 0; telemetryConfig->gpsNoFixLongitude = 0; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 43c45c1864..5783a3a170 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -787,6 +787,7 @@ const clivalue_t valueTable[] = { #ifdef TELEMETRY { "telemetry_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->telemetry_switch, .config.lookup = { TABLE_OFF_ON } }, { "telemetry_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->telemetry_inversion, .config.lookup = { TABLE_OFF_ON } }, + { "sport_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->sportHalfDuplex, .config.lookup = { TABLE_OFF_ON } }, { "frsky_default_lattitude", VAR_FLOAT | MASTER_VALUE, &telemetryConfig()->gpsNoFixLatitude, .config.minmax = { -90.0, 90.0 } }, { "frsky_default_longitude", VAR_FLOAT | MASTER_VALUE, &telemetryConfig()->gpsNoFixLongitude, .config.minmax = { -180.0, 180.0 } }, { "frsky_coordinates_format", VAR_UINT8 | MASTER_VALUE, &telemetryConfig()->frsky_coordinate_format, .config.minmax = { 0, FRSKY_FORMAT_NMEA } }, diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 4d1d19718a..d33ebb09a7 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -328,12 +328,14 @@ void configureSmartPortTelemetryPort(void) return; } - portOptions = SERIAL_BIDIR; - + if (telemetryConfig->sportHalfDuplex) { + portOptions = SERIAL_BIDIR; + } + if (telemetryConfig->telemetry_inversion) { portOptions |= SERIAL_INVERTED; } - + smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions); if (!smartPortSerialPort) { diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index f656578b95..91b2ff004a 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -37,6 +37,7 @@ typedef enum { typedef struct telemetryConfig_s { uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed. uint8_t telemetry_inversion; // also shared with smartport inversion + uint8_t sportHalfDuplex; float gpsNoFixLatitude; float gpsNoFixLongitude; frskyGpsCoordFormat_e frsky_coordinate_format;