mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-19 14:25:20 +03:00
Combine ibus serial rx and ibus telemetry on the same fc uart on a single pin.
``` +---------+ | FS-iA6B | | | | Ser RX |---|<---\ +------------+ | | | | FC | | Sensor |---[R]--*-------| Uart TX | +---------+ +------------+ ``` R = 10Kohm, Diode 1N4148 or similar. Both uart tx and rx channels are used so it's not possible to use the spare pin for rx of something else. Configure the serial port like this to get both serial rx and ibus telemetry on the same port: ``` serial 1 4160 115200 57600 115200 115200 ``` It is still possible to run the serial rx and ibus telemetry on two uarts like before, an example: ``` serial 1 64 115200 57600 0 115200 serial 2 4096 115200 57600 115200 115200 ```
This commit is contained in:
Magnus Ivarsson
parent
355c7cc215
commit
b13d4694bb
12 changed files with 640 additions and 347 deletions
1
Makefile
1
Makefile
|
|
@ -684,6 +684,7 @@ COMMON_SRC = \
|
|||
telemetry/ltm.c \
|
||||
telemetry/mavlink.c \
|
||||
telemetry/ibus.c \
|
||||
telemetry/ibus_shared.c \
|
||||
sensors/esc_sensor.c \
|
||||
io/vtx_string.c \
|
||||
io/vtx_smartaudio.c \
|
||||
|
|
|
|||
|
|
@ -43,6 +43,8 @@
|
|||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/ibus.h"
|
||||
#include "telemetry/ibus.h"
|
||||
#include "telemetry/ibus_shared.h"
|
||||
|
||||
#define IBUS_MAX_CHANNEL 14
|
||||
#define IBUS_BUFFSIZE 32
|
||||
|
|
@ -51,11 +53,14 @@
|
|||
#define IBUS_FRAME_GAP 500
|
||||
|
||||
#define IBUS_BAUDRATE 115200
|
||||
#define IBUS_TELEMETRY_PACKET_LENGTH (4)
|
||||
#define IBUS_SERIAL_RX_PACKET_LENGTH (32)
|
||||
|
||||
static uint8_t ibusModel;
|
||||
static uint8_t ibusSyncByte;
|
||||
static uint8_t ibusFrameSize;
|
||||
static uint8_t ibusChannelOffset;
|
||||
static uint8_t rxBytesToIgnore;
|
||||
static uint16_t ibusChecksum;
|
||||
|
||||
static bool ibusFrameDone = false;
|
||||
|
|
@ -63,6 +68,13 @@ static uint32_t ibusChannelData[IBUS_MAX_CHANNEL];
|
|||
|
||||
static uint8_t ibus[IBUS_BUFFSIZE] = { 0, };
|
||||
|
||||
|
||||
static bool isValidIa6bIbusPacketLength(uint8_t length)
|
||||
{
|
||||
return (length == IBUS_TELEMETRY_PACKET_LENGTH) || (length == IBUS_SERIAL_RX_PACKET_LENGTH);
|
||||
}
|
||||
|
||||
|
||||
// Receive ISR callback
|
||||
static void ibusDataReceive(uint16_t c)
|
||||
{
|
||||
|
|
@ -72,29 +84,29 @@ static void ibusDataReceive(uint16_t c)
|
|||
|
||||
ibusTime = micros();
|
||||
|
||||
if ((ibusTime - ibusTimeLast) > IBUS_FRAME_GAP)
|
||||
if ((ibusTime - ibusTimeLast) > IBUS_FRAME_GAP) {
|
||||
ibusFramePosition = 0;
|
||||
rxBytesToIgnore = 0;
|
||||
} else if (rxBytesToIgnore) {
|
||||
rxBytesToIgnore--;
|
||||
return;
|
||||
}
|
||||
|
||||
ibusTimeLast = ibusTime;
|
||||
|
||||
if (ibusFramePosition == 0) {
|
||||
if (ibusSyncByte == 0) {
|
||||
// detect the frame type based on the STX byte.
|
||||
if (c == 0x55) {
|
||||
if (isValidIa6bIbusPacketLength(c)) {
|
||||
ibusModel = IBUS_MODEL_IA6B;
|
||||
ibusSyncByte = c;
|
||||
ibusFrameSize = c;
|
||||
ibusChannelOffset = 2;
|
||||
ibusChecksum = 0xFFFF;
|
||||
} else if ((ibusSyncByte == 0) && (c == 0x55)) {
|
||||
ibusModel = IBUS_MODEL_IA6;
|
||||
ibusSyncByte = 0x55;
|
||||
ibusFrameSize = 31;
|
||||
ibusChecksum = 0x0000;
|
||||
ibusChannelOffset = 1;
|
||||
} else if (c == 0x20) {
|
||||
ibusModel = IBUS_MODEL_IA6B;
|
||||
ibusSyncByte = 0x20;
|
||||
ibusFrameSize = 32;
|
||||
ibusChannelOffset = 2;
|
||||
ibusChecksum = 0xFFFF;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
} else if (ibusSyncByte != c) {
|
||||
return;
|
||||
}
|
||||
|
|
@ -109,11 +121,42 @@ static void ibusDataReceive(uint16_t c)
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
static bool isChecksumOkIa6(void)
|
||||
{
|
||||
uint8_t offset;
|
||||
uint8_t i;
|
||||
uint16_t chksum, rxsum;
|
||||
chksum = ibusChecksum;
|
||||
rxsum = ibus[ibusFrameSize - 2] + (ibus[ibusFrameSize - 1] << 8);
|
||||
for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_CHANNEL; i++, offset += 2) {
|
||||
chksum += ibus[offset] + (ibus[offset + 1] << 8);
|
||||
}
|
||||
return chksum == rxsum;
|
||||
}
|
||||
|
||||
|
||||
static bool checksumIsOk(void) {
|
||||
if (ibusModel == IBUS_MODEL_IA6 ) {
|
||||
return isChecksumOkIa6();
|
||||
} else {
|
||||
return isChecksumOkIa6b(ibus, ibusFrameSize);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void updateChannelData(void) {
|
||||
uint8_t i;
|
||||
uint8_t offset;
|
||||
|
||||
for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_CHANNEL; i++, offset += 2) {
|
||||
ibusChannelData[i] = ibus[offset] + (ibus[offset + 1] << 8);
|
||||
}
|
||||
}
|
||||
|
||||
static uint8_t ibusFrameStatus(void)
|
||||
{
|
||||
uint8_t i, offset;
|
||||
uint8_t frameStatus = RX_FRAME_PENDING;
|
||||
uint16_t chksum, rxsum;
|
||||
|
||||
if (!ibusFrameDone) {
|
||||
return frameStatus;
|
||||
|
|
@ -121,32 +164,30 @@ static uint8_t ibusFrameStatus(void)
|
|||
|
||||
ibusFrameDone = false;
|
||||
|
||||
chksum = ibusChecksum;
|
||||
rxsum = ibus[ibusFrameSize - 2] + (ibus[ibusFrameSize - 1] << 8);
|
||||
if (ibusModel == IBUS_MODEL_IA6) {
|
||||
for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_CHANNEL; i++, offset += 2)
|
||||
chksum += ibus[offset] + (ibus[offset + 1] << 8);
|
||||
} else {
|
||||
for (i = 0; i < 30; i++)
|
||||
chksum -= ibus[i];
|
||||
}
|
||||
|
||||
if (chksum == rxsum) {
|
||||
for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_CHANNEL; i++, offset += 2) {
|
||||
ibusChannelData[i] = ibus[offset] + (ibus[offset + 1] << 8);
|
||||
}
|
||||
if (checksumIsOk()) {
|
||||
if (ibusModel == IBUS_MODEL_IA6 || ibusSyncByte == 0x20) {
|
||||
updateChannelData();
|
||||
frameStatus = RX_FRAME_COMPLETE;
|
||||
}
|
||||
else
|
||||
{
|
||||
#if defined(TELEMETRY) && defined(TELEMETRY_IBUS)
|
||||
rxBytesToIgnore = respondToIbusRequest(ibus);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
return frameStatus;
|
||||
}
|
||||
|
||||
|
||||
static uint16_t ibusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
|
||||
{
|
||||
UNUSED(rxRuntimeConfig);
|
||||
return ibusChannelData[chan];
|
||||
}
|
||||
|
||||
|
||||
bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||
{
|
||||
UNUSED(rxConfig);
|
||||
|
|
@ -164,25 +205,29 @@ bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
|||
}
|
||||
|
||||
#ifdef TELEMETRY
|
||||
bool portShared = telemetryCheckRxPortShared(portConfig);
|
||||
// bool portShared = telemetryCheckRxPortShared(portConfig);
|
||||
bool portShared = isSerialPortShared(portConfig, FUNCTION_RX_SERIAL, FUNCTION_TELEMETRY_IBUS);
|
||||
#else
|
||||
bool portShared = false;
|
||||
#endif
|
||||
|
||||
|
||||
rxBytesToIgnore = 0;
|
||||
serialPort_t *ibusPort = openSerialPort(portConfig->identifier,
|
||||
FUNCTION_RX_SERIAL,
|
||||
ibusDataReceive,
|
||||
IBUS_BAUDRATE,
|
||||
portShared ? MODE_RXTX : MODE_RX,
|
||||
SERIAL_NOT_INVERTED | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
|
||||
SERIAL_NOT_INVERTED | (rxConfig->halfDuplex || portShared ? SERIAL_BIDIR : 0)
|
||||
);
|
||||
|
||||
#ifdef TELEMETRY
|
||||
#if defined(TELEMETRY) && defined(TELEMETRY_IBUS)
|
||||
if (portShared) {
|
||||
telemetrySharedPort = ibusPort;
|
||||
initSharedIbusTelemetry(ibusPort);
|
||||
}
|
||||
#endif
|
||||
|
||||
return ibusPort != NULL;
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif //SERIAL_RX
|
||||
|
|
|
|||
|
|
@ -17,6 +17,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#define TELEMETRY_IBUS
|
||||
|
||||
#define TARGET_CONFIG
|
||||
#define TARGET_VALIDATECONFIG
|
||||
#define USE_HARDWARE_REVISION_DETECTION
|
||||
|
|
|
|||
|
|
@ -17,6 +17,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#undef TELEMETRY_IBUS //no space left
|
||||
|
||||
#define TARGET_BOARD_IDENTIFIER "OMNI" // https://en.wikipedia.org/wiki/Omnibus
|
||||
|
||||
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE
|
||||
|
|
|
|||
|
|
@ -97,6 +97,7 @@
|
|||
#define GPS
|
||||
#define CMS
|
||||
#define TELEMETRY_CRSF
|
||||
#define TELEMETRY_IBUS
|
||||
#define TELEMETRY_JETIEXBUS
|
||||
#define TELEMETRY_MAVLINK
|
||||
#define TELEMETRY_SRXL
|
||||
|
|
|
|||
|
|
@ -55,114 +55,9 @@
|
|||
#include "scheduler/scheduler.h"
|
||||
|
||||
#include "telemetry/ibus.h"
|
||||
#include "telemetry/ibus_shared.h"
|
||||
#include "telemetry/telemetry.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.
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#define IBUS_TASK_PERIOD_US (1000)
|
||||
|
||||
|
|
@ -170,37 +65,11 @@
|
|||
#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;
|
||||
|
|
@ -212,131 +81,9 @@ 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 (telemetryConfig()->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 = gyroGetTemperature() * 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)
|
||||
{
|
||||
|
|
@ -344,13 +91,15 @@ static void pushOntoTail(uint8_t buffer[IBUS_MIN_LEN], size_t bufferLength, uint
|
|||
ibusReceiveBuffer[bufferLength - 1] = value;
|
||||
}
|
||||
|
||||
|
||||
void initIbusTelemetry(void)
|
||||
{
|
||||
ibusSerialPortConfig = findSerialPortConfig(FUNCTION_TELEMETRY_IBUS);
|
||||
ibusPortSharing = determinePortSharing(ibusSerialPortConfig, FUNCTION_TELEMETRY_IBUS);
|
||||
ibusBaseAddress = INVALID_IBUS_ADDRESS;
|
||||
ibusTelemetryEnabled = false;
|
||||
}
|
||||
|
||||
|
||||
void handleIbusTelemetry(void)
|
||||
{
|
||||
if (!ibusTelemetryEnabled) {
|
||||
|
|
@ -367,12 +116,13 @@ void handleIbusTelemetry(void)
|
|||
|
||||
pushOntoTail(ibusReceiveBuffer, IBUS_RX_BUF_LEN, c);
|
||||
|
||||
if (isChecksumOk(ibusReceiveBuffer)) {
|
||||
respondToIbusRequest(ibusReceiveBuffer);
|
||||
if (isChecksumOkIa6b(ibusReceiveBuffer, IBUS_RX_BUF_LEN)) {
|
||||
outboundBytesToIgnoreOnRxCount += respondToIbusRequest(ibusReceiveBuffer);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool checkIbusTelemetryState(void)
|
||||
{
|
||||
bool newTelemetryEnabledValue = telemetryDetermineEnabledState(ibusPortSharing);
|
||||
|
|
@ -391,32 +141,34 @@ bool checkIbusTelemetryState(void)
|
|||
return true;
|
||||
}
|
||||
|
||||
|
||||
void configureIbusTelemetryPort(void)
|
||||
{
|
||||
portOptions_t portOptions;
|
||||
|
||||
if (!ibusSerialPortConfig) {
|
||||
return;
|
||||
}
|
||||
|
||||
portOptions = SERIAL_BIDIR;
|
||||
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,
|
||||
IBUS_UART_MODE, portOptions);
|
||||
ibusSerialPort = openSerialPort(ibusSerialPortConfig->identifier, FUNCTION_TELEMETRY_IBUS, NULL, IBUS_BAUDRATE, IBUS_UART_MODE, SERIAL_BIDIR);
|
||||
|
||||
if (!ibusSerialPort) {
|
||||
return;
|
||||
}
|
||||
|
||||
initSharedIbusTelemetry(ibusSerialPort);
|
||||
ibusTelemetryEnabled = true;
|
||||
outboundBytesToIgnoreOnRxCount = 0;
|
||||
}
|
||||
|
||||
|
||||
void freeIbusTelemetryPort(void)
|
||||
{
|
||||
closeSerialPort(ibusSerialPort);
|
||||
ibusSerialPort = NULL;
|
||||
|
||||
ibusTelemetryEnabled = false;
|
||||
}
|
||||
|
||||
|
|
|
|||
204
src/main/telemetry/ibus_shared.c
Normal file
204
src/main/telemetry/ibus_shared.c
Normal file
|
|
@ -0,0 +1,204 @@
|
|||
/*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* 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 <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
// #include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
//#include "common/utils.h"
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "telemetry/ibus_shared.h"
|
||||
|
||||
static uint16_t calculateChecksum(const uint8_t *ibusPacket, size_t packetLength);
|
||||
|
||||
|
||||
#if defined(TELEMETRY) && defined(TELEMETRY_IBUS)
|
||||
|
||||
#include "config/parameter_group.h"
|
||||
#include "config/parameter_group_ids.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "sensors/gyro.h"
|
||||
|
||||
|
||||
#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;
|
||||
|
||||
#define INVALID_IBUS_ADDRESS 0
|
||||
static ibusAddress_t ibusBaseAddress = INVALID_IBUS_ADDRESS;
|
||||
|
||||
|
||||
|
||||
static uint8_t 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);
|
||||
return payloadLength + IBUS_CHECKSUM_SIZE;
|
||||
}
|
||||
|
||||
static uint8_t sendIbusDiscoverSensorReply(ibusAddress_t address)
|
||||
{
|
||||
uint8_t sendBuffer[] = { 0x04, IBUS_COMMAND_DISCOVER_SENSOR | address};
|
||||
return transmitIbusPacket(sendBuffer, sizeof(sendBuffer));
|
||||
}
|
||||
|
||||
static uint8_t sendIbusSensorType(ibusAddress_t address)
|
||||
{
|
||||
uint8_t sendBuffer[] = {0x06,
|
||||
IBUS_COMMAND_SENSOR_TYPE | address,
|
||||
sensorAddressTypeLookup[address - ibusBaseAddress],
|
||||
0x02
|
||||
};
|
||||
return transmitIbusPacket(sendBuffer, sizeof(sendBuffer));
|
||||
}
|
||||
|
||||
static uint8_t sendIbusMeasurement(ibusAddress_t address, uint16_t measurement)
|
||||
{
|
||||
uint8_t sendBuffer[] = { 0x06, IBUS_COMMAND_MEASUREMENT | address, measurement & 0xFF, measurement >> 8};
|
||||
return 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 uint8_t dispatchMeasurementReply(ibusAddress_t address)
|
||||
{
|
||||
int value;
|
||||
|
||||
switch (sensorAddressTypeLookup[address - ibusBaseAddress]) {
|
||||
case IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE:
|
||||
value = getVbat() * 10;
|
||||
if (telemetryConfig()->report_cell_voltage) {
|
||||
value /= batteryCellCount;
|
||||
}
|
||||
return sendIbusMeasurement(address, value);
|
||||
|
||||
case IBUS_SENSOR_TYPE_TEMPERATURE:
|
||||
value = gyroGetTemperature() * 10;
|
||||
return sendIbusMeasurement(address, value + IBUS_TEMPERATURE_OFFSET);
|
||||
|
||||
case IBUS_SENSOR_TYPE_RPM:
|
||||
return sendIbusMeasurement(address, (uint16_t) rcCommand[THROTTLE]);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
uint8_t respondToIbusRequest(uint8_t const * const ibusPacket)
|
||||
{
|
||||
ibusAddress_t returnAddress = getAddress(ibusPacket);
|
||||
autodetectFirstReceivedAddressAsBaseAddress(returnAddress);
|
||||
|
||||
if (theAddressIsWithinOurRange(returnAddress)) {
|
||||
if (isCommand(IBUS_COMMAND_DISCOVER_SENSOR, ibusPacket)) {
|
||||
return sendIbusDiscoverSensorReply(returnAddress);
|
||||
} else if (isCommand(IBUS_COMMAND_SENSOR_TYPE, ibusPacket)) {
|
||||
return sendIbusSensorType(returnAddress);
|
||||
} else if (isCommand(IBUS_COMMAND_MEASUREMENT, ibusPacket)) {
|
||||
return dispatchMeasurementReply(returnAddress);
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
void initSharedIbusTelemetry(serialPort_t *port)
|
||||
{
|
||||
ibusSerialPort = port;
|
||||
ibusBaseAddress = INVALID_IBUS_ADDRESS;
|
||||
}
|
||||
|
||||
|
||||
#endif //defined(TELEMETRY) && defined(TELEMETRY_IBUS)
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
bool isChecksumOkIa6b(const uint8_t *ibusPacket, const uint8_t length)
|
||||
{
|
||||
uint16_t calculatedChecksum = calculateChecksum(ibusPacket, length);
|
||||
|
||||
// Note that there's a byte order swap to little endian here
|
||||
return (calculatedChecksum >> 8) == ibusPacket[length - 1]
|
||||
&& (calculatedChecksum & 0xFF) == ibusPacket[length - 2];
|
||||
}
|
||||
|
||||
44
src/main/telemetry/ibus_shared.h
Normal file
44
src/main/telemetry/ibus_shared.h
Normal file
|
|
@ -0,0 +1,44 @@
|
|||
/*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* The ibus_shared module implements the ibus telemetry packet handling
|
||||
* which is shared between the ibus serial rx and the ibus telemetry.
|
||||
*
|
||||
* There is only one 'user' active at any time, serial rx will open the
|
||||
* serial port if both functions are enabled at the same time
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "platform.h"
|
||||
#include "drivers/serial.h"
|
||||
|
||||
#define IBUS_CHECKSUM_SIZE (2)
|
||||
|
||||
#if defined(TELEMETRY) && defined(TELEMETRY_IBUS)
|
||||
|
||||
uint8_t respondToIbusRequest(uint8_t const * const ibusPacket);
|
||||
void initSharedIbusTelemetry(serialPort_t * port);
|
||||
|
||||
#endif //defined(TELEMETRY) && defined(TELEMETRY_IBUS)
|
||||
|
||||
|
||||
bool isChecksumOkIa6b(const uint8_t *ibusPacket, const uint8_t length);
|
||||
|
||||
|
||||
|
||||
|
|
@ -126,7 +126,18 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing)
|
|||
|
||||
bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig)
|
||||
{
|
||||
return portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK;
|
||||
if (portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK) {
|
||||
return true;
|
||||
}
|
||||
#ifdef TELEMETRY_IBUS
|
||||
if ( portConfig->functionMask & FUNCTION_TELEMETRY_IBUS
|
||||
&& portConfig->functionMask & FUNCTION_RX_SERIAL
|
||||
&& rxConfig()->serialrx_provider == SERIALRX_IBUS) {
|
||||
// IBUS serial RX & telemetry
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
return false;
|
||||
}
|
||||
|
||||
serialPort_t *telemetrySharedPort = NULL;
|
||||
|
|
|
|||
|
|
@ -851,6 +851,14 @@ $(OBJECT_DIR)/telemetry/ibus.o : \
|
|||
@mkdir -p $(dir $@)
|
||||
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/telemetry/ibus.c -o $@
|
||||
|
||||
$(OBJECT_DIR)/telemetry/ibus_shared.o : \
|
||||
$(USER_DIR)/telemetry/ibus_shared.c \
|
||||
$(USER_DIR)/telemetry/ibus_shared.h \
|
||||
$(GTEST_HEADERS)
|
||||
|
||||
@mkdir -p $(dir $@)
|
||||
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/telemetry/ibus_shared.c -o $@
|
||||
|
||||
$(OBJECT_DIR)/telemetry_ibus_unittest.o : \
|
||||
$(TEST_DIR)/telemetry_ibus_unittest.cc \
|
||||
$(GTEST_HEADERS)
|
||||
|
|
@ -860,6 +868,7 @@ $(OBJECT_DIR)/telemetry_ibus_unittest.o : \
|
|||
|
||||
$(OBJECT_DIR)/telemetry_ibus_unittest : \
|
||||
$(OBJECT_DIR)/telemetry_ibus_unittest.o \
|
||||
$(OBJECT_DIR)/telemetry/ibus_shared.o \
|
||||
$(OBJECT_DIR)/telemetry/ibus.o \
|
||||
$(OBJECT_DIR)/gtest_main.a
|
||||
|
||||
|
|
|
|||
|
|
@ -25,13 +25,25 @@ extern "C" {
|
|||
#include "io/serial.h"
|
||||
#include "rx/rx.h"
|
||||
#include "rx/ibus.h"
|
||||
#include "telemetry/ibus_shared.h"
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/battery.h"
|
||||
}
|
||||
|
||||
#include "unittest_macros.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
|
||||
extern "C" {
|
||||
uint8_t batteryCellCount = 3;
|
||||
int16_t rcCommand[4] = {0, 0, 0, 0};
|
||||
int16_t telemTemperature1 = 0;
|
||||
baro_t baro = { .baroTemperature = 50 };
|
||||
telemetryConfig_t telemetryConfig_System;
|
||||
}
|
||||
|
||||
|
||||
bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig)
|
||||
{
|
||||
|
|
@ -42,6 +54,11 @@ bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig)
|
|||
|
||||
serialPort_t * telemetrySharedPort = NULL;
|
||||
|
||||
static uint16_t vbat = 100;
|
||||
uint16_t getVbat(void)
|
||||
{
|
||||
return vbat;
|
||||
}
|
||||
|
||||
uint32_t microseconds_stub_value = 0;
|
||||
uint32_t micros(void)
|
||||
|
|
@ -49,8 +66,15 @@ uint32_t micros(void)
|
|||
return microseconds_stub_value;
|
||||
}
|
||||
|
||||
|
||||
#define SERIAL_BUFFER_SIZE 256
|
||||
#define SERIAL_PORT_DUMMY_IDENTIFIER (serialPortIdentifier_e)0x1234
|
||||
|
||||
typedef struct serialPortStub_s {
|
||||
uint8_t buffer[SERIAL_BUFFER_SIZE];
|
||||
int pos = 0;
|
||||
int end = 0;
|
||||
} serialPortStub_t;
|
||||
|
||||
static serialPort_t serialTestInstance;
|
||||
static serialPortConfig_t serialTestInstanceConfig = {
|
||||
.identifier = SERIAL_PORT_DUMMY_IDENTIFIER,
|
||||
|
|
@ -60,7 +84,18 @@ static serialPortConfig_t serialTestInstanceConfig = {
|
|||
static serialReceiveCallbackPtr stub_serialRxCallback;
|
||||
static serialPortConfig_t *findSerialPortConfig_stub_retval;
|
||||
static bool openSerial_called = false;
|
||||
static serialPortStub_t serialWriteStub;
|
||||
static bool portIsShared = false;
|
||||
|
||||
bool isSerialPortShared(const serialPortConfig_t *portConfig,
|
||||
uint16_t functionMask,
|
||||
serialPortFunction_e sharedWithFunction)
|
||||
{
|
||||
EXPECT_EQ(portConfig, findSerialPortConfig_stub_retval);
|
||||
EXPECT_EQ(functionMask, FUNCTION_RX_SERIAL);
|
||||
EXPECT_EQ(sharedWithFunction, FUNCTION_TELEMETRY_IBUS);
|
||||
return portIsShared;
|
||||
}
|
||||
|
||||
serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function)
|
||||
{
|
||||
|
|
@ -68,6 +103,8 @@ serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function)
|
|||
return findSerialPortConfig_stub_retval;
|
||||
}
|
||||
|
||||
static portMode_t serialExpectedMode = MODE_RX;
|
||||
static portOptions_t serialExpectedOptions = SERIAL_UNIDIR;
|
||||
|
||||
serialPort_t *openSerialPort(
|
||||
serialPortIdentifier_e identifier,
|
||||
|
|
@ -81,22 +118,71 @@ serialPort_t *openSerialPort(
|
|||
openSerial_called = true;
|
||||
EXPECT_FALSE(NULL == callback);
|
||||
EXPECT_EQ(identifier, SERIAL_PORT_DUMMY_IDENTIFIER);
|
||||
EXPECT_EQ(options, SERIAL_UNIDIR);
|
||||
EXPECT_EQ(options, serialExpectedOptions);
|
||||
EXPECT_EQ(function, FUNCTION_RX_SERIAL);
|
||||
EXPECT_EQ(baudrate, 115200);
|
||||
EXPECT_EQ(mode, MODE_RX);
|
||||
EXPECT_EQ(mode, serialExpectedMode);
|
||||
stub_serialRxCallback = callback;
|
||||
return &serialTestInstance;
|
||||
}
|
||||
|
||||
void serialWrite(serialPort_t *instance, uint8_t ch)
|
||||
{
|
||||
EXPECT_EQ(instance, &serialTestInstance);
|
||||
EXPECT_LT(serialWriteStub.pos, sizeof(serialWriteStub.buffer));
|
||||
serialWriteStub.buffer[serialWriteStub.pos++] = ch;
|
||||
//TODO serialReadStub.buffer[serialReadStub.end++] = ch; //characters echoes back on the shared wire
|
||||
//printf("w: %02d 0x%02x\n", serialWriteStub.pos, ch);
|
||||
}
|
||||
|
||||
|
||||
void serialTestResetPort()
|
||||
{
|
||||
openSerial_called = false;
|
||||
stub_serialRxCallback = NULL;
|
||||
portIsShared = false;
|
||||
serialExpectedMode = MODE_RX;
|
||||
serialExpectedOptions = SERIAL_UNIDIR;
|
||||
}
|
||||
|
||||
|
||||
static bool isChecksumOkReturnValue = true;
|
||||
bool isChecksumOkIa6b(const uint8_t *ibusPacket, const uint8_t length)
|
||||
{
|
||||
(void) ibusPacket;
|
||||
(void) length;
|
||||
return isChecksumOkReturnValue;
|
||||
}
|
||||
|
||||
|
||||
static bool initSharedIbusTelemetryCalled = false;
|
||||
void initSharedIbusTelemetry(serialPort_t * port)
|
||||
{
|
||||
EXPECT_EQ(port, &serialTestInstance);
|
||||
initSharedIbusTelemetryCalled = true;
|
||||
}
|
||||
|
||||
static bool stubTelemetryCalled = false;
|
||||
static uint8_t stubTelemetryPacket[100];
|
||||
static uint8_t stubTelemetryIgnoreRxChars = 0;
|
||||
|
||||
uint8_t respondToIbusRequest(uint8_t const * const ibusPacket) {
|
||||
uint8_t len = ibusPacket[0];
|
||||
EXPECT_LT(len, sizeof(stubTelemetryPacket));
|
||||
memcpy(stubTelemetryPacket, ibusPacket, len);
|
||||
stubTelemetryCalled = true;
|
||||
return stubTelemetryIgnoreRxChars;
|
||||
}
|
||||
|
||||
void resetStubTelemetry(void)
|
||||
{
|
||||
memset(stubTelemetryPacket, 0, sizeof(stubTelemetryPacket));
|
||||
stubTelemetryCalled = false;
|
||||
stubTelemetryIgnoreRxChars = 0;
|
||||
initSharedIbusTelemetryCalled = false;
|
||||
isChecksumOkReturnValue = true;
|
||||
}
|
||||
|
||||
|
||||
class IbusRxInitUnitTest : public ::testing::Test
|
||||
{
|
||||
|
|
@ -154,23 +240,36 @@ protected:
|
|||
virtual void SetUp()
|
||||
{
|
||||
serialTestResetPort();
|
||||
resetStubTelemetry();
|
||||
portIsShared = true;
|
||||
serialExpectedOptions = SERIAL_BIDIR;
|
||||
serialExpectedMode = MODE_RXTX;
|
||||
|
||||
const rxConfig_t initialRxConfig = {};
|
||||
findSerialPortConfig_stub_retval = &serialTestInstanceConfig;
|
||||
|
||||
EXPECT_TRUE(ibusInit(&initialRxConfig, &rxRuntimeConfig));
|
||||
|
||||
EXPECT_TRUE(initSharedIbusTelemetryCalled);
|
||||
|
||||
//handle that internal ibus position is not set to zero at init
|
||||
microseconds_stub_value += 5000;
|
||||
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn());
|
||||
}
|
||||
|
||||
virtual void receivePacket(uint8_t const * const packet, const size_t length)
|
||||
{
|
||||
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn());
|
||||
for (size_t i=0; i < length; i++) {
|
||||
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn());
|
||||
stub_serialRxCallback(packet[i]);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
TEST_F(IbusRxProtocollUnitTest, Test_InitialFrameState)
|
||||
{
|
||||
|
||||
//TODO: ibusFrameStatus should return rxFrameState_t not uint8_t
|
||||
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn());
|
||||
|
||||
//TODO: is it ok to have undefined channel values after init?
|
||||
|
|
@ -209,6 +308,7 @@ TEST_F(IbusRxProtocollUnitTest, Test_IA6B_OnePacketReceivedWithBadCrc)
|
|||
0x0a, 0x33, 0x0b, 0x33, 0x0c, 0x33, 0x0d, 0x33, //channel 11..14
|
||||
0x00, 0x00}; //checksum
|
||||
|
||||
isChecksumOkReturnValue = false;
|
||||
for (size_t i=0; i < sizeof(packet); i++) {
|
||||
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn());
|
||||
stub_serialRxCallback(packet[i]);
|
||||
|
|
@ -304,3 +404,110 @@ TEST_F(IbusRxProtocollUnitTest, Test_IA6_OnePacketReceivedBadCrc)
|
|||
ASSERT_NE(i + (0x33 << 8), rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, i));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
TEST_F(IbusRxProtocollUnitTest, Test_IA6B_OnePacketReceived_not_shared_port)
|
||||
{
|
||||
uint8_t packet[] = {0x20, 0x00, //length and reserved (unknown) bits
|
||||
0x00, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x04, 0x00, //channel 1..5
|
||||
0x05, 0x00, 0x06, 0x00, 0x07, 0x00, 0x08, 0x00, 0x09, 0x00, //channel 6..10
|
||||
0x0a, 0x00, 0x0b, 0x00, 0x0c, 0x00, 0x0d, 0x00, //channel 11..14
|
||||
0x84, 0xff}; //checksum
|
||||
|
||||
{
|
||||
|
||||
serialTestResetPort();
|
||||
resetStubTelemetry();
|
||||
portIsShared = false;
|
||||
serialExpectedOptions = SERIAL_NOT_INVERTED;
|
||||
serialExpectedMode = MODE_RX;
|
||||
|
||||
const rxConfig_t initialRxConfig = {};
|
||||
findSerialPortConfig_stub_retval = &serialTestInstanceConfig;
|
||||
|
||||
EXPECT_TRUE(ibusInit(&initialRxConfig, &rxRuntimeConfig));
|
||||
EXPECT_FALSE(initSharedIbusTelemetryCalled);
|
||||
|
||||
//handle that internal ibus position is not set to zero at init
|
||||
microseconds_stub_value += 5000;
|
||||
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn());
|
||||
}
|
||||
|
||||
for (size_t i=0; i < sizeof(packet); i++) {
|
||||
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn());
|
||||
stub_serialRxCallback(packet[i]);
|
||||
}
|
||||
|
||||
//report frame complete once
|
||||
EXPECT_EQ(RX_FRAME_COMPLETE, rxRuntimeConfig.rcFrameStatusFn());
|
||||
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn());
|
||||
|
||||
//check that channel values have been updated
|
||||
for (int i=0; i<14; i++) {
|
||||
ASSERT_EQ(i, rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, i));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
TEST_F(IbusRxProtocollUnitTest, Test_OneTelemetryPacketReceived)
|
||||
{
|
||||
uint8_t packet[] = {0x04, 0x81, 0x7a, 0xff}; //ibus sensor discovery
|
||||
resetStubTelemetry();
|
||||
|
||||
receivePacket(packet, sizeof(packet));
|
||||
|
||||
//no frame complete signal to rx system, but telemetry system is called
|
||||
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn());
|
||||
EXPECT_TRUE(stubTelemetryCalled);
|
||||
EXPECT_TRUE( 0 == memcmp( stubTelemetryPacket, packet, sizeof(packet)));
|
||||
}
|
||||
|
||||
|
||||
TEST_F(IbusRxProtocollUnitTest, Test_OneTelemetryIgnoreTxEchoToRx)
|
||||
{
|
||||
uint8_t packet[] = {0x04, 0x81, 0x7a, 0xff}; //ibus sensor discovery
|
||||
resetStubTelemetry();
|
||||
stubTelemetryIgnoreRxChars = 4;
|
||||
|
||||
//given one packet received, that will respond with four characters to be ignored
|
||||
receivePacket(packet, sizeof(packet));
|
||||
rxRuntimeConfig.rcFrameStatusFn();
|
||||
EXPECT_TRUE(stubTelemetryCalled);
|
||||
|
||||
//when those four bytes are sent and looped back
|
||||
resetStubTelemetry();
|
||||
rxRuntimeConfig.rcFrameStatusFn();
|
||||
receivePacket(packet, sizeof(packet));
|
||||
|
||||
//then they are ignored
|
||||
EXPECT_FALSE(stubTelemetryCalled);
|
||||
|
||||
//and then next packet can be received
|
||||
receivePacket(packet, sizeof(packet));
|
||||
rxRuntimeConfig.rcFrameStatusFn();
|
||||
EXPECT_TRUE(stubTelemetryCalled);
|
||||
}
|
||||
|
||||
|
||||
TEST_F(IbusRxProtocollUnitTest, Test_OneTelemetryShouldNotIgnoreTxEchoAfterInterFrameGap)
|
||||
{
|
||||
uint8_t packet[] = {0x04, 0x81, 0x7a, 0xff}; //ibus sensor discovery
|
||||
resetStubTelemetry();
|
||||
stubTelemetryIgnoreRxChars = 4;
|
||||
|
||||
//given one packet received, that will respond with four characters to be ignored
|
||||
receivePacket(packet, sizeof(packet));
|
||||
rxRuntimeConfig.rcFrameStatusFn();
|
||||
EXPECT_TRUE(stubTelemetryCalled);
|
||||
|
||||
//when there is an interPacketGap
|
||||
microseconds_stub_value += 5000;
|
||||
resetStubTelemetry();
|
||||
rxRuntimeConfig.rcFrameStatusFn();
|
||||
|
||||
//then next packet can be received
|
||||
receivePacket(packet, sizeof(packet));
|
||||
rxRuntimeConfig.rcFrameStatusFn();
|
||||
EXPECT_TRUE(stubTelemetryCalled);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@ extern "C" {
|
|||
#include "fc/rc_controls.h"
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "telemetry/ibus.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "scheduler/scheduler.h"
|
||||
#include "fc/fc_tasks.h"
|
||||
|
|
@ -39,11 +39,19 @@ extern "C" {
|
|||
extern "C" {
|
||||
uint8_t batteryCellCount = 3;
|
||||
int16_t rcCommand[4] = {0, 0, 0, 0};
|
||||
int16_t telemTemperature1 = 0;
|
||||
baro_t baro = { .baroTemperature = 50 };
|
||||
telemetryConfig_t telemetryConfig_System;
|
||||
}
|
||||
|
||||
static int16_t gyroTemperature;
|
||||
int16_t gyroGetTemperature(void) {
|
||||
return gyroTemperature;
|
||||
}
|
||||
|
||||
static uint16_t vbat = 100;
|
||||
uint16_t getVbat(void)
|
||||
{
|
||||
return vbat;
|
||||
}
|
||||
|
||||
#define SERIAL_BUFFER_SIZE 256
|
||||
|
||||
|
|
@ -54,12 +62,6 @@ typedef struct serialPortStub_s {
|
|||
} serialPortStub_t;
|
||||
|
||||
|
||||
static uint16_t vbat = 100;
|
||||
uint16_t getVbat(void)
|
||||
{
|
||||
return vbat;
|
||||
}
|
||||
|
||||
static serialPortStub_t serialWriteStub;
|
||||
static serialPortStub_t serialReadStub;
|
||||
|
||||
|
|
@ -72,6 +74,7 @@ serialPortConfig_t serialTestInstanceConfig = {
|
|||
|
||||
static serialPortConfig_t *findSerialPortConfig_stub_retval;
|
||||
static portSharing_e determinePortSharing_stub_retval;
|
||||
static bool portIsShared = false;
|
||||
static bool openSerial_called = false;
|
||||
static bool telemetryDetermineEnabledState_stub_retval;
|
||||
|
||||
|
|
@ -105,6 +108,17 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing)
|
|||
}
|
||||
|
||||
|
||||
bool isSerialPortShared(const serialPortConfig_t *portConfig,
|
||||
uint16_t functionMask,
|
||||
serialPortFunction_e sharedWithFunction)
|
||||
{
|
||||
EXPECT_EQ(portConfig, findSerialPortConfig_stub_retval);
|
||||
EXPECT_EQ(functionMask, FUNCTION_RX_SERIAL);
|
||||
EXPECT_EQ(sharedWithFunction, FUNCTION_TELEMETRY_IBUS);
|
||||
return portIsShared;
|
||||
}
|
||||
|
||||
|
||||
serialPortConfig_t *findSerialPortConfig(uint16_t mask)
|
||||
{
|
||||
EXPECT_EQ(mask, FUNCTION_TELEMETRY_IBUS);
|
||||
|
|
@ -179,6 +193,7 @@ void serialTestResetBuffers()
|
|||
|
||||
void serialTestResetPort()
|
||||
{
|
||||
portIsShared = false;
|
||||
openSerial_called = false;
|
||||
determinePortSharing_stub_retval = PORTSHARING_UNUSED;
|
||||
telemetryDetermineEnabledState_stub_retval = true;
|
||||
|
|
@ -235,11 +250,31 @@ TEST_F(IbusTelemteryInitUnitTest, Test_IbusInitEnabled)
|
|||
}
|
||||
|
||||
|
||||
TEST_F(IbusTelemteryInitUnitTest, Test_IbusInitSerialRxAndTelemetryEnabled)
|
||||
{
|
||||
findSerialPortConfig_stub_retval = &serialTestInstanceConfig;
|
||||
|
||||
//given stuff in serial read
|
||||
serialReadStub.end++;
|
||||
//and serial rx enabled too
|
||||
portIsShared = true;
|
||||
|
||||
//when initializing and polling ibus
|
||||
initIbusTelemetry();
|
||||
checkIbusTelemetryState();
|
||||
handleIbusTelemetry();
|
||||
|
||||
//then all is read from serial port
|
||||
EXPECT_NE(serialReadStub.pos, serialReadStub.end);
|
||||
EXPECT_FALSE(openSerial_called);
|
||||
}
|
||||
|
||||
class IbusTelemetryProtocolUnitTestBase : public ::testing::Test
|
||||
{
|
||||
protected:
|
||||
virtual void SetUp()
|
||||
{
|
||||
serialTestResetPort();
|
||||
telemetryConfigMutable()->report_cell_voltage = false;
|
||||
serialTestResetBuffers();
|
||||
initIbusTelemetry();
|
||||
|
|
@ -373,33 +408,20 @@ TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToGetMeasurementVbattPackV
|
|||
|
||||
TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToGetMeasurementTemperature)
|
||||
{
|
||||
#ifdef BARO
|
||||
//Given ibus command: Sensor at address 2, please send your measurement
|
||||
//then we respond
|
||||
baro.baroTemperature = 50;
|
||||
checkResponseToCommand("\x04\xA2\x59\xff", 4, "\x06\xA2\x95\x01\xc1\xFE", 6);
|
||||
gyroTemperature = 50;
|
||||
checkResponseToCommand("\x04\xA2\x59\xff", 4, "\x06\xA2\x84\x03\xd0\xfe", 6);
|
||||
|
||||
//Given ibus command: Sensor at address 2, please send your measurement
|
||||
//then we respond
|
||||
baro.baroTemperature = 59; //test integer rounding
|
||||
checkResponseToCommand("\x04\xA2\x59\xff", 4, "\x06\xA2\x96\x01\xc0\xFE", 6);
|
||||
gyroTemperature = 59; //test integer rounding
|
||||
checkResponseToCommand("\x04\xA2\x59\xff", 4, "\x06\xA2\xde\x03\x76\xfe", 6);
|
||||
|
||||
//Given ibus command: Sensor at address 2, please send your measurement
|
||||
//then we respond
|
||||
baro.baroTemperature = 150;
|
||||
checkResponseToCommand("\x04\xA2\x59\xff", 4, "\x06\xA2\x9f\x01\xb7\xFE", 6);
|
||||
#else
|
||||
#error not tested, may be obsolete
|
||||
// //Given ibus command: Sensor at address 2, please send your measurement
|
||||
// //then we respond with: I'm reading 0 degrees + constant offset 0x190
|
||||
// telemTemperature1 = 0;
|
||||
// checkResponseToCommand("\x04\xA2\x59\xff", 4, "\x06\xA2\x90\x01\xC6\xFE", 6);
|
||||
|
||||
// //Given ibus command: Sensor at address 2, please send your measurement
|
||||
// //then we respond with: I'm reading 100 degrees + constant offset 0x190
|
||||
// telemTemperature1 = 100;
|
||||
// checkResponseToCommand("\x04\xA2\x59\xff", 4, "\x06\xA2\xF4\x01\x62\xFE", 6);
|
||||
#endif
|
||||
gyroTemperature = 150;
|
||||
checkResponseToCommand("\x04\xA2\x59\xff", 4, "\x06\xA2\x6c\x07\xe4\xfe", 6);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -468,19 +490,12 @@ TEST_F(IbusTelemteryProtocolUnitTestDaisyChained, Test_IbusRespondToGetMeasureme
|
|||
//then we respond with: I'm reading 0.1 volts
|
||||
batteryCellCount = 1;
|
||||
vbat = 10;
|
||||
checkResponseToCommand("\x04\xA3\x58\xff", 4, "\x06\xA3\x64\x00\xf2\xFe", 6);
|
||||
checkResponseToCommand("\x04\xA3\x58\xff", 4, "\x06\xA3\x64\x00\xf2\xfe", 6);
|
||||
|
||||
#ifdef BARO
|
||||
//Given ibus command: Sensor at address 4, please send your measurement
|
||||
//then we respond
|
||||
baro.baroTemperature = 150;
|
||||
checkResponseToCommand("\x04\xA4\x57\xff", 4, "\x06\xA4\x9f\x01\xb5\xFE", 6);
|
||||
#else
|
||||
//Given ibus command: Sensor at address 4, please send your measurement
|
||||
//then we respond with: I'm reading 100 degrees + constant offset 0x190
|
||||
telemTemperature1 = 100;
|
||||
checkResponseToCommand("\x04\xA4\x57\xff", 4, "\x06\xA4\xF4\x01\x60\xFE", 6);
|
||||
#endif
|
||||
gyroTemperature = 150;
|
||||
checkResponseToCommand("\x04\xA4\x57\xff", 4, "\x06\xA4\x6c\x07\xe2\xfe", 6);
|
||||
|
||||
//Given ibus command: Sensor at address 5, please send your measurement
|
||||
//then we respond with: I'm reading 100 rpm
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue