Implement sharing an UART for IBUS SerialRX and telemetry (kudos to @hali9 and @unitware)

Makefile

@@ -599,6 +599,7 @@ HIGHEND_SRC = \
             telemetry/crsf.c \
             telemetry/frsky.c \
             telemetry/hott.c \
+	    telemetry/ibus_shared.c \
             telemetry/ibus.c \
             telemetry/ltm.c \
             telemetry/mavlink.c \

docs/Rx.md

@@ -126,12 +126,22 @@ SUMH is a legacy Graupner protocol.  Graupner have issued a firmware updates for
 
 10 channels via serial currently supported.
 
-IBUS is the FlySky digital serial protocol and is available with the FS-IA6B and
+IBUS is the FlySky digital serial protocol and is available with the FS-IA6B and FS-IA10 receivers. 
-FS-IA10 receivers. The Turnigy TGY-IA6B and TGY-IA10 are the same
+The Turnigy TGY-IA6B and TGY-IA10 are the same devices with a different label, therefore they also work.
-devices with a different label, therefore they also work.
 
 If you are using a 6ch tx such as the FS-I6 or TGY-I6 then you must flash a 10ch
 firmware on the tx to make use of these extra channels.
+The flash is avaliable here: https://github.com/benb0jangles/FlySky-i6-Mod-
+```
+     _______
+    /       \                                              /------------\
+    | STM32 |-->UART TX-->[Bi-directional @ 115200 baud]-->| Flysky RX  |
+    |  uC   |-  UART RX--x[not connected]                  | IBUS-Servo |
+    \_______/                                              \------------/
+```
+After flash "10ch Timer Mod i6 Updater", it is passible to get RSSI signal on selected Aux channel from FS-i6 Err sensor.
+
+It is possible to use ibus RX and ibus telemetry on only one port of the hardware UART. More information in Telemetry.md.
 
 ## MultiWii serial protocol (MSP)
 
@@ -236,6 +246,9 @@ For Serial RX enable `RX_SERIAL` and set the `serialrx_provider` CLI setting as
 | SUMH               | 4     |
 | XBUS_MODE_B        | 5     |
 | XBUS_MODE_B_RJ01   | 6     |
+| SERIALRX_IBUS      | 7     |
+| SERIALRX_JETIEXBUS | 8     |
+| SERIALRX_CRSF      | 9     |
 
 ### PPM/PWM input filtering.
 

docs/Telemetry.md

@@ -176,19 +176,19 @@ It shares 1 line for both TX and RX, the rx pin cannot be used for other serial
 It runs at a fixed baud rate of 115200, so it need hardware uart (softserial is limit to 19200).
 ```
      _______
-    /       \                                              /---------\
+    /       \                                              /-------------\
-    | STM32 |-->UART TX-->[Bi-directional @ 115200 baud]-->| IBUS RX |
+    | STM32 |-->UART TX-->[Bi-directional @ 115200 baud]-->| Flysky RX   |
-    |  uC   |-  UART RX--x[not connected]                  \---------/
+    |  uC   |-  UART RX--x[not connected]                  | IBUS-Sensor |
-    \_______/
+    \_______/                                              \-------------/
 ```
 It is possible to daisy chain multiple sensors with ibus, but telemetry sensor will be overwrite by value sensor.
 In this case sensor should be connected to RX and FC to sensor.
 ```
      _______
-    /       \                                              /---------\   /-------------\   /---------\
+    /       \                                              /---------\   /-------------\   /-------------\
-    | STM32 |-->UART TX-->[Bi-directional @ 115200 baud]-->| CVT-01  |-->|others sensor|-->| IBUS RX |
+    | STM32 |-->UART TX-->[Bi-directional @ 115200 baud]-->| CVT-01  |-->|others sensor|-->| Flysky RX   |
-    |  uC   |-  UART RX--x[not connected]                  \---------/   \-------------/   \---------/
+    |  uC   |-  UART RX--x[not connected]                  \---------/   \-------------/   | IBUS-Sensor |
-    \_______/
+    \_______/                                                                              \-------------/
 ```
 
 ### Configuration
@@ -248,7 +248,7 @@ FIX: 1 is No, 2 is 2D, 3 is 3D, 6 is No+FixHome, 7 is 2D+FixHome, 8 is 3D+FixHom
 
 HDOP: 0 is 0-9m, 8 is 80-90m, 9 is >90m
 
-Mode: 1-Armed(rate), 2-Horizon, 3-Angle, 4-HeadFree or Mag, 5-AltHold, 6-PosHold, 7-Rth, 8-Fail and Rth, 9-Fail
+Mode: 0 - Passthrough, 1-Armed(rate), 2-Horizon, 3-Angle, 4-Waypoint, 5-AltHold, 6-PosHold, 7-Rth, 8-Launch, 9-Failsafe
 
 Example: 12803 is 12 satelites, Fix3D, FixHome, 0-9m HDOP, Angle Mode
 
@@ -270,3 +270,30 @@ These receivers are reported to work with i-bus telemetry:
 - FlySky/Turnigy FS-iA10B 10-Channel Receiver (http://www.flysky-cn.com/products_detail/productId=52.html)
 
 Note that the FlySky/Turnigy FS-iA4B 4-Channel Receiver (http://www.flysky-cn.com/products_detail/productId=46.html) seems to work but has a bug that might lose the binding, DO NOT FLY the FS-iA4B!
+
+### Use ibus RX and ibus telemetry on only one port.
+
+Case:
+
+A. For use only IBUS RX connect directly Flysky IBUS-SERVO to FC-UART-TX.
+In configurator set RX on selected port, set receiver mode to RX_SERIAL and Receiver provider to IBUS.
+
+B. For use only IBUS telemetry connect directly Flysky IBUS-SENS to FC-UART-TX.
+In configurator set IBUS telemetry on selected port and enable telemetry feature.
+
+C. For use RX IBUS and telemetry IBUS together connect Flysky IBUS-SERVO and IBUS-SENS to FC-UART-TX using schematic:
+```
++---------+
+| FS-iA6B |
+|         |
+| Servo   |---|<---\       +------------+
+|         |        |       | FC         |
+| Sensor  |---[R]--*-------| Serial TX  |
++---------+                +------------+
+```
+R = 10Kohm, Diode 1N4148 (connect cathode to IBUS-Servo of Flysky receiver).
+
+In configurator set IBUS telemetry and RX on this same port, enable telemetry feature, set receiver mode to RX_SERIAL and Receiver provider to IBUS.
+
+Warning:
+Schematic above work also for connect telemetry only, but not work for connect rx only - will stop FC.

src/main/rx/ibus.c

@@ -38,41 +38,59 @@
 
 #include "io/serial.h"
 
+#include "telemetry/ibus.h"
+#include "telemetry/ibus_shared.h"
+    
 #include "rx/rx.h"
 #include "rx/ibus.h"
 
 
-#define IBUS_MAX_CHANNEL 10
+#define IBUS_MAX_CHANNEL (10)
-#define IBUS_BUFFSIZE 32
+#define IBUS_TELEMETRY_PACKET_LENGTH (4)
-#define IBUS_SYNCBYTE 0x20
+#define IBUS_SERIAL_RX_PACKET_LENGTH (32)
-
-#define IBUS_BAUDRATE 115200
 
+static uint8_t rxBytesToIgnore = 0;
+static uint8_t ibusFramePosition = 0;
 static bool ibusFrameDone = false;
 static uint32_t ibusChannelData[IBUS_MAX_CHANNEL];
+static uint8_t ibus[IBUS_SERIAL_RX_PACKET_LENGTH] = { 0, };
 
-static uint8_t ibus[IBUS_BUFFSIZE] = { 0, };
+static bool isValidIbusPacketLength(uint8_t length) {
+    return (length == IBUS_TELEMETRY_PACKET_LENGTH) || (length == IBUS_SERIAL_RX_PACKET_LENGTH);
+}
 
 // Receive ISR callback
 static void ibusDataReceive(uint16_t c)
 {
     timeUs_t ibusTime;
     static timeUs_t ibusTimeLast;
-    static uint8_t ibusFramePosition;
+    static uint8_t ibusLength = 0;
 
     ibusTime = micros();
 
-    if (cmpTimeUs(ibusTime, ibusTimeLast) > 3000)
+    if (cmpTimeUs(ibusTime, ibusTimeLast) > 3000) {
         ibusFramePosition = 0;
+        ibusLength = 0;
+        rxBytesToIgnore = 0;
+    }
+    if (rxBytesToIgnore) {
+        rxBytesToIgnore--;
+        return;
+    }
 
     ibusTimeLast = ibusTime;
 
-    if (ibusFramePosition == 0 && c != IBUS_SYNCBYTE)
+    if (ibusFramePosition == 0) {
-        return;
+       if (isValidIbusPacketLength(c)) {
+           ibusLength = c;
+       } else {
+           return;
+       }
+    }
 
     ibus[ibusFramePosition] = (uint8_t)c;
 
-    if (ibusFramePosition == IBUS_BUFFSIZE - 1) {
+    if (ibusFramePosition == ibusLength - 1) {
         ibusFrameDone = true;
     } else {
         ibusFramePosition++;
@@ -81,36 +99,30 @@ static void ibusDataReceive(uint16_t c)
 
 uint8_t ibusFrameStatus(void)
 {
-    uint8_t i;
+    uint8_t i, offset;
     uint8_t frameStatus = RX_FRAME_PENDING;
-    uint16_t chksum, rxsum;
+    uint8_t ibusLength;
 
     if (!ibusFrameDone) {
         return frameStatus;
     }
-
+    
+    ibusLength = ibus[0];
     ibusFrameDone = false;
-
+   
-    chksum = 0xFFFF;
+    if (isChecksumOk(ibus, ibusLength)) {
-    for (i = 0; i < 30; i++)
+        if (ibusLength == IBUS_SERIAL_RX_PACKET_LENGTH) {
-        chksum -= ibus[i];
+            for (i = 0, offset = 2; i < IBUS_MAX_CHANNEL; i++, offset += 2) {
-
+                ibusChannelData[i] = ibus[offset] + (ibus[offset + 1] << 8);
-    rxsum = ibus[30] + (ibus[31] << 8);
+            }
-
+            frameStatus = RX_FRAME_COMPLETE;
-    if (chksum == rxsum) {
+        } else {
-        ibusChannelData[0] = (ibus[ 3] << 8) + ibus[ 2];
+#if defined(TELEMETRY) && defined(TELEMETRY_IBUS)
-        ibusChannelData[1] = (ibus[ 5] << 8) + ibus[ 4];
+            rxBytesToIgnore = respondToIbusRequest(ibus);
-        ibusChannelData[2] = (ibus[ 7] << 8) + ibus[ 6];
+#endif
-        ibusChannelData[3] = (ibus[ 9] << 8) + ibus[ 8];
+        }
-        ibusChannelData[4] = (ibus[11] << 8) + ibus[10];
-        ibusChannelData[5] = (ibus[13] << 8) + ibus[12];
-        ibusChannelData[6] = (ibus[15] << 8) + ibus[14];
-        ibusChannelData[7] = (ibus[17] << 8) + ibus[16];
-        ibusChannelData[8] = (ibus[19] << 8) + ibus[18];
-        ibusChannelData[9] = (ibus[21] << 8) + ibus[20];
-
-        frameStatus = RX_FRAME_COMPLETE;
     }
+    ibusFramePosition = 0;
 
     return frameStatus;
 }
@@ -123,10 +135,11 @@ static uint16_t ibusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t
 
 bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 {
+    serialPort_t *ibusPort = NULL;
     UNUSED(rxConfig);
 
     rxRuntimeConfig->channelCount = IBUS_MAX_CHANNEL;
-    rxRuntimeConfig->rxRefreshRate = 20000; // TODO - Verify speed
+    rxRuntimeConfig->rxRefreshRate = 20000; // TODO - Verify speed 11000 ?
 
     rxRuntimeConfig->rcReadRawFn = ibusReadRawRC;
     rxRuntimeConfig->rcFrameStatusFn = ibusFrameStatus;
@@ -135,9 +148,18 @@ bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
     if (!portConfig) {
         return false;
     }
-
+    rxBytesToIgnore = 0;
-    serialPort_t *ibusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, ibusDataReceive, IBUS_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED);
+#if defined(TELEMETRY) && defined(TELEMETRY_IBUS)
-
+    if (isSerialPortShared(portConfig, FUNCTION_RX_SERIAL, FUNCTION_TELEMETRY_IBUS)) {
+        //combined, open port as bidirectional
+        ibusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, ibusDataReceive, IBUS_BAUDRATE, MODE_RXTX, SERIAL_BIDIR | SERIAL_NOT_INVERTED);
+        initSharedIbusTelemetry(ibusPort);
+    } else 
+#endif
+    {
+        //ibusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, ibusDataReceive, IBUS_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED);
+        ibusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, ibusDataReceive, IBUS_BAUDRATE, MODE_RXTX, SERIAL_BIDIR | SERIAL_NOT_INVERTED);
+    }    
     return ibusPort != NULL;
 }
 #endif // USE_SERIALRX_IBUS

src/main/telemetry/ibus.c

@@ -28,7 +28,7 @@
 
 #include "platform.h"
 
-#ifdef TELEMETRY
+#if defined(TELEMETRY) && defined(TELEMETRY_IBUS)
 
 #include "common/maths.h"
 #include "common/axis.h"
@@ -55,6 +55,7 @@
 #include "navigation/navigation.h"
 
 #include "telemetry/ibus.h"
+#include "telemetry/ibus_shared.h"
 #include "telemetry/telemetry.h"
 #include "fc/config.h"
 #include "config/feature.h"
@@ -165,205 +166,14 @@
  *
  */
 
-#define IBUS_TASK_PERIOD_US (500)
-#define IBUS_UART_MODE     (MODE_RXTX)
-#define IBUS_BAUDRATE      (115200)
-#define IBUS_CYCLE_TIME_MS (8)
-
-#define IBUS_CHECKSUM_SIZE (2)
-
-#define IBUS_MIN_LEN       (2 + IBUS_CHECKSUM_SIZE)
-#define IBUS_MAX_TX_LEN    (6)
-#define IBUS_MAX_RX_LEN    (4)
-#define IBUS_RX_BUF_LEN    (IBUS_MAX_RX_LEN)
-
-#define IBUS_TEMPERATURE_OFFSET (0x0190)
-
-typedef uint8_t ibusAddress_t;
-
-typedef enum {
-    IBUS_COMMAND_DISCOVER_SENSOR      = 0x80,
-    IBUS_COMMAND_SENSOR_TYPE          = 0x90,
-    IBUS_COMMAND_MEASUREMENT          = 0xA0
-} ibusCommand_e;
-
-typedef enum {
-    IBUS_MEAS_TYPE_INTERNAL_VOLTAGE = 0x00, // Internal Voltage
-    IBUS_MEAS_TYPE_TEMPERATURE      = 0x01, // Temperature -##0.0 C, 0=-40.0 C, 400=0.0 C, 65535=6513.5 C
-    IBUS_MEAS_TYPE_RPM              = 0x02, // Rotation RPM, ####0RPM, 0=0RPM, 65535=65535RPM
-    IBUS_MEAS_TYPE_EXTERNAL_VOLTAGE = 0x03, // External Voltage, -##0.00V, 0=0.00V, 32767=327.67V, 32768=na, 32769=-327.67V, 65535=-0.01V
-    IBUS_MEAS_TYPE_PRES             = 0x41, // Pressure, not work
-    IBUS_MEAS_TYPE_ODO1             = 0x7c, // Odometer1, 0.0km, 0.0 only
-    IBUS_MEAS_TYPE_ODO2             = 0x7d, // Odometer2, 0.0km, 0.0 only
-    IBUS_MEAS_TYPE_SPE              = 0x7e, // Speed km/h, ###0km/h, 0=0Km/h, 1000=100Km/h
-    IBUS_MEAS_TYPE_ALT              = 0xf9, // Altitude m, not work
-    IBUS_MEAS_TYPE_SNR              = 0xfa, // SNR, not work
-    IBUS_MEAS_TYPE_NOISE            = 0xfb, // Noise, not work
-    IBUS_MEAS_TYPE_RSSI             = 0xfc, // RSSI, not work
-    IBUS_MEAS_TYPE_ERR              = 0xfe  // Error rate, #0%
-} ibusSensorType_e;
-
-static uint8_t SENSOR_ADDRESS_TYPE_LOOKUP[] = {
-    IBUS_MEAS_TYPE_INTERNAL_VOLTAGE,  // Address 0, sensor 1, not usable since it is reserved for internal voltage
-    IBUS_MEAS_TYPE_EXTERNAL_VOLTAGE,  // Address 1 ,sensor 2, VBAT
-    IBUS_MEAS_TYPE_TEMPERATURE,       // Address 2, sensor 3, Baro/Gyro Temp	
-    IBUS_MEAS_TYPE_RPM,               // Address 3, sensor 4, Status AS RPM
-    IBUS_MEAS_TYPE_RPM,               // Address 4, sensor 5, MAG_COURSE in deg AS RPM
-    IBUS_MEAS_TYPE_EXTERNAL_VOLTAGE,  // Address 5, sensor 6, Current in A AS ExtV
-    IBUS_MEAS_TYPE_EXTERNAL_VOLTAGE   // Address 6, sensor 7, Baro Alt in cm AS ExtV
-#if defined(GPS)
-   ,IBUS_MEAS_TYPE_RPM,               // Address 7, sensor 8, HOME_DIR in deg AS RPM	
-    IBUS_MEAS_TYPE_RPM,               // Address 8, sensor 9, HOME_DIST in m AS RPM
-    IBUS_MEAS_TYPE_RPM,               // Address 9, sensor 10,GPS_COURSE in deg AS RPM
-    IBUS_MEAS_TYPE_RPM,               // Address 10,sensor 11,GPS_ALT in m AS RPM
-    IBUS_MEAS_TYPE_RPM,               // Address 11,sensor 12,GPS_LAT2 AS RPM 5678
-    IBUS_MEAS_TYPE_RPM,               // Address 12,sensor 13,GPS_LON2 AS RPM 6789
-    IBUS_MEAS_TYPE_EXTERNAL_VOLTAGE,  // Address 13,sensor 14,GPS_LAT1 AS ExtV -12.45 (-12.3456789 N)
-    IBUS_MEAS_TYPE_EXTERNAL_VOLTAGE,  // Address 14,sensor 15,GPS_LON1 AS ExtV -123.45 (-123.4567890 E)
-    IBUS_MEAS_TYPE_RPM                // Address 15,sensor 16,GPS_SPEED in km/h AS RPM
-#endif
-    //IBUS_MEAS_TYPE_TX_VOLTAGE,
-    //IBUS_MEAS_TYPE_ERR
-};
-
 static serialPort_t *ibusSerialPort = NULL;
 static serialPortConfig_t *ibusSerialPortConfig;
-
+static uint8_t outboundBytesToIgnoreOnRxCount = 0;
 static bool ibusTelemetryEnabled = false;
 static portSharing_e ibusPortSharing;
 
 static uint8_t ibusReceiveBuffer[IBUS_RX_BUF_LEN] = { 0x0 };
 
-static uint16_t calculateChecksum(uint8_t ibusPacket[static IBUS_CHECKSUM_SIZE], size_t packetLength) {
-    uint16_t checksum = 0xFFFF;
-    for (size_t i = 0; i < packetLength - IBUS_CHECKSUM_SIZE; i++) {
-        checksum -= ibusPacket[i];
-    }
-    return checksum;
-}
-
-static bool isChecksumOk(uint8_t ibusPacket[static IBUS_CHECKSUM_SIZE], size_t packetLength, uint16_t calcuatedChecksum) {
-    // Note that there's a byte order swap to little endian here
-    return (calcuatedChecksum >> 8) == ibusPacket[packetLength - 1]
-            && (calcuatedChecksum & 0xFF) == ibusPacket[packetLength - 2];
-}
-
-static void transmitIbusPacket(uint8_t ibusPacket[static IBUS_MIN_LEN], size_t packetLength) {
-    uint16_t checksum = calculateChecksum(ibusPacket, packetLength);
-    ibusPacket[packetLength - IBUS_CHECKSUM_SIZE] = (checksum & 0xFF);
-    ibusPacket[packetLength - IBUS_CHECKSUM_SIZE + 1] = (checksum >> 8);
-    for (size_t i = 0; i < packetLength; i++) {
-        serialWrite(ibusSerialPort, ibusPacket[i]);
-    }
-}
-
-static void sendIbusCommand(ibusAddress_t address) {
-    uint8_t sendBuffer[] = { 0x04, IBUS_COMMAND_DISCOVER_SENSOR | address, 0x00, 0x00 };
-    transmitIbusPacket(sendBuffer, sizeof sendBuffer);
-}
-
-static void sendIbusSensorType(ibusAddress_t address) {
-    uint8_t sendBuffer[] = { 0x06, IBUS_COMMAND_SENSOR_TYPE | address, SENSOR_ADDRESS_TYPE_LOOKUP[address], 0x02, 0x0, 0x0 };
-    transmitIbusPacket(sendBuffer, sizeof sendBuffer);
-}
-
-static void sendIbusMeasurement(ibusAddress_t address, uint16_t measurement) {
-    uint8_t sendBuffer[] = { 0x06, IBUS_COMMAND_MEASUREMENT | address, measurement & 0xFF, measurement >> 8, 0x0, 0x0 };
-    transmitIbusPacket(sendBuffer, sizeof sendBuffer);
-}
-
-static bool isCommand(ibusCommand_e expected, uint8_t ibusPacket[static IBUS_MIN_LEN]) {
-    return (ibusPacket[1] & 0xF0) == expected;
-}
-
-static ibusAddress_t getAddress(uint8_t ibusPacket[static IBUS_MIN_LEN]) {
-    return (ibusPacket[1] & 0x0F);
-}
-
-static void dispatchMeasurementRequest(ibusAddress_t address) {
-    if (address == 1) { //2. VBAT
-        sendIbusMeasurement(address, vbat * 10);
-    } else if (address == 2) { //3. BARO_TEMP\GYRO_TEMP
-        if (sensors(SENSOR_BARO)) sendIbusMeasurement(address, (uint16_t) ((baro.baroTemperature + 50) / 10  + IBUS_TEMPERATURE_OFFSET)); //int32_t 
-        else sendIbusMeasurement(address, (uint16_t) (telemTemperature1 + IBUS_TEMPERATURE_OFFSET)); //int16_t
-    } else if (address == 3) { //4. STATUS (sat num AS #0, FIX AS 0, HDOP AS 0, Mode AS 0)
-        int16_t status = 0;
-        if (ARMING_FLAG(ARMED)) status = 1; //Rate
-        if (FLIGHT_MODE(HORIZON_MODE)) status = 2;
-        if (FLIGHT_MODE(ANGLE_MODE)) status = 3;
-        if (FLIGHT_MODE(HEADFREE_MODE) || FLIGHT_MODE(HEADING_MODE)) status = 4; 
-        if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) status = 5; 
-        if (FLIGHT_MODE(NAV_POSHOLD_MODE)) status = 6;
-        if (FLIGHT_MODE(NAV_RTH_MODE)) status = 7;
-        if (failsafeIsActive()) {
-        if (FLIGHT_MODE(NAV_RTH_MODE)) status = 8; else status = 9; 
-    }
-#if defined(GPS)
-    if (sensors(SENSOR_GPS)) {
-        status += gpsSol.numSat * 1000;
-        if (gpsSol.fixType == GPS_NO_FIX) status += 100;
-        else if (gpsSol.fixType == GPS_FIX_2D) status += 200;
-        else if (gpsSol.fixType == GPS_FIX_3D) status += 300;
-        if (STATE(GPS_FIX_HOME)) status += 500;
-            status += constrain(gpsSol.hdop / 1000, 0, 9) * 10;
-        }
-#endif
-        sendIbusMeasurement(address, (uint16_t) status);
-    } else if (address == 4) { //5. MAG_COURSE (0-360*, 0=north) //In ddeg ==> deg, 10ddeg = 1deg
-        sendIbusMeasurement(address, (uint16_t) (attitude.values.yaw / 10));
-    } else if (address == 5) { //6. CURR //In 10*mA, 1 = 10 mA
-        if (feature(FEATURE_CURRENT_METER)) sendIbusMeasurement(address, (uint16_t) amperage); //int32_t
-        else sendIbusMeasurement(address, 0); 
-    } else if (address == 6) { //7. BARO_ALT //In cm => m
-        if (sensors(SENSOR_BARO)) sendIbusMeasurement(address, (uint16_t) baro.BaroAlt); //int32_t 
-        else sendIbusMeasurement(address, 0);
-    }
-#if defined(GPS)
-      else if (address == 7) { //8. HOME_DIR (0-360deg, 0=head)
-        if (sensors(SENSOR_GPS)) sendIbusMeasurement(address, (uint16_t) GPS_directionToHome); //int16_t
-        else sendIbusMeasurement(address, 0);
-    } else if (address == 8) { //9. HOME_DIST //In m
-        if (sensors(SENSOR_GPS)) sendIbusMeasurement(address, (uint16_t) GPS_distanceToHome); //uint16_t
-        else sendIbusMeasurement(address, 0);
-    } else if (address == 9) { //10.GPS_COURSE (0-360deg, 0=north)
-        if (sensors(SENSOR_GPS)) sendIbusMeasurement(address, (uint16_t) (gpsSol.groundCourse / 10)); //int16_t
-        else sendIbusMeasurement(address, 0);
-    } else if (address == 10) { //11. GPS_ALT //In cm => m
-        if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) sendIbusMeasurement(address, (uint16_t) (gpsSol.llh.alt / 100));
-        else sendIbusMeasurement(address, 0);
-    } else if (address == 11) { //12. GPS_LAT2 //Lattitude * 1e+7
-        if (sensors(SENSOR_GPS)) sendIbusMeasurement(address, (uint16_t) ((gpsSol.llh.lat % 100000)/10));
-        else sendIbusMeasurement(address, 0);
-    } else if (address == 12) { //13. GPS_LON2 //Longitude * 1e+7
-        if (sensors(SENSOR_GPS)) sendIbusMeasurement(address, (uint16_t) ((gpsSol.llh.lon % 100000)/10));
-        else sendIbusMeasurement(address, 0);
-    } else if (address == 13) { //14. GPS_LAT1 //Lattitude * 1e+7
-        if (sensors(SENSOR_GPS)) sendIbusMeasurement(address, (uint16_t) (gpsSol.llh.lat / 100000)); 
-        else sendIbusMeasurement(address, 0);
-    } else if (address == 14) { //15. GPS_LON1 //Longitude * 1e+7
-        if (sensors(SENSOR_GPS)) sendIbusMeasurement(address, (uint16_t) (gpsSol.llh.lon / 100000));
-        else sendIbusMeasurement(address, 0);
-    } else if (address == 15) { //16. GPS_SPEED //In cm/s => km/h, 1cm/s = 0.0194384449 knots
-        if (sensors(SENSOR_GPS)) sendIbusMeasurement(address, (uint16_t) gpsSol.groundSpeed * 1944 / 10000); //int16_t
-        else sendIbusMeasurement(address, 0);
-    }
-#endif
-}
-
-static void respondToIbusRequest(uint8_t ibusPacket[static IBUS_RX_BUF_LEN]) {
-    ibusAddress_t returnAddress = getAddress(ibusPacket);
-
-    if (returnAddress < sizeof SENSOR_ADDRESS_TYPE_LOOKUP) {
-        if (isCommand(IBUS_COMMAND_DISCOVER_SENSOR, ibusPacket)) {
-            sendIbusCommand(returnAddress);
-        } else if (isCommand(IBUS_COMMAND_SENSOR_TYPE, ibusPacket)) {
-            sendIbusSensorType(returnAddress);
-        } else if (isCommand(IBUS_COMMAND_MEASUREMENT, ibusPacket)) {
-            dispatchMeasurementRequest(returnAddress);
-        }
-    }
-}
-
 static void pushOntoTail(uint8_t buffer[IBUS_MIN_LEN], size_t bufferLength, uint8_t value) {
     for (size_t i = 0; i < bufferLength - 1; i++) {
         buffer[i] = buffer[i + 1];
@@ -375,33 +185,35 @@ void initIbusTelemetry(void) {
     ibusSerialPortConfig = findSerialPortConfig(FUNCTION_TELEMETRY_IBUS);
     uint8_t type = telemetryConfig()->ibusTelemetryType;
 #if defined(GPS)
-    if (type == 1 || type == 2) SENSOR_ADDRESS_TYPE_LOOKUP[15] = IBUS_MEAS_TYPE_SPE;
+    if (type == 1 || type == 2) changeTypeIbusTelemetry(15, IBUS_MEAS_TYPE_SPE);
-    if (type == 2) SENSOR_ADDRESS_TYPE_LOOKUP[10] = IBUS_MEAS_TYPE_ALT;
+    if (type == 2) changeTypeIbusTelemetry(10, IBUS_MEAS_TYPE_ALT);
 #endif	
     ibusPortSharing = determinePortSharing(ibusSerialPortConfig, FUNCTION_TELEMETRY_IBUS);
+    ibusTelemetryEnabled = false;
 }
 
 void handleIbusTelemetry(void) {
     if (!ibusTelemetryEnabled) {
         return;
     }
-
     while (serialRxBytesWaiting(ibusSerialPort) > 0) {
         uint8_t c = serialRead(ibusSerialPort);
+        if (outboundBytesToIgnoreOnRxCount) {
+            outboundBytesToIgnoreOnRxCount--;
+            continue;
+        }
         pushOntoTail(ibusReceiveBuffer, IBUS_RX_BUF_LEN, c);
-        uint16_t expectedChecksum = calculateChecksum(ibusReceiveBuffer, IBUS_RX_BUF_LEN);
+        if (isChecksumOk(ibusReceiveBuffer, IBUS_RX_BUF_LEN)) {
-
+            outboundBytesToIgnoreOnRxCount += respondToIbusRequest(ibusReceiveBuffer);
-        if (isChecksumOk(ibusReceiveBuffer, IBUS_RX_BUF_LEN, expectedChecksum)) {
-            respondToIbusRequest(ibusReceiveBuffer);
         }
     }
 }
 
-void checkIbusTelemetryState(void) {
+bool checkIbusTelemetryState(void) {
     bool newTelemetryEnabledValue = telemetryDetermineEnabledState(ibusPortSharing);
 
     if (newTelemetryEnabledValue == ibusTelemetryEnabled) {
-        return;
+        return false;
     }
 
     if (newTelemetryEnabledValue) {
@@ -410,24 +222,25 @@ void checkIbusTelemetryState(void) {
     } else {
         freeIbusTelemetryPort();
     }
+    
+    return true;
 }
 
 void configureIbusTelemetryPort(void) {
-    portOptions_t portOptions;
-
     if (!ibusSerialPortConfig) {
         return;
     }
-
+    if (isSerialPortShared(ibusSerialPortConfig, FUNCTION_RX_SERIAL, FUNCTION_TELEMETRY_IBUS)) {
-    portOptions = SERIAL_BIDIR;
+        // serialRx will open port and handle telemetry
-
+        return;
-    ibusSerialPort = openSerialPort(ibusSerialPortConfig->identifier, FUNCTION_TELEMETRY_IBUS, NULL, IBUS_BAUDRATE, IBUS_UART_MODE, portOptions);
+    }
-
+    ibusSerialPort = openSerialPort(ibusSerialPortConfig->identifier, FUNCTION_TELEMETRY_IBUS, NULL, IBUS_BAUDRATE, MODE_RXTX, SERIAL_BIDIR | SERIAL_NOT_INVERTED);
     if (!ibusSerialPort) {
         return;
     }
-
+    initSharedIbusTelemetry(ibusSerialPort);
     ibusTelemetryEnabled = true;
+    outboundBytesToIgnoreOnRxCount = 0;
 }
 
 void freeIbusTelemetryPort(void) {

src/main/telemetry/ibus.h

@@ -16,11 +16,12 @@
  */
 
 #pragma once
+#include <stdbool.h>
 
 void initIbusTelemetry(void);
 
 void handleIbusTelemetry(void);
-void checkIbusTelemetryState(void);
+bool checkIbusTelemetryState(void);
 
 void configureIbusTelemetryPort(void);
 void freeIbusTelemetryPort(void);

src/main/telemetry/telemetry.h

@@ -69,4 +69,4 @@ void telemetryProcess(timeUs_t currentTimeUs);
 
 bool telemetryDetermineEnabledState(portSharing_e portSharing);
 
-#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM)
+#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_IBUS)