diff --git a/Makefile b/Makefile
index 2ae9aa1c2..bdad0725f 100644
--- a/Makefile
+++ b/Makefile
@@ -463,7 +463,8 @@ HIGHEND_SRC = \
telemetry/hott.c \
telemetry/smartport.c \
telemetry/mavlink.c \
- telemetry/ltm.c
+ telemetry/ltm.c \
+ telemetry/ibus.c
ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS)))
VCP_SRC = \
diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c
index 87aee6acb..4d23f0064 100755
--- a/src/main/fc/mw.c
+++ b/src/main/fc/mw.c
@@ -214,7 +214,7 @@ void mwDisarm(void)
}
}
-#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK)
+#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS)
void releaseSharedTelemetryPorts(void) {
serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
diff --git a/src/main/fc/mw.h b/src/main/fc/mw.h
index de19f1e0e..420b5fae7 100644
--- a/src/main/fc/mw.h
+++ b/src/main/fc/mw.h
@@ -17,6 +17,8 @@
#pragma once
+extern int16_t telemTemperature1;
+
void handleInflightCalibrationStickPosition();
void mwDisarm(void);
diff --git a/src/main/io/serial.c b/src/main/io/serial.c
index 70db726d0..db80a6fa9 100644
--- a/src/main/io/serial.c
+++ b/src/main/io/serial.c
@@ -192,7 +192,7 @@ serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction
return NULL;
}
-#define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK)
+#define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS)
#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | ALL_TELEMETRY_FUNCTIONS_MASK)
bool isSerialConfigValid(serialConfig_t *serialConfigToCheck)
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index 46e361811..b6398017a 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -34,6 +34,7 @@ typedef enum {
FUNCTION_RX_SERIAL = (1 << 6), // 64
FUNCTION_BLACKBOX = (1 << 7), // 128
FUNCTION_TELEMETRY_MAVLINK = (1 << 8), // 256
+ FUNCTION_TELEMETRY_IBUS = (1 << 9) // 512
} serialPortFunction_e;
typedef enum {
diff --git a/src/main/target/ALIENWIIF3/target.h b/src/main/target/ALIENWIIF3/target.h
new file mode 100644
index 000000000..ba7c96780
--- /dev/null
+++ b/src/main/target/ALIENWIIF3/target.h
@@ -0,0 +1,140 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "AWF3" // AlienWii32 F3.
+
+#define LED0
+#define LED0_GPIO GPIOB
+#define LED0_PIN Pin_4 // Blue LEDs - PB4
+#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOB
+
+#define LED1
+#define LED1_GPIO GPIOB
+#define LED1_PIN Pin_5 // Green LEDs - PB5
+#define LED1_PERIPHERAL RCC_AHBPeriph_GPIOB
+
+#define BEEPER
+#define BEEP_GPIO GPIOA
+#define BEEP_PIN Pin_5 // White LEDs - PA5
+#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOA
+
+#define USABLE_TIMER_CHANNEL_COUNT 11
+
+// Using MPU6050 for the moment.
+#define GYRO
+#define USE_GYRO_MPU6050
+
+#define GYRO_MPU6050_ALIGN CW270_DEG
+
+#define ACC
+#define USE_ACC_MPU6050
+
+#define ACC_MPU6050_ALIGN CW270_DEG
+
+// No baro support.
+//#define BARO
+//#define USE_BARO_MS5611
+
+// No mag support for now (option to use MPU9150 in the future).
+//#define MAG
+//#define USE_MAG_AK8975
+
+#define MAG_AK8975_ALIGN CW0_DEG_FLIP
+
+#define USE_VCP
+#define USE_USART1 // Not connected - TX (PB6) RX PB7 (AF7)
+#define USE_USART2 // Receiver - RX (PA3)
+#define USE_USART3 // Not connected - 10/RX (PB11) 11/TX (PB10)
+#define SERIAL_PORT_COUNT 4
+
+#define UART1_TX_PIN GPIO_Pin_6 // PB6
+#define UART1_RX_PIN GPIO_Pin_7 // PB7
+#define UART1_GPIO GPIOB
+#define UART1_GPIO_AF GPIO_AF_7
+#define UART1_TX_PINSOURCE GPIO_PinSource6
+#define UART1_RX_PINSOURCE GPIO_PinSource7
+
+#define UART2_TX_PIN GPIO_Pin_2 // PA2
+#define UART2_RX_PIN GPIO_Pin_3 // PA3
+#define UART2_GPIO GPIOA
+#define UART2_GPIO_AF GPIO_AF_7
+#define UART2_TX_PINSOURCE GPIO_PinSource2
+#define UART2_RX_PINSOURCE GPIO_PinSource3
+
+#define UART3_TX_PIN GPIO_Pin_10 // PB10 (AF7)
+#define UART3_RX_PIN GPIO_Pin_11 // PB11 (AF7)
+#define UART3_GPIO_AF GPIO_AF_7
+#define UART3_GPIO GPIOB
+#define UART3_TX_PINSOURCE GPIO_PinSource10
+#define UART3_RX_PINSOURCE GPIO_PinSource11
+
+
+#define USE_I2C
+#define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4)
+
+#define I2C2_SCL_GPIO GPIOA
+#define I2C2_SCL_GPIO_AF GPIO_AF_4
+#define I2C2_SCL_PIN GPIO_Pin_9
+#define I2C2_SCL_PIN_SOURCE GPIO_PinSource9
+#define I2C2_SCL_CLK_SOURCE RCC_AHBPeriph_GPIOA
+#define I2C2_SDA_GPIO GPIOA
+#define I2C2_SDA_GPIO_AF GPIO_AF_4
+#define I2C2_SDA_PIN GPIO_Pin_10
+#define I2C2_SDA_PIN_SOURCE GPIO_PinSource10
+#define I2C2_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOA
+
+#define USE_ADC
+
+#define ADC_INSTANCE ADC2
+#define ADC_DMA_CHANNEL DMA2_Channel1
+#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2
+
+//#define BOARD_HAS_VOLTAGE_DIVIDER
+
+#define VBAT_ADC_GPIO GPIOA
+#define VBAT_ADC_GPIO_PIN GPIO_Pin_4
+#define VBAT_ADC_CHANNEL ADC_Channel_1
+
+#define SPEKTRUM_BIND
+// USART2, PA3
+#define BIND_PORT GPIOA
+#define BIND_PIN Pin_3
+
+// alternative defaults for AlienWii32 F3 target
+#define ALIENWII32
+#define HARDWARE_BIND_PLUG
+
+// Hardware bind plug at PB12 (Pin 25)
+#define BINDPLUG_PORT GPIOB
+#define BINDPLUG_PIN Pin_12
+
+#undef BLACKBOX
+
+#undef GPS
+#undef GPS_PROTO_NMEA
+#undef GPS_PROTO_UBLOX
+#undef GPS_PROTO_I2C_NAV
+#undef GPS_PROTO_NAZA
+
+#undef TELEMETRY
+#undef TELEMETRY_FRSKY
+#undef TELEMETRY_HOTT
+#undef TELEMETRY_SMARTPORT
+#undef TELEMETRY_LTM
+#undef TELEMETRY_IBUS
diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h
index 9d9d70951..4aad6a86e 100644
--- a/src/main/target/CC3D/target.h
+++ b/src/main/target/CC3D/target.h
@@ -205,6 +205,7 @@
#undef SPEKTRUM_BIND
#undef TELEMETRY
#undef TELEMETRY_LTM
+#undef TELEMETRY_IBUS
#endif
// Number of available PWM outputs
diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index 2fd1fe89a..f21c4aa57 100644
--- a/src/main/target/NAZE/target.h
+++ b/src/main/target/NAZE/target.h
@@ -205,6 +205,7 @@
#undef TELEMETRY_HOTT
#undef TELEMETRY_SMARTPORT
+#undef TELEMETRY_IBUS
// Disable all GPS protocols except UBLOX
#undef GPS_PROTO_NMEA
diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h
index 7e09e8de5..6d4b79553 100644
--- a/src/main/target/RMDO/target.h
+++ b/src/main/target/RMDO/target.h
@@ -119,6 +119,7 @@
#undef GPS_PROTO_NAZA
#undef TELEMETRY_HOTT
#undef TELEMETRY_SMARTPORT
+#undef TELEMETRY_IBUS
// Number of available PWM outputs
#define MAX_PWM_OUTPUT_PORTS 12
diff --git a/src/main/target/common.h b/src/main/target/common.h
index 82eb58598..e148bda10 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -48,9 +48,9 @@
#define DISPLAY_ARMED_BITMAP
#define TELEMETRY_MAVLINK
#define BOOTLOG_DESCRIPTIONS
+#define TELEMETRY_IBUS
#else
#define SKIP_CLI_COMMAND_HELP
#define SKIP_RX_MSP
#define DISABLE_UNCOMMON_MIXERS
#endif
-
diff --git a/src/main/telemetry/ibus.c b/src/main/telemetry/ibus.c
new file mode 100644
index 000000000..909293b02
--- /dev/null
+++ b/src/main/telemetry/ibus.c
@@ -0,0 +1,340 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ *
+ * FlySky iBus telemetry implementation by CraigJPerry.
+ *
+ * Many thanks to Dave Borthwick's iBus telemetry dongle converter for
+ * PIC 12F1572 (also distributed under GPLv3) which was referenced to
+ * clarify the protocol.
+ *
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef TELEMETRY
+
+#include "common/axis.h"
+
+#include "drivers/system.h"
+#include "drivers/sensor.h"
+#include "drivers/accgyro.h"
+#include "drivers/serial.h"
+
+#include "io/serial.h"
+#include "fc/rc_controls.h"
+
+#include "sensors/sensors.h"
+#include "sensors/acceleration.h"
+#include "sensors/battery.h"
+
+#include "telemetry/telemetry.h"
+#include "telemetry/ibus.h"
+
+#include "fc/mw.h"
+
+/*
+ * iBus Telemetry is a half-duplex serial protocol. It shares 1 line for
+ * both TX and RX. It runs at a fixed baud rate of 115200. Queries are sent
+ * every 7ms by the iBus receiver. Multiple sensors can be daisy chained with
+ * ibus but not with this implementation, only because i don't have one of the
+ * sensors to test with!
+ *
+ * _______
+ * / \ /---------\
+ * | STM32 |--UART TX-->[Bi-directional @ 115200 baud]<--| IBUS RX |
+ * | uC |--UART RX--x[not connected] \---------/
+ * \_______/
+ *
+ *
+ * The protocol is driven by the iBus receiver, currently either an IA6B or
+ * IA10. All iBus traffic is little endian. It begins with the iBus rx
+ * querying for a sensor on the iBus:
+ *
+ *
+ * /---------\
+ * | IBUS RX | > Hello sensor at address 1, are you there?
+ * \---------/ [ 0x04, 0x81, 0x7A, 0xFF ]
+ *
+ * 0x04 - Packet Length
+ * 0x81 - bits 7-4 Command (1000 = discover sensor)
+ * bits 3-0 Address (0001 = address 1)
+ * 0x7A, 0xFF - Checksum, 0xFFFF - (0x04 + 0x81)
+ *
+ *
+ * Due to the daisy-chaining, this hello also serves to inform the sensor
+ * of its address (position in the chain). There are 16 possible addresses
+ * in iBus, however address 0 is reserved for the rx's internally measured
+ * voltage leaving 0x1 to 0xF remaining.
+ *
+ * Having learned it's address, the sensor simply echos the message back:
+ *
+ *
+ * /--------\
+ * Yes, i'm here, hello! < | Sensor |
+ * [ 0x04, 0x81, 0x7A, 0xFF ] \--------/
+ *
+ * 0x04, 0x81, 0x7A, 0xFF - Echo back received packet
+ *
+ *
+ * On receipt of a response, the iBus rx next requests the sensor's type:
+ *
+ *
+ * /---------\
+ * | IBUS RX | > Sensor at address 1, what type are you?
+ * \---------/ [ 0x04, 0x91, 0x6A, 0xFF ]
+ *
+ * 0x04 - Packet Length
+ * 0x91 - bits 7-4 Command (1001 = request sensor type)
+ * bits 3-0 Address (0001 = address 1)
+ * 0x6A, 0xFF - Checksum, 0xFFFF - (0x04 + 0x91)
+ *
+ *
+ * To which the sensor responds with its details:
+ *
+ *
+ * /--------\
+ * Yes, i'm here, hello! < | Sensor |
+ * [ 0x06 0x91 0x00 0x02 0x66 0xFF ] \--------/
+ *
+ * 0x06 - Packet Length
+ * 0x91 - bits 7-4 Command (1001 = request sensor type)
+ * bits 3-0 Address (0001 = address 1)
+ * 0x00 - Measurement type (0 = internal voltage)
+ * 0x02 - Unknown, always 0x02
+ * 0x66, 0xFF - Checksum, 0xFFFF - (0x06 + 0x91 + 0x00 + 0x02)
+ *
+ *
+ * The iBus rx continues the discovery process by querying the next
+ * address. Discovery stops at the first address which does not respond.
+ *
+ * The iBus rx then begins a continual loop, requesting measurements from
+ * each sensor discovered:
+ *
+ *
+ * /---------\
+ * | IBUS RX | > Sensor at address 1, please send your measurement
+ * \---------/ [ 0x04, 0xA1, 0x5A, 0xFF ]
+ *
+ * 0x04 - Packet Length
+ * 0xA1 - bits 7-4 Command (1010 = request measurement)
+ * bits 3-0 Address (0001 = address 1)
+ * 0x5A, 0xFF - Checksum, 0xFFFF - (0x04 + 0xA1)
+ *
+ *
+ * /--------\
+ * I'm reading 0 volts < | Sensor |
+ * [ 0x06 0xA1 0x00 0x00 0x5E 0xFF ] \--------/
+ *
+ * 0x06 - Packet Length
+ * 0xA1 - bits 7-4 Command (1010 = request measurement)
+ * bits 3-0 Address (0001 = address 1)
+ * 0x00, 0x00 - The measurement
+ * 0x5E, 0xFF - Checksum, 0xFF - (0x06 + 0xA1 + 0x00 + 0x00)
+ *
+ *
+ * Due to the limited telemetry data types possible with ibus, we
+ * simply send everything which can be represented. Currently this
+ * is voltage and temperature.
+ *
+ */
+
+#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_SENSOR_TYPE_INTERNAL_VOLTAGE = 0x00,
+ IBUS_SENSOR_TYPE_TEMPERATURE = 0x01,
+ IBUS_SENSOR_TYPE_RPM = 0x02,
+ IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE = 0x03
+} ibusSensorType_e;
+
+static const uint8_t SENSOR_ADDRESS_TYPE_LOOKUP[] = {
+ IBUS_SENSOR_TYPE_INTERNAL_VOLTAGE, // Address 0, not usable since it is reserved for internal voltage
+ IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE, // Address 1, VBAT
+ IBUS_SENSOR_TYPE_TEMPERATURE, // Address 2, Gyro Temp
+ IBUS_SENSOR_TYPE_RPM // Address 3, Throttle command
+};
+
+static serialPort_t *ibusSerialPort = NULL;
+static serialPortConfig_t *ibusSerialPortConfig;
+
+static telemetryConfig_t *telemetryConfig;
+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 (1 == address) {
+ sendIbusMeasurement(address, vbat * 10);
+ } else if (2 == address) {
+ sendIbusMeasurement(address, (uint16_t) telemTemperature1 + IBUS_TEMPERATURE_OFFSET);
+ } else if (3 == address) {
+ sendIbusMeasurement(address, (uint16_t) rcCommand[THROTTLE]);
+ }
+}
+
+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];
+ }
+ ibusReceiveBuffer[bufferLength - 1] = value;
+}
+
+void initIbusTelemetry(telemetryConfig_t *initialTelemetryConfig) {
+ telemetryConfig = initialTelemetryConfig;
+ ibusSerialPortConfig = findSerialPortConfig(FUNCTION_TELEMETRY_IBUS);
+ ibusPortSharing = determinePortSharing(ibusSerialPortConfig, FUNCTION_TELEMETRY_IBUS);
+}
+
+void handleIbusTelemetry(void) {
+ if (!ibusTelemetryEnabled) {
+ return;
+ }
+
+ while (serialRxBytesWaiting(ibusSerialPort) > 0) {
+ uint8_t c = serialRead(ibusSerialPort);
+ pushOntoTail(ibusReceiveBuffer, IBUS_RX_BUF_LEN, c);
+ uint16_t expectedChecksum = calculateChecksum(ibusReceiveBuffer, IBUS_RX_BUF_LEN);
+
+ if (isChecksumOk(ibusReceiveBuffer, IBUS_RX_BUF_LEN, expectedChecksum)) {
+ respondToIbusRequest(ibusReceiveBuffer);
+ }
+ }
+}
+
+void checkIbusTelemetryState(void) {
+ bool newTelemetryEnabledValue = telemetryDetermineEnabledState(ibusPortSharing);
+
+ if (newTelemetryEnabledValue == ibusTelemetryEnabled) {
+ return;
+ }
+
+ if (newTelemetryEnabledValue) {
+ configureIbusTelemetryPort();
+ } else {
+ freeIbusTelemetryPort();
+ }
+}
+
+void configureIbusTelemetryPort(void) {
+ portOptions_t portOptions;
+
+ if (!ibusSerialPortConfig) {
+ return;
+ }
+
+ portOptions = SERIAL_BIDIR;
+
+ ibusSerialPort = openSerialPort(ibusSerialPortConfig->identifier, FUNCTION_TELEMETRY_IBUS, NULL, IBUS_BAUDRATE, IBUS_UART_MODE, portOptions);
+
+ if (!ibusSerialPort) {
+ return;
+ }
+
+ ibusTelemetryEnabled = true;
+}
+
+void freeIbusTelemetryPort(void) {
+ closeSerialPort(ibusSerialPort);
+ ibusSerialPort = NULL;
+
+ ibusTelemetryEnabled = false;
+}
+
+#endif
diff --git a/src/main/telemetry/ibus.h b/src/main/telemetry/ibus.h
new file mode 100644
index 000000000..1e8e87a9a
--- /dev/null
+++ b/src/main/telemetry/ibus.h
@@ -0,0 +1,26 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#pragma once
+
+void initIbusTelemetry(telemetryConfig_t *);
+
+void handleIbusTelemetry(void);
+void checkIbusTelemetryState(void);
+
+void configureIbusTelemetryPort(void);
+void freeIbusTelemetryPort(void);
diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
index 730398b5a..a3cf44781 100644
--- a/src/main/telemetry/telemetry.c
+++ b/src/main/telemetry/telemetry.c
@@ -44,9 +44,9 @@
#include "telemetry/hott.h"
#include "telemetry/smartport.h"
#include "telemetry/ltm.h"
-#include "telemetry/ltm.h"
#include "telemetry/mavlink.h"
#include "telemetry/jetiexbus.h"
+#include "telemetry/ibus.h"
static telemetryConfig_t *telemetryConfig;
@@ -81,6 +81,10 @@ void telemetryInit(void)
initJetiExBusTelemetry();
#endif
+#if defined(TELEMETRY_IBUS)
+ initIbusTelemetry(telemetryConfig);
+#endif
+
telemetryCheckState();
}
@@ -130,6 +134,11 @@ void telemetryCheckState(void)
#if defined(TELEMETRY_JETIEXBUS)
checkJetiExBusTelemetryState();
#endif
+
+#if defined(TELEMETRY_IBUS)
+ checkIbusTelemetryState();
+#endif
+
}
void telemetryProcess(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
@@ -160,6 +169,11 @@ void telemetryProcess(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
#if defined(TELEMETRY_JETIEXBUS)
handleJetiExBusTelemetry();
#endif
+
+#if defined(TELEMETRY_IBUS)
+ handleIbusTelemetry();
+#endif
+
}
#endif