Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 15:25:36 +03:00
Code Issues Wiki Activity

Merge pull request #2639 from unitware/beta-combine-ibus-serial-rx-and-telemetry

Browse source 
Combine ibus serial rx and telemetry on same uart and pin
This commit is contained in:
Michael Keller 2017-03-14 11:11:58 +13:00 • committed by GitHub
commit 4a7555fde3
12 changed files with 640 additions and 347 deletions
Download patch file Download diff file Expand all files Collapse all files

1
Makefile
View file

@ -684,6 +684,7 @@ COMMON_SRC = \
telemetry/ltm.c \ telemetry/ltm.c \
telemetry/mavlink.c \ telemetry/mavlink.c \
telemetry/ibus.c \ telemetry/ibus.c \
telemetry/ibus_shared.c \
sensors/esc_sensor.c \ sensors/esc_sensor.c \
io/vtx_string.c \ io/vtx_string.c \
io/vtx_smartaudio.c \ io/vtx_smartaudio.c \

123
src/main/rx/ibus.c
View file

@ -43,6 +43,8 @@
#include "rx/rx.h" #include "rx/rx.h"
#include "rx/ibus.h" #include "rx/ibus.h"
#include "telemetry/ibus.h"
#include "telemetry/ibus_shared.h"
#define IBUS_MAX_CHANNEL 14 #define IBUS_MAX_CHANNEL 14
#define IBUS_BUFFSIZE 32 #define IBUS_BUFFSIZE 32
@ -51,11 +53,14 @@
#define IBUS_FRAME_GAP 500 #define IBUS_FRAME_GAP 500
#define IBUS_BAUDRATE 115200 #define IBUS_BAUDRATE 115200
#define IBUS_TELEMETRY_PACKET_LENGTH (4)
#define IBUS_SERIAL_RX_PACKET_LENGTH (32)
static uint8_t ibusModel; static uint8_t ibusModel;
static uint8_t ibusSyncByte; static uint8_t ibusSyncByte;
static uint8_t ibusFrameSize; static uint8_t ibusFrameSize;
static uint8_t ibusChannelOffset; static uint8_t ibusChannelOffset;
static uint8_t rxBytesToIgnore;
static uint16_t ibusChecksum; static uint16_t ibusChecksum;
static bool ibusFrameDone = false; static bool ibusFrameDone = false;
@ -63,6 +68,13 @@ static uint32_t ibusChannelData[IBUS_MAX_CHANNEL];
static uint8_t ibus[IBUS_BUFFSIZE] = { 0, }; 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 // Receive ISR callback
static void ibusDataReceive(uint16_t c) static void ibusDataReceive(uint16_t c)
{ {
@ -72,29 +84,29 @@ static void ibusDataReceive(uint16_t c)
ibusTime = micros(); ibusTime = micros();
if ((ibusTime - ibusTimeLast) > IBUS_FRAME_GAP) if ((ibusTime - ibusTimeLast) > IBUS_FRAME_GAP) {
ibusFramePosition = 0; ibusFramePosition = 0;
rxBytesToIgnore = 0;
} else if (rxBytesToIgnore) {
rxBytesToIgnore--;
return;
}
ibusTimeLast = ibusTime; ibusTimeLast = ibusTime;
if (ibusFramePosition == 0) { if (ibusFramePosition == 0) {
if (ibusSyncByte == 0) { if (isValidIa6bIbusPacketLength(c)) {
// detect the frame type based on the STX byte. ibusModel = IBUS_MODEL_IA6B;
if (c == 0x55) { ibusSyncByte = c;
ibusModel = IBUS_MODEL_IA6; ibusFrameSize = c;
ibusSyncByte = 0x55; ibusChannelOffset = 2;
ibusFrameSize = 31; ibusChecksum = 0xFFFF;
ibusChecksum = 0x0000; } else if ((ibusSyncByte == 0) && (c == 0x55)) {
ibusChannelOffset = 1; ibusModel = IBUS_MODEL_IA6;
} else if (c == 0x20) { ibusSyncByte = 0x55;
ibusModel = IBUS_MODEL_IA6B; ibusFrameSize = 31;
ibusSyncByte = 0x20; ibusChecksum = 0x0000;
ibusFrameSize = 32; ibusChannelOffset = 1;
ibusChannelOffset = 2;
ibusChecksum = 0xFFFF;
} else {
return;
}
} else if (ibusSyncByte != c) { } else if (ibusSyncByte != c) {
return; 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) static uint8_t ibusFrameStatus(void)
{ {
uint8_t i, offset;
uint8_t frameStatus = RX_FRAME_PENDING; uint8_t frameStatus = RX_FRAME_PENDING;
uint16_t chksum, rxsum;
if (!ibusFrameDone) { if (!ibusFrameDone) {
return frameStatus; return frameStatus;
@ -121,32 +164,30 @@ static uint8_t ibusFrameStatus(void)
ibusFrameDone = false; ibusFrameDone = false;
chksum = ibusChecksum; if (checksumIsOk()) {
rxsum = ibus[ibusFrameSize - 2] + (ibus[ibusFrameSize - 1] << 8); if (ibusModel == IBUS_MODEL_IA6 || ibusSyncByte == 0x20) {
if (ibusModel == IBUS_MODEL_IA6) { updateChannelData();
for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_CHANNEL; i++, offset += 2) frameStatus = RX_FRAME_COMPLETE;
chksum += ibus[offset] + (ibus[offset + 1] << 8); }
} else { else
for (i = 0; i < 30; i++) {
chksum -= ibus[i]; #if defined(TELEMETRY) && defined(TELEMETRY_IBUS)
} rxBytesToIgnore = respondToIbusRequest(ibus);
#endif
if (chksum == rxsum) {
for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_CHANNEL; i++, offset += 2) {
ibusChannelData[i] = ibus[offset] + (ibus[offset + 1] << 8);
} }
frameStatus = RX_FRAME_COMPLETE;
} }
return frameStatus; return frameStatus;
} }
static uint16_t ibusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) static uint16_t ibusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
{ {
UNUSED(rxRuntimeConfig); UNUSED(rxRuntimeConfig);
return ibusChannelData[chan]; return ibusChannelData[chan];
} }
bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{ {
UNUSED(rxConfig); UNUSED(rxConfig);
@ -164,25 +205,29 @@ bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
} }
#ifdef TELEMETRY #ifdef TELEMETRY
bool portShared = telemetryCheckRxPortShared(portConfig); // bool portShared = telemetryCheckRxPortShared(portConfig);
bool portShared = isSerialPortShared(portConfig, FUNCTION_RX_SERIAL, FUNCTION_TELEMETRY_IBUS);
#else #else
bool portShared = false; bool portShared = false;
#endif #endif
rxBytesToIgnore = 0;
serialPort_t *ibusPort = openSerialPort(portConfig->identifier, serialPort_t *ibusPort = openSerialPort(portConfig->identifier,
FUNCTION_RX_SERIAL, FUNCTION_RX_SERIAL,
ibusDataReceive, ibusDataReceive,
IBUS_BAUDRATE, IBUS_BAUDRATE,
portShared ? MODE_RXTX : MODE_RX, 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) { if (portShared) {
telemetrySharedPort = ibusPort; initSharedIbusTelemetry(ibusPort);
} }
#endif #endif
return ibusPort != NULL; return ibusPort != NULL;
} }
#endif
#endif //SERIAL_RX

2
src/main/target/NAZE/target.h
View file

@ -17,6 +17,8 @@
#pragma once #pragma once
#define TELEMETRY_IBUS
#define TARGET_CONFIG #define TARGET_CONFIG
#define TARGET_VALIDATECONFIG #define TARGET_VALIDATECONFIG
#define USE_HARDWARE_REVISION_DETECTION #define USE_HARDWARE_REVISION_DETECTION

2
src/main/target/OMNIBUS/target.h
View file

@ -17,6 +17,8 @@
#pragma once #pragma once
#undef TELEMETRY_IBUS //no space left
#define TARGET_BOARD_IDENTIFIER "OMNI" // https://en.wikipedia.org/wiki/Omnibus #define TARGET_BOARD_IDENTIFIER "OMNI" // https://en.wikipedia.org/wiki/Omnibus
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE

1
src/main/target/common.h
View file

@ -97,6 +97,7 @@
#define GPS #define GPS
#define CMS #define CMS
#define TELEMETRY_CRSF #define TELEMETRY_CRSF
#define TELEMETRY_IBUS
#define TELEMETRY_JETIEXBUS #define TELEMETRY_JETIEXBUS
#define TELEMETRY_MAVLINK #define TELEMETRY_MAVLINK
#define TELEMETRY_SRXL #define TELEMETRY_SRXL

278
src/main/telemetry/ibus.c
View file

@ -55,114 +55,9 @@
#include "scheduler/scheduler.h" #include "scheduler/scheduler.h"
#include "telemetry/ibus.h" #include "telemetry/ibus.h"
#include "telemetry/ibus_shared.h"
#include "telemetry/telemetry.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) #define IBUS_TASK_PERIOD_US (1000)
@ -170,37 +65,11 @@
#define IBUS_BAUDRATE (115200) #define IBUS_BAUDRATE (115200)
#define IBUS_CYCLE_TIME_MS (8) #define IBUS_CYCLE_TIME_MS (8)
#define IBUS_CHECKSUM_SIZE (2)
#define IBUS_MIN_LEN (2 + IBUS_CHECKSUM_SIZE) #define IBUS_MIN_LEN (2 + IBUS_CHECKSUM_SIZE)
#define IBUS_MAX_TX_LEN (6) #define IBUS_MAX_TX_LEN (6)
#define IBUS_MAX_RX_LEN (4) #define IBUS_MAX_RX_LEN (4)
#define IBUS_RX_BUF_LEN (IBUS_MAX_RX_LEN) #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 serialPort_t *ibusSerialPort = NULL;
static serialPortConfig_t *ibusSerialPortConfig; static serialPortConfig_t *ibusSerialPortConfig;
@ -212,131 +81,9 @@ static uint8_t outboundBytesToIgnoreOnRxCount = 0;
static bool ibusTelemetryEnabled = false; static bool ibusTelemetryEnabled = false;
static portSharing_e ibusPortSharing; 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 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) 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; ibusReceiveBuffer[bufferLength - 1] = value;
} }
void initIbusTelemetry(void) void initIbusTelemetry(void)
{ {
ibusSerialPortConfig = findSerialPortConfig(FUNCTION_TELEMETRY_IBUS); ibusSerialPortConfig = findSerialPortConfig(FUNCTION_TELEMETRY_IBUS);
ibusPortSharing = determinePortSharing(ibusSerialPortConfig, FUNCTION_TELEMETRY_IBUS); ibusPortSharing = determinePortSharing(ibusSerialPortConfig, FUNCTION_TELEMETRY_IBUS);
ibusBaseAddress = INVALID_IBUS_ADDRESS; ibusTelemetryEnabled = false;
} }
void handleIbusTelemetry(void) void handleIbusTelemetry(void)
{ {
if (!ibusTelemetryEnabled) { if (!ibusTelemetryEnabled) {
@ -367,12 +116,13 @@ void handleIbusTelemetry(void)
pushOntoTail(ibusReceiveBuffer, IBUS_RX_BUF_LEN, c); pushOntoTail(ibusReceiveBuffer, IBUS_RX_BUF_LEN, c);
if (isChecksumOk(ibusReceiveBuffer)) { if (isChecksumOkIa6b(ibusReceiveBuffer, IBUS_RX_BUF_LEN)) {
respondToIbusRequest(ibusReceiveBuffer); outboundBytesToIgnoreOnRxCount += respondToIbusRequest(ibusReceiveBuffer);
} }
} }
} }
bool checkIbusTelemetryState(void) bool checkIbusTelemetryState(void)
{ {
bool newTelemetryEnabledValue = telemetryDetermineEnabledState(ibusPortSharing); bool newTelemetryEnabledValue = telemetryDetermineEnabledState(ibusPortSharing);
@ -391,32 +141,34 @@ bool checkIbusTelemetryState(void)
return true; return true;
} }
void configureIbusTelemetryPort(void) void configureIbusTelemetryPort(void)
{ {
portOptions_t portOptions;
if (!ibusSerialPortConfig) { if (!ibusSerialPortConfig) {
return; 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, ibusSerialPort = openSerialPort(ibusSerialPortConfig->identifier, FUNCTION_TELEMETRY_IBUS, NULL, IBUS_BAUDRATE, IBUS_UART_MODE, SERIAL_BIDIR);
IBUS_UART_MODE, portOptions);
if (!ibusSerialPort) { if (!ibusSerialPort) {
return; return;
} }
initSharedIbusTelemetry(ibusSerialPort);
ibusTelemetryEnabled = true; ibusTelemetryEnabled = true;
outboundBytesToIgnoreOnRxCount = 0; outboundBytesToIgnoreOnRxCount = 0;
} }
void freeIbusTelemetryPort(void) void freeIbusTelemetryPort(void)
{ {
closeSerialPort(ibusSerialPort); closeSerialPort(ibusSerialPort);
ibusSerialPort = NULL; ibusSerialPort = NULL;
ibusTelemetryEnabled = false; ibusTelemetryEnabled = false;
} }

204
src/main/telemetry/ibus_shared.c Normal file
View 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
View 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);

13
src/main/telemetry/telemetry.c
View file

@ -126,7 +126,18 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing)
bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig) 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; serialPort_t *telemetrySharedPort = NULL;

9
src/test/Makefile
View file

@ -851,6 +851,14 @@ $(OBJECT_DIR)/telemetry/ibus.o : \
@mkdir -p $(dir $@) @mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/telemetry/ibus.c -o $@ $(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 : \ $(OBJECT_DIR)/telemetry_ibus_unittest.o : \
$(TEST_DIR)/telemetry_ibus_unittest.cc \ $(TEST_DIR)/telemetry_ibus_unittest.cc \
$(GTEST_HEADERS) $(GTEST_HEADERS)
@ -860,6 +868,7 @@ $(OBJECT_DIR)/telemetry_ibus_unittest.o : \
$(OBJECT_DIR)/telemetry_ibus_unittest : \ $(OBJECT_DIR)/telemetry_ibus_unittest : \
$(OBJECT_DIR)/telemetry_ibus_unittest.o \ $(OBJECT_DIR)/telemetry_ibus_unittest.o \
$(OBJECT_DIR)/telemetry/ibus_shared.o \
$(OBJECT_DIR)/telemetry/ibus.o \ $(OBJECT_DIR)/telemetry/ibus.o \
$(OBJECT_DIR)/gtest_main.a $(OBJECT_DIR)/gtest_main.a

217
src/test/unit/rx_ibus_unittest.cc
View file

@ -25,13 +25,25 @@ extern "C" {
#include "io/serial.h" #include "io/serial.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "rx/ibus.h" #include "rx/ibus.h"
#include "telemetry/ibus_shared.h"
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
#include "fc/rc_controls.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
} }
#include "unittest_macros.h" #include "unittest_macros.h"
#include "gtest/gtest.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) bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig)
{ {
@ -42,6 +54,11 @@ bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig)
serialPort_t * telemetrySharedPort = NULL; serialPort_t * telemetrySharedPort = NULL;
static uint16_t vbat = 100;
uint16_t getVbat(void)
{
return vbat;
}
uint32_t microseconds_stub_value = 0; uint32_t microseconds_stub_value = 0;
uint32_t micros(void) uint32_t micros(void)
@ -49,8 +66,15 @@ uint32_t micros(void)
return microseconds_stub_value; return microseconds_stub_value;
} }
#define SERIAL_BUFFER_SIZE 256
#define SERIAL_PORT_DUMMY_IDENTIFIER (serialPortIdentifier_e)0x1234 #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 serialPort_t serialTestInstance;
static serialPortConfig_t serialTestInstanceConfig = { static serialPortConfig_t serialTestInstanceConfig = {
.identifier = SERIAL_PORT_DUMMY_IDENTIFIER, .identifier = SERIAL_PORT_DUMMY_IDENTIFIER,
@ -60,7 +84,18 @@ static serialPortConfig_t serialTestInstanceConfig = {
static serialReceiveCallbackPtr stub_serialRxCallback; static serialReceiveCallbackPtr stub_serialRxCallback;
static serialPortConfig_t *findSerialPortConfig_stub_retval; static serialPortConfig_t *findSerialPortConfig_stub_retval;
static bool openSerial_called = false; 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) serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function)
{ {
@ -68,6 +103,8 @@ serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function)
return findSerialPortConfig_stub_retval; return findSerialPortConfig_stub_retval;
} }
static portMode_t serialExpectedMode = MODE_RX;
static portOptions_t serialExpectedOptions = SERIAL_UNIDIR;
serialPort_t *openSerialPort( serialPort_t *openSerialPort(
serialPortIdentifier_e identifier, serialPortIdentifier_e identifier,
@ -81,22 +118,71 @@ serialPort_t *openSerialPort(
openSerial_called = true; openSerial_called = true;
EXPECT_FALSE(NULL == callback); EXPECT_FALSE(NULL == callback);
EXPECT_EQ(identifier, SERIAL_PORT_DUMMY_IDENTIFIER); EXPECT_EQ(identifier, SERIAL_PORT_DUMMY_IDENTIFIER);
EXPECT_EQ(options, SERIAL_UNIDIR); EXPECT_EQ(options, serialExpectedOptions);
EXPECT_EQ(function, FUNCTION_RX_SERIAL); EXPECT_EQ(function, FUNCTION_RX_SERIAL);
EXPECT_EQ(baudrate, 115200); EXPECT_EQ(baudrate, 115200);
EXPECT_EQ(mode, MODE_RX); EXPECT_EQ(mode, serialExpectedMode);
stub_serialRxCallback = callback; stub_serialRxCallback = callback;
return &serialTestInstance; 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() void serialTestResetPort()
{ {
openSerial_called = false; openSerial_called = false;
stub_serialRxCallback = NULL; 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 class IbusRxInitUnitTest : public ::testing::Test
{ {
@ -154,23 +240,36 @@ protected:
virtual void SetUp() virtual void SetUp()
{ {
serialTestResetPort(); serialTestResetPort();
resetStubTelemetry();
portIsShared = true;
serialExpectedOptions = SERIAL_BIDIR;
serialExpectedMode = MODE_RXTX;
const rxConfig_t initialRxConfig = {}; const rxConfig_t initialRxConfig = {};
findSerialPortConfig_stub_retval = &serialTestInstanceConfig; findSerialPortConfig_stub_retval = &serialTestInstanceConfig;
EXPECT_TRUE(ibusInit(&initialRxConfig, &rxRuntimeConfig)); EXPECT_TRUE(ibusInit(&initialRxConfig, &rxRuntimeConfig));
EXPECT_TRUE(initSharedIbusTelemetryCalled);
//handle that internal ibus position is not set to zero at init //handle that internal ibus position is not set to zero at init
microseconds_stub_value += 5000; microseconds_stub_value += 5000;
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); 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) TEST_F(IbusRxProtocollUnitTest, Test_InitialFrameState)
{ {
//TODO: ibusFrameStatus should return rxFrameState_t not uint8_t
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn());
//TODO: is it ok to have undefined channel values after init? //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 0x0a, 0x33, 0x0b, 0x33, 0x0c, 0x33, 0x0d, 0x33, //channel 11..14
0x00, 0x00}; //checksum 0x00, 0x00}; //checksum
isChecksumOkReturnValue = false;
for (size_t i=0; i < sizeof(packet); i++) { for (size_t i=0; i < sizeof(packet); i++) {
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn());
stub_serialRxCallback(packet[i]); stub_serialRxCallback(packet[i]);
@ -304,3 +404,110 @@ TEST_F(IbusRxProtocollUnitTest, Test_IA6_OnePacketReceivedBadCrc)
ASSERT_NE(i + (0x33 << 8), rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, i)); 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);
}

91
src/test/unit/telemetry_ibus_unittest.cc
View file

@ -26,7 +26,7 @@ extern "C" {
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
#include "telemetry/ibus.h" #include "telemetry/ibus.h"
#include "sensors/barometer.h" #include "sensors/gyro.h"
#include "sensors/battery.h" #include "sensors/battery.h"
#include "scheduler/scheduler.h" #include "scheduler/scheduler.h"
#include "fc/fc_tasks.h" #include "fc/fc_tasks.h"
@ -39,11 +39,19 @@ extern "C" {
extern "C" { extern "C" {
uint8_t batteryCellCount = 3; uint8_t batteryCellCount = 3;
int16_t rcCommand[4] = {0, 0, 0, 0}; int16_t rcCommand[4] = {0, 0, 0, 0};
int16_t telemTemperature1 = 0;
baro_t baro = { .baroTemperature = 50 };
telemetryConfig_t telemetryConfig_System; 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 #define SERIAL_BUFFER_SIZE 256
@ -54,12 +62,6 @@ typedef struct serialPortStub_s {
} serialPortStub_t; } serialPortStub_t;
static uint16_t vbat = 100;
uint16_t getVbat(void)
{
return vbat;
}
static serialPortStub_t serialWriteStub; static serialPortStub_t serialWriteStub;
static serialPortStub_t serialReadStub; static serialPortStub_t serialReadStub;
@ -72,6 +74,7 @@ serialPortConfig_t serialTestInstanceConfig = {
static serialPortConfig_t *findSerialPortConfig_stub_retval; static serialPortConfig_t *findSerialPortConfig_stub_retval;
static portSharing_e determinePortSharing_stub_retval; static portSharing_e determinePortSharing_stub_retval;
static bool portIsShared = false;
static bool openSerial_called = false; static bool openSerial_called = false;
static bool telemetryDetermineEnabledState_stub_retval; 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) serialPortConfig_t *findSerialPortConfig(uint16_t mask)
{ {
EXPECT_EQ(mask, FUNCTION_TELEMETRY_IBUS); EXPECT_EQ(mask, FUNCTION_TELEMETRY_IBUS);
@ -179,6 +193,7 @@ void serialTestResetBuffers()
void serialTestResetPort() void serialTestResetPort()
{ {
portIsShared = false;
openSerial_called = false; openSerial_called = false;
determinePortSharing_stub_retval = PORTSHARING_UNUSED; determinePortSharing_stub_retval = PORTSHARING_UNUSED;
telemetryDetermineEnabledState_stub_retval = true; 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 class IbusTelemetryProtocolUnitTestBase : public ::testing::Test
{ {
protected: protected:
virtual void SetUp() virtual void SetUp()
{ {
serialTestResetPort();
telemetryConfigMutable()->report_cell_voltage = false; telemetryConfigMutable()->report_cell_voltage = false;
serialTestResetBuffers(); serialTestResetBuffers();
initIbusTelemetry(); initIbusTelemetry();
@ -373,33 +408,20 @@ TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToGetMeasurementVbattPackV
TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToGetMeasurementTemperature) TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToGetMeasurementTemperature)
{ {
#ifdef BARO
//Given ibus command: Sensor at address 2, please send your measurement //Given ibus command: Sensor at address 2, please send your measurement
//then we respond //then we respond
baro.baroTemperature = 50; gyroTemperature = 50;
checkResponseToCommand("\x04\xA2\x59\xff", 4, "\x06\xA2\x95\x01\xc1\xFE", 6); checkResponseToCommand("\x04\xA2\x59\xff", 4, "\x06\xA2\x84\x03\xd0\xfe", 6);
//Given ibus command: Sensor at address 2, please send your measurement //Given ibus command: Sensor at address 2, please send your measurement
//then we respond //then we respond
baro.baroTemperature = 59; //test integer rounding gyroTemperature = 59; //test integer rounding
checkResponseToCommand("\x04\xA2\x59\xff", 4, "\x06\xA2\x96\x01\xc0\xFE", 6); checkResponseToCommand("\x04\xA2\x59\xff", 4, "\x06\xA2\xde\x03\x76\xfe", 6);
//Given ibus command: Sensor at address 2, please send your measurement //Given ibus command: Sensor at address 2, please send your measurement
//then we respond //then we respond
baro.baroTemperature = 150; gyroTemperature = 150;
checkResponseToCommand("\x04\xA2\x59\xff", 4, "\x06\xA2\x9f\x01\xb7\xFE", 6); checkResponseToCommand("\x04\xA2\x59\xff", 4, "\x06\xA2\x6c\x07\xe4\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
} }
@ -468,19 +490,12 @@ TEST_F(IbusTelemteryProtocolUnitTestDaisyChained, Test_IbusRespondToGetMeasureme
//then we respond with: I'm reading 0.1 volts //then we respond with: I'm reading 0.1 volts
batteryCellCount = 1; batteryCellCount = 1;
vbat = 10; 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 //Given ibus command: Sensor at address 4, please send your measurement
//then we respond //then we respond
baro.baroTemperature = 150; gyroTemperature = 150;
checkResponseToCommand("\x04\xA4\x57\xff", 4, "\x06\xA4\x9f\x01\xb5\xFE", 6); checkResponseToCommand("\x04\xA4\x57\xff", 4, "\x06\xA4\x6c\x07\xe2\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
//Given ibus command: Sensor at address 5, please send your measurement //Given ibus command: Sensor at address 5, please send your measurement
//then we respond with: I'm reading 100 rpm //then we respond with: I'm reading 100 rpm