mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-13 11:29:56 +03:00
Implement sharing an UART for IBUS SerialRX and telemetry (kudos to @hali9 and @unitware)
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity)
parent
c8fd52dc5d
commit
7a3e46ed53
7 changed files with 139 additions and 262 deletions
1
Makefile
1
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 \
|
||||
|
|
|
|||
19
docs/Rx.md
19
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
|
||||
FS-IA10 receivers. The Turnigy TGY-IA6B and TGY-IA10 are the same
|
||||
devices with a different label, therefore they also work.
|
||||
IBUS is the FlySky digital serial protocol and is available with the FS-IA6B and FS-IA10 receivers.
|
||||
The Turnigy TGY-IA6B and TGY-IA10 are the same 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.
|
||||
|
||||
|
|
|
|||
|
|
@ -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 |
|
||||
| uC |- UART RX--x[not connected] \---------/
|
||||
\_______/
|
||||
/ \ /-------------\
|
||||
| STM32 |-->UART TX-->[Bi-directional @ 115200 baud]-->| Flysky RX |
|
||||
| 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 |
|
||||
| uC |- UART RX--x[not connected] \---------/ \-------------/ \---------/
|
||||
\_______/
|
||||
/ \ /---------\ /-------------\ /-------------\
|
||||
| STM32 |-->UART TX-->[Bi-directional @ 115200 baud]-->| CVT-01 |-->|others sensor|-->| Flysky RX |
|
||||
| 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.
|
||||
|
|
|
|||
|
|
@ -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_BUFFSIZE 32
|
||||
#define IBUS_SYNCBYTE 0x20
|
||||
|
||||
#define IBUS_BAUDRATE 115200
|
||||
#define IBUS_MAX_CHANNEL (10)
|
||||
#define IBUS_TELEMETRY_PACKET_LENGTH (4)
|
||||
#define IBUS_SERIAL_RX_PACKET_LENGTH (32)
|
||||
|
||||
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)
|
||||
return;
|
||||
if (ibusFramePosition == 0) {
|
||||
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;
|
||||
for (i = 0; i < 30; i++)
|
||||
chksum -= ibus[i];
|
||||
|
||||
rxsum = ibus[30] + (ibus[31] << 8);
|
||||
|
||||
if (chksum == rxsum) {
|
||||
ibusChannelData[0] = (ibus[ 3] << 8) + ibus[ 2];
|
||||
ibusChannelData[1] = (ibus[ 5] << 8) + ibus[ 4];
|
||||
ibusChannelData[2] = (ibus[ 7] << 8) + ibus[ 6];
|
||||
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;
|
||||
|
||||
if (isChecksumOk(ibus, ibusLength)) {
|
||||
if (ibusLength == IBUS_SERIAL_RX_PACKET_LENGTH) {
|
||||
for (i = 0, offset = 2; i < IBUS_MAX_CHANNEL; i++, offset += 2) {
|
||||
ibusChannelData[i] = ibus[offset] + (ibus[offset + 1] << 8);
|
||||
}
|
||||
frameStatus = RX_FRAME_COMPLETE;
|
||||
} else {
|
||||
#if defined(TELEMETRY) && defined(TELEMETRY_IBUS)
|
||||
rxBytesToIgnore = respondToIbusRequest(ibus);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
serialPort_t *ibusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, ibusDataReceive, IBUS_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED);
|
||||
|
||||
rxBytesToIgnore = 0;
|
||||
#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
|
||||
|
|
|
|||
|
|
@ -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 == 2) SENSOR_ADDRESS_TYPE_LOOKUP[10] = IBUS_MEAS_TYPE_ALT;
|
||||
if (type == 1 || type == 2) changeTypeIbusTelemetry(15, IBUS_MEAS_TYPE_SPE);
|
||||
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, expectedChecksum)) {
|
||||
respondToIbusRequest(ibusReceiveBuffer);
|
||||
if (isChecksumOk(ibusReceiveBuffer, IBUS_RX_BUF_LEN)) {
|
||||
outboundBytesToIgnoreOnRxCount += 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;
|
||||
}
|
||||
|
||||
portOptions = SERIAL_BIDIR;
|
||||
|
||||
ibusSerialPort = openSerialPort(ibusSerialPortConfig->identifier, FUNCTION_TELEMETRY_IBUS, NULL, IBUS_BAUDRATE, IBUS_UART_MODE, portOptions);
|
||||
|
||||
if (isSerialPortShared(ibusSerialPortConfig, FUNCTION_RX_SERIAL, FUNCTION_TELEMETRY_IBUS)) {
|
||||
// serialRx will open port and handle telemetry
|
||||
return;
|
||||
}
|
||||
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) {
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue