From 79cf530b97960d9138646744b5d29d6ac00729e7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Spychalski?= Date: Thu, 8 Sep 2016 01:01:12 +0200 Subject: [PATCH] FlySky ibus telemetry (#259) * Add ibus telemetry support * Integrate ibus runtime * Expose throttle via motor telemetry field. Reformat source file. * Fix battery voltage scale --- Makefile | 3 +- src/main/fc/mw.c | 2 +- src/main/fc/mw.h | 2 + src/main/io/serial.c | 2 +- src/main/io/serial.h | 1 + src/main/target/ALIENWIIF3/target.h | 140 ++++++++++++ src/main/target/CC3D/target.h | 1 + src/main/target/NAZE/target.h | 1 + src/main/target/RMDO/target.h | 1 + src/main/target/common.h | 2 +- src/main/telemetry/ibus.c | 340 ++++++++++++++++++++++++++++ src/main/telemetry/ibus.h | 26 +++ src/main/telemetry/telemetry.c | 16 +- 13 files changed, 532 insertions(+), 5 deletions(-) create mode 100644 src/main/target/ALIENWIIF3/target.h create mode 100644 src/main/telemetry/ibus.c create mode 100644 src/main/telemetry/ibus.h 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