diff --git a/Makefile b/Makefile index bd92e6f586..ecd1bd3271 100644 --- a/Makefile +++ b/Makefile @@ -607,6 +607,7 @@ HIGHEND_SRC = \ telemetry/smartport.c \ telemetry/ltm.c \ telemetry/mavlink.c \ + telemetry/ibus.c \ sensors/esc_sensor.c \ io/vtx_smartaudio.c diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index c5416882ae..f704f72f51 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -86,6 +86,7 @@ #define failsafeConfig(x) (&masterConfig.failsafeConfig) #define serialConfig(x) (&masterConfig.serialConfig) #define telemetryConfig(x) (&masterConfig.telemetryConfig) +#define ibusTelemetryConfig(x) (&masterConfig.telemetryConfig) #define ppmConfig(x) (&masterConfig.ppmConfig) #define pwmConfig(x) (&masterConfig.pwmConfig) #define adcConfig(x) (&masterConfig.adcConfig) diff --git a/src/main/fc/serial_cli.c b/src/main/fc/serial_cli.c index 2a7d0b0024..a03119432e 100755 --- a/src/main/fc/serial_cli.c +++ b/src/main/fc/serial_cli.c @@ -574,6 +574,9 @@ const clivalue_t valueTable[] = { { "frsky_vfas_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->frsky_vfas_cell_voltage, .config.lookup = { TABLE_OFF_ON } }, { "hott_alarm_sound_interval", VAR_UINT8 | MASTER_VALUE, &telemetryConfig()->hottAlarmSoundInterval, .config.minmax = { 0, 120 } }, { "pid_values_as_telemetry", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->pidValuesAsTelemetry, .config.lookup = {TABLE_OFF_ON } }, +#if defined(TELEMETRY_IBUS) + { "ibus_report_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &ibusTelemetryConfig()->report_cell_voltage, .config.lookup = { TABLE_OFF_ON } }, +#endif #endif { "battery_capacity", VAR_UINT16 | MASTER_VALUE, &batteryConfig()->batteryCapacity, .config.minmax = { 0, 20000 } }, diff --git a/src/main/io/serial.h b/src/main/io/serial.h index c8c9a66679..7ec570beca 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -27,18 +27,18 @@ typedef enum { typedef enum { FUNCTION_NONE = 0, - FUNCTION_MSP = (1 << 0), // 1 - FUNCTION_GPS = (1 << 1), // 2 - FUNCTION_TELEMETRY_FRSKY = (1 << 2), // 4 - FUNCTION_TELEMETRY_HOTT = (1 << 3), // 8 - FUNCTION_TELEMETRY_LTM = (1 << 4), // 16 - FUNCTION_TELEMETRY_SMARTPORT = (1 << 5), // 32 - FUNCTION_RX_SERIAL = (1 << 6), // 64 - FUNCTION_BLACKBOX = (1 << 7), // 128 - - FUNCTION_TELEMETRY_MAVLINK = (1 << 9), // 512 - FUNCTION_ESC_SENSOR = (1 << 10),// 1024 - FUNCTION_VTX_CONTROL = (1 << 11),// 2048 + FUNCTION_MSP = (1 << 0), // 1 + FUNCTION_GPS = (1 << 1), // 2 + FUNCTION_TELEMETRY_FRSKY = (1 << 2), // 4 + FUNCTION_TELEMETRY_HOTT = (1 << 3), // 8 + FUNCTION_TELEMETRY_LTM = (1 << 4), // 16 + FUNCTION_TELEMETRY_SMARTPORT = (1 << 5), // 32 + FUNCTION_RX_SERIAL = (1 << 6), // 64 + FUNCTION_BLACKBOX = (1 << 7), // 128 + FUNCTION_TELEMETRY_MAVLINK = (1 << 9), // 512 + FUNCTION_ESC_SENSOR = (1 << 10), // 1024 + FUNCTION_VTX_CONTROL = (1 << 11), // 2048 + FUNCTION_TELEMETRY_IBUS = (1 << 12) // 4096 } serialPortFunction_e; typedef enum { diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index a93446511a..a7e6f10308 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -25,6 +25,7 @@ typedef enum { SENSOR_INDEX_COUNT } sensorIndex_e; +extern int16_t telemTemperature1; //FIXME move to temp sensor...? extern uint8_t detectedSensors[SENSOR_INDEX_COUNT]; typedef struct int16_flightDynamicsTrims_s { diff --git a/src/main/target/common.h b/src/main/target/common.h index 66d17911a4..e419793763 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -99,6 +99,7 @@ #define TELEMETRY_SRXL #define TELEMETRY_JETIEXBUS #define TELEMETRY_MAVLINK +#define TELEMETRY_IBUS #define USE_RX_MSP #define USE_SERIALRX_JETIEXBUS #define VTX_CONTROL diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 1c4df50119..f696419016 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -79,8 +79,6 @@ static portSharing_e frskyPortSharing; extern batteryConfig_t *batteryConfig; -extern int16_t telemTemperature1; // FIXME dependency on mw.c - #define CYCLETIME 125 #define PROTOCOL_HEADER 0x5E diff --git a/src/main/telemetry/ibus.c b/src/main/telemetry/ibus.c new file mode 100644 index 0000000000..52b91c838e --- /dev/null +++ b/src/main/telemetry/ibus.c @@ -0,0 +1,426 @@ +/* + * 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. + * Unit tests and some additions by Unitware + * + * 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 + +#include "platform.h" +#include "common/utils.h" + +#if defined(TELEMETRY) && defined(TELEMETRY_IBUS) + +#include "config/config_master.h" + +#include "common/axis.h" + +#include "drivers/system.h" +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/serial.h" + +#include "sensors/sensors.h" +#include "sensors/acceleration.h" +#include "sensors/battery.h" +#include "sensors/barometer.h" + +#include "io/serial.h" +#include "fc/rc_controls.h" +#include "scheduler/scheduler.h" + +#include "telemetry/telemetry.h" +#include "telemetry/ibus.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 this is implemented but not tested 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 + * 0x58, 0xFF - Checksum, 0xFFFF - (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. + * + */ + +/* +PG_REGISTER_WITH_RESET_TEMPLATE(ibusTelemetryConfig_t, ibusTelemetryConfig, PG_IBUS_TELEMETRY_CONFIG, 0); + +PG_RESET_TEMPLATE(ibusTelemetryConfig_t, ibusTelemetryConfig, + .report_cell_voltage = false, + ); +*/ + +#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 (400) + + +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_TEMPERATURE = 0x01, + IBUS_SENSOR_TYPE_RPM = 0x02, + IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE = 0x03 +} ibusSensorType_e; + +/* Address lookup relative to the sensor base address which is the lowest address seen by the FC + The actual lowest value is likely to change when sensors are daisy chained */ +static const uint8_t sensorAddressTypeLookup[] = { + IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE, + IBUS_SENSOR_TYPE_TEMPERATURE, + IBUS_SENSOR_TYPE_RPM +}; + +static serialPort_t *ibusSerialPort = NULL; +static serialPortConfig_t *ibusSerialPortConfig; + +/* The sent bytes will be echoed back since Tx and Rx are wired together, this counter + * will keep track of how many rx chars that shall be discarded */ +static uint8_t outboundBytesToIgnoreOnRxCount = 0; + +static bool ibusTelemetryEnabled = false; +static portSharing_e ibusPortSharing; + +#define INVALID_IBUS_ADDRESS 0 +static ibusAddress_t ibusBaseAddress = INVALID_IBUS_ADDRESS; + +static uint8_t ibusReceiveBuffer[IBUS_RX_BUF_LEN] = { 0x0 }; + +static uint16_t calculateChecksum(const uint8_t *ibusPacket, 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(const uint8_t *ibusPacket) +{ + uint16_t calculatedChecksum = calculateChecksum(ibusReceiveBuffer, IBUS_RX_BUF_LEN); + + // Note that there's a byte order swap to little endian here + return (calculatedChecksum >> 8) == ibusPacket[IBUS_RX_BUF_LEN - 1] + && (calculatedChecksum & 0xFF) == ibusPacket[IBUS_RX_BUF_LEN - 2]; +} + +static void transmitIbusPacket(uint8_t *ibusPacket, size_t payloadLength) +{ + uint16_t checksum = calculateChecksum(ibusPacket, payloadLength + IBUS_CHECKSUM_SIZE); + for (size_t i = 0; i < payloadLength; i++) { + serialWrite(ibusSerialPort, ibusPacket[i]); + } + serialWrite(ibusSerialPort, checksum & 0xFF); + serialWrite(ibusSerialPort, checksum >> 8); + outboundBytesToIgnoreOnRxCount += payloadLength + IBUS_CHECKSUM_SIZE; +} + +static void sendIbusDiscoverSensorReply(ibusAddress_t address) +{ + uint8_t sendBuffer[] = { 0x04, IBUS_COMMAND_DISCOVER_SENSOR | address}; + transmitIbusPacket(sendBuffer, sizeof(sendBuffer)); +} + +static void sendIbusSensorType(ibusAddress_t address) +{ + uint8_t sendBuffer[] = {0x06, + IBUS_COMMAND_SENSOR_TYPE | address, + sensorAddressTypeLookup[address - ibusBaseAddress], + 0x02 + }; + 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}; + transmitIbusPacket(sendBuffer, sizeof(sendBuffer)); +} + +static bool isCommand(ibusCommand_e expected, const uint8_t *ibusPacket) +{ + return (ibusPacket[1] & 0xF0) == expected; +} + +static ibusAddress_t getAddress(const uint8_t *ibusPacket) +{ + return (ibusPacket[1] & 0x0F); +} + +static void dispatchMeasurementReply(ibusAddress_t address) +{ + int value; + + switch (sensorAddressTypeLookup[address - ibusBaseAddress]) { + case IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE: + value = getVbat() * 10; + if (ibusTelemetryConfig()->report_cell_voltage) { + value /= batteryCellCount; + } + sendIbusMeasurement(address, value); + break; + + case IBUS_SENSOR_TYPE_TEMPERATURE: + #ifdef BARO + value = (baro.baroTemperature + 5) / 10; // +5 to make integer division rounding correct + #else + value = telemTemperature1 * 10; + #endif + sendIbusMeasurement(address, value + IBUS_TEMPERATURE_OFFSET); + break; + + case IBUS_SENSOR_TYPE_RPM: + sendIbusMeasurement(address, (uint16_t) rcCommand[THROTTLE]); + break; + } +} + +static void autodetectFirstReceivedAddressAsBaseAddress(ibusAddress_t returnAddress) +{ + if ((INVALID_IBUS_ADDRESS == ibusBaseAddress) && + (INVALID_IBUS_ADDRESS != returnAddress)) { + ibusBaseAddress = returnAddress; + } +} + +static bool theAddressIsWithinOurRange(ibusAddress_t returnAddress) +{ + return (returnAddress >= ibusBaseAddress) && + (ibusAddress_t)(returnAddress - ibusBaseAddress) < ARRAYLEN(sensorAddressTypeLookup); +} + +static void respondToIbusRequest(uint8_t ibusPacket[static IBUS_RX_BUF_LEN]) +{ + ibusAddress_t returnAddress = getAddress(ibusPacket); + + autodetectFirstReceivedAddressAsBaseAddress(returnAddress); + + if (theAddressIsWithinOurRange(returnAddress)) { + if (isCommand(IBUS_COMMAND_DISCOVER_SENSOR, ibusPacket)) { + sendIbusDiscoverSensorReply(returnAddress); + } else if (isCommand(IBUS_COMMAND_SENSOR_TYPE, ibusPacket)) { + sendIbusSensorType(returnAddress); + } else if (isCommand(IBUS_COMMAND_MEASUREMENT, ibusPacket)) { + dispatchMeasurementReply(returnAddress); + } + } +} + +static void pushOntoTail(uint8_t buffer[IBUS_MIN_LEN], size_t bufferLength, uint8_t value) +{ + memmove(buffer, buffer + 1, bufferLength - 1); + ibusReceiveBuffer[bufferLength - 1] = value; +} + +void initIbusTelemetry(void) +{ + ibusSerialPortConfig = findSerialPortConfig(FUNCTION_TELEMETRY_IBUS); + ibusPortSharing = determinePortSharing(ibusSerialPortConfig, FUNCTION_TELEMETRY_IBUS); + ibusBaseAddress = INVALID_IBUS_ADDRESS; +} + +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); + + if (isChecksumOk(ibusReceiveBuffer)) { + respondToIbusRequest(ibusReceiveBuffer); + } + } +} + +bool checkIbusTelemetryState(void) +{ + bool newTelemetryEnabledValue = telemetryDetermineEnabledState(ibusPortSharing); + + if (newTelemetryEnabledValue == ibusTelemetryEnabled) { + return false; + } + + if (newTelemetryEnabledValue) { + rescheduleTask(TASK_TELEMETRY, IBUS_TASK_PERIOD_US); + configureIbusTelemetryPort(); + } else { + freeIbusTelemetryPort(); + } + + return true; +} + +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; + outboundBytesToIgnoreOnRxCount = 0; +} + +void freeIbusTelemetryPort(void) +{ + closeSerialPort(ibusSerialPort); + ibusSerialPort = NULL; + + ibusTelemetryEnabled = false; +} + +#endif \ No newline at end of file diff --git a/src/main/telemetry/ibus.h b/src/main/telemetry/ibus.h new file mode 100644 index 0000000000..8bbd30c5d8 --- /dev/null +++ b/src/main/telemetry/ibus.h @@ -0,0 +1,34 @@ +/* + * 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 + +/* +typedef struct ibusTelemetryConfig_s { + uint8_t report_cell_voltage; // report vbatt divided with cellcount +} ibusTelemetryConfig_t; + +PG_DECLARE(ibusTelemetryConfig_t, ibusTelemetryConfig); +*/ + +void initIbusTelemetry(void); + +void handleIbusTelemetry(void); +bool checkIbusTelemetryState(void); + +void configureIbusTelemetryPort(void); +void freeIbusTelemetryPort(void); \ No newline at end of file diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index fc7e3bc9de..fd475d50af 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -48,6 +48,7 @@ #include "telemetry/mavlink.h" #include "telemetry/crsf.h" #include "telemetry/srxl.h" +#include "telemetry/ibus.h" static telemetryConfig_t *telemetryConfig; @@ -82,6 +83,9 @@ void telemetryInit(void) #ifdef TELEMETRY_SRXL initSrxlTelemetry(); #endif +#ifdef TELEMETRY_IBUS + initIbusTelemetry(); +#endif telemetryCheckState(); } diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 91b2ff004a..aa60912147 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -46,6 +46,7 @@ typedef struct telemetryConfig_s { uint8_t frsky_vfas_cell_voltage; uint8_t hottAlarmSoundInterval; uint8_t pidValuesAsTelemetry; + uint8_t report_cell_voltage; } telemetryConfig_t; void telemetryInit(void);