From 409bbab455e0dce37bfe3b1e40be45f623d383a6 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Fri, 2 Jun 2017 15:31:30 +1200 Subject: [PATCH] Apply 'telemetry_inversion' consistently to all protocols. --- src/main/fc/settings.c | 2 +- src/main/target/ALIENFLIGHTF3/config.c | 2 +- src/main/target/ALIENFLIGHTF4/config.c | 2 +- src/main/target/ALIENFLIGHTNGF7/config.c | 2 +- src/main/target/BLUEJAYF4/initialisation.c | 2 +- src/main/target/SPRACINGF3NEO/config.c | 1 - src/main/target/SPRACINGF4EVO/config.c | 2 +- src/main/target/TINYFISH/config.c | 2 +- src/main/telemetry/frsky.c | 2 +- src/main/telemetry/hott.c | 2 +- src/main/telemetry/ibus.c | 2 +- src/main/telemetry/ltm.c | 2 +- src/main/telemetry/mavlink.c | 2 +- src/main/telemetry/smartport.c | 10 +----- src/main/telemetry/telemetry.c | 4 +-- src/main/telemetry/telemetry.h | 2 +- src/test/unit/telemetry_ibus_unittest.cc | 39 +++++++++++----------- 17 files changed, 34 insertions(+), 46 deletions(-) diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index d1542a23d6..fe2ceacc1b 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -593,7 +593,7 @@ const clivalue_t valueTable[] = { // PG_TELEMETRY_CONFIG #ifdef TELEMETRY { "tlm_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_switch) }, - { "tlm_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inversion) }, + { "tlm_inverted", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inverted) }, { "tlm_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, halfDuplex) }, { "frsky_default_lat", VAR_INT16 | MASTER_VALUE, .config.minmax = { -9000, 9000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, gpsNoFixLatitude) }, { "frsky_default_long", VAR_INT16 | MASTER_VALUE, .config.minmax = { -18000, 18000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, gpsNoFixLongitude) }, diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index a6d3da9486..dee3884cb2 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -95,7 +95,7 @@ void targetConfiguration(void) rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; rxConfigMutable()->sbus_inversion = 0; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL; - telemetryConfigMutable()->telemetry_inversion = 0; + telemetryConfigMutable()->telemetry_inverted = true; featureSet(FEATURE_TELEMETRY); } diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index 0964a86131..560e8bdc43 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -65,7 +65,7 @@ void targetConfiguration(void) rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; rxConfigMutable()->sbus_inversion = 0; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL; - telemetryConfigMutable()->telemetry_inversion = 0; + telemetryConfigMutable()->telemetry_inverted = true; batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_ADC; batteryConfigMutable()->currentMeterSource = CURRENT_METER_ADC; featureSet(FEATURE_TELEMETRY); diff --git a/src/main/target/ALIENFLIGHTNGF7/config.c b/src/main/target/ALIENFLIGHTNGF7/config.c index 99c4f23781..c1cdbaad20 100644 --- a/src/main/target/ALIENFLIGHTNGF7/config.c +++ b/src/main/target/ALIENFLIGHTNGF7/config.c @@ -64,7 +64,7 @@ void targetConfiguration(void) rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; rxConfigMutable()->sbus_inversion = 0; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL; - telemetryConfigMutable()->telemetry_inversion = 0; + telemetryConfigMutable()->telemetry_inverted = true; batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_ADC; batteryConfigMutable()->currentMeterSource = CURRENT_METER_ADC; featureSet(FEATURE_TELEMETRY); diff --git a/src/main/target/BLUEJAYF4/initialisation.c b/src/main/target/BLUEJAYF4/initialisation.c index 9196d3ff8c..3f324c8e1a 100644 --- a/src/main/target/BLUEJAYF4/initialisation.c +++ b/src/main/target/BLUEJAYF4/initialisation.c @@ -53,7 +53,7 @@ void targetPreInit(void) serialPortConfig_t *portConfig = serialFindPortConfiguration(SERIAL_PORT_USART1); if (portConfig) { bool smartportEnabled = (portConfig->functionMask & FUNCTION_TELEMETRY_SMARTPORT); - if (smartportEnabled && (telemetryConfig()->telemetry_inversion) && (feature(FEATURE_TELEMETRY))) { + if (smartportEnabled && (!telemetryConfig()->telemetry_inverted) && (feature(FEATURE_TELEMETRY))) { high = true; } } diff --git a/src/main/target/SPRACINGF3NEO/config.c b/src/main/target/SPRACINGF3NEO/config.c index 8d106eb5c8..50757d0410 100644 --- a/src/main/target/SPRACINGF3NEO/config.c +++ b/src/main/target/SPRACINGF3NEO/config.c @@ -55,7 +55,6 @@ void targetConfiguration(void) serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; // So Bluetooth users don't have to change anything. serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = TELEMETRY_PROVIDER_DEFAULT; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(GPS_UART)].functionMask = FUNCTION_GPS; - telemetryConfigMutable()->telemetry_inversion = true; telemetryConfigMutable()->halfDuplex = true; } #endif diff --git a/src/main/target/SPRACINGF4EVO/config.c b/src/main/target/SPRACINGF4EVO/config.c index 8ea9e98586..4c00c94f01 100644 --- a/src/main/target/SPRACINGF4EVO/config.c +++ b/src/main/target/SPRACINGF4EVO/config.c @@ -38,6 +38,6 @@ void targetConfiguration(void) serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; // So SPRacingF3OSD users don't have to change anything. serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; telemetryConfigMutable()->halfDuplex = 0; - telemetryConfigMutable()->telemetry_inversion = 0; + telemetryConfigMutable()->telemetry_inverted = true; } #endif diff --git a/src/main/target/TINYFISH/config.c b/src/main/target/TINYFISH/config.c index 8a6a0c5bf3..bc84c72542 100644 --- a/src/main/target/TINYFISH/config.c +++ b/src/main/target/TINYFISH/config.c @@ -45,6 +45,6 @@ void targetConfiguration(void) rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; rxConfigMutable()->sbus_inversion = 0; - telemetryConfigMutable()->telemetry_inversion = 0; + telemetryConfigMutable()->telemetry_inverted = true; } #endif diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 0942779bc7..60a280c43b 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -472,7 +472,7 @@ void configureFrSkyTelemetryPort(void) return; } - frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inversion ? SERIAL_INVERTED : SERIAL_NOT_INVERTED); + frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED); if (!frskyPort) { return; } diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index a1e828d2e1..c2c3c09072 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -330,7 +330,7 @@ static void flushHottRxBuffer(void) static void workAroundForHottTelemetryOnUsart(serialPort_t *instance, portMode_t mode) { closeSerialPort(hottPort); - portOptions_t portOptions = SERIAL_NOT_INVERTED; + portOptions_t portOptions = telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED; if (telemetryConfig()->halfDuplex) { portOptions |= SERIAL_BIDIR; diff --git a/src/main/telemetry/ibus.c b/src/main/telemetry/ibus.c index b21c67f5f4..1105745385 100644 --- a/src/main/telemetry/ibus.c +++ b/src/main/telemetry/ibus.c @@ -152,7 +152,7 @@ void configureIbusTelemetryPort(void) return; } - ibusSerialPort = openSerialPort(ibusSerialPortConfig->identifier, FUNCTION_TELEMETRY_IBUS, NULL, IBUS_BAUDRATE, IBUS_UART_MODE, SERIAL_BIDIR); + ibusSerialPort = openSerialPort(ibusSerialPortConfig->identifier, FUNCTION_TELEMETRY_IBUS, NULL, IBUS_BAUDRATE, IBUS_UART_MODE, SERIAL_BIDIR | (telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED)); if (!ibusSerialPort) { return; diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 4a65899834..c5997f6306 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -283,7 +283,7 @@ void configureLtmTelemetryPort(void) if (baudRateIndex == BAUD_AUTO) { baudRateIndex = BAUD_19200; } - ltmPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_LTM, NULL, baudRates[baudRateIndex], TELEMETRY_LTM_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED); + ltmPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_LTM, NULL, baudRates[baudRateIndex], TELEMETRY_LTM_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED); if (!ltmPort) return; ltmEnabled = true; diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 8b7a896f6f..1cedab5bb6 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -158,7 +158,7 @@ void configureMAVLinkTelemetryPort(void) baudRateIndex = BAUD_57600; } - mavlinkPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_MAVLINK, NULL, baudRates[baudRateIndex], TELEMETRY_MAVLINK_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED); + mavlinkPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_MAVLINK, NULL, baudRates[baudRateIndex], TELEMETRY_MAVLINK_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED); if (!mavlinkPort) { return; diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index b2167550cc..8a372c116b 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -321,15 +321,7 @@ void configureSmartPortTelemetryPort(void) return; } - portOptions_t portOptions = 0; - - if (telemetryConfig()->halfDuplex) { - portOptions |= SERIAL_BIDIR; - } - - if (telemetryConfig()->telemetry_inversion) { - portOptions |= SERIAL_INVERTED; - } + portOptions_t portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED); smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index e7c0b6c551..39b8470c03 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -56,10 +56,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); -#define TELEMETRY_DEFAULT_INVERSION 1 - PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, - .telemetry_inversion = TELEMETRY_DEFAULT_INVERSION, + .telemetry_inverted = false, .halfDuplex = 1, .telemetry_switch = 0, .gpsNoFixLatitude = 0, diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 01ed096a7b..443dfa0c96 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -41,7 +41,7 @@ typedef struct telemetryConfig_s { int16_t gpsNoFixLatitude; int16_t gpsNoFixLongitude; 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 telemetry_inverted; uint8_t halfDuplex; frskyGpsCoordFormat_e frsky_coordinate_format; frskyUnit_e frsky_unit; diff --git a/src/test/unit/telemetry_ibus_unittest.cc b/src/test/unit/telemetry_ibus_unittest.cc index bdbe1b0199..04dcb69b3f 100644 --- a/src/test/unit/telemetry_ibus_unittest.cc +++ b/src/test/unit/telemetry_ibus_unittest.cc @@ -90,23 +90,23 @@ static bool telemetryDetermineEnabledState_stub_retval; void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros) { - EXPECT_EQ(taskId, TASK_TELEMETRY); - EXPECT_EQ(newPeriodMicros, 1000); + EXPECT_EQ(TASK_TELEMETRY, taskId); + EXPECT_EQ(1000, newPeriodMicros); } serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function) { - EXPECT_EQ(function, FUNCTION_TELEMETRY_IBUS); + EXPECT_EQ(FUNCTION_TELEMETRY_IBUS, function); return findSerialPortConfig_stub_retval; } portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialPortFunction_e function) { - EXPECT_EQ(portConfig, findSerialPortConfig_stub_retval); - EXPECT_EQ(function, FUNCTION_TELEMETRY_IBUS); + EXPECT_EQ(findSerialPortConfig_stub_retval, portConfig); + EXPECT_EQ(FUNCTION_TELEMETRY_IBUS, function); return PORTSHARING_UNUSED; } @@ -122,16 +122,16 @@ bool isSerialPortShared(const serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction) { - EXPECT_EQ(portConfig, findSerialPortConfig_stub_retval); - EXPECT_EQ(functionMask, FUNCTION_RX_SERIAL); - EXPECT_EQ(sharedWithFunction, FUNCTION_TELEMETRY_IBUS); + EXPECT_EQ(findSerialPortConfig_stub_retval, portConfig); + EXPECT_EQ(FUNCTION_RX_SERIAL, functionMask); + EXPECT_EQ(FUNCTION_TELEMETRY_IBUS, sharedWithFunction); return portIsShared; } serialPortConfig_t *findSerialPortConfig(uint16_t mask) { - EXPECT_EQ(mask, FUNCTION_TELEMETRY_IBUS); + EXPECT_EQ(FUNCTION_TELEMETRY_IBUS, mask); return findSerialPortConfig_stub_retval ; } @@ -147,24 +147,23 @@ serialPort_t *openSerialPort( { openSerial_called = true; (void) callback; - EXPECT_EQ(identifier, SERIAL_PORT_DUMMY_IDENTIFIER); - EXPECT_EQ(options, SERIAL_BIDIR); - EXPECT_EQ(function, FUNCTION_TELEMETRY_IBUS); - EXPECT_EQ(baudrate, 115200); - EXPECT_EQ(mode, MODE_RXTX); + EXPECT_EQ(SERIAL_PORT_DUMMY_IDENTIFIER, identifier); + EXPECT_EQ(SERIAL_BIDIR, options); + EXPECT_EQ(FUNCTION_TELEMETRY_IBUS, function); + EXPECT_EQ(115200, baudrate); + EXPECT_EQ(MODE_RXTX, mode); return &serialTestInstance; } - void closeSerialPort(serialPort_t *serialPort) { - EXPECT_EQ(serialPort, &serialTestInstance); + EXPECT_EQ(&serialTestInstance, serialPort); } void serialWrite(serialPort_t *instance, uint8_t ch) { - EXPECT_EQ(instance, &serialTestInstance); + EXPECT_EQ(&serialTestInstance, instance); EXPECT_LT(serialWriteStub.pos, sizeof(serialWriteStub.buffer)); serialWriteStub.buffer[serialWriteStub.pos++] = ch; serialReadStub.buffer[serialReadStub.end++] = ch; //characters echoes back on the shared wire @@ -174,7 +173,7 @@ void serialWrite(serialPort_t *instance, uint8_t ch) uint32_t serialRxBytesWaiting(const serialPort_t *instance) { - EXPECT_EQ(instance, &serialTestInstance); + EXPECT_EQ(&serialTestInstance, instance); EXPECT_GE(serialReadStub.end, serialReadStub.pos); int ret = serialReadStub.end - serialReadStub.pos; if (ret < 0) { @@ -187,7 +186,7 @@ uint32_t serialRxBytesWaiting(const serialPort_t *instance) uint8_t serialRead(serialPort_t *instance) { - EXPECT_EQ(instance, &serialTestInstance); + EXPECT_EQ(&serialTestInstance, instance); EXPECT_LT(serialReadStub.pos, serialReadStub.end); const uint8_t ch = serialReadStub.buffer[serialReadStub.pos++]; return ch; @@ -255,7 +254,7 @@ TEST_F(IbusTelemteryInitUnitTest, Test_IbusInitEnabled) handleIbusTelemetry(); //then all is read from serial port - EXPECT_EQ(serialReadStub.pos, serialReadStub.end); + EXPECT_EQ(serialReadStub.end, serialReadStub.pos); EXPECT_TRUE(openSerial_called); }