From 588166fa56a33c01aeb2f26be4a8b0b3d0751154 Mon Sep 17 00:00:00 2001 From: Jakub Date: Fri, 23 Aug 2019 20:08:54 +0200 Subject: [PATCH] Fixed IBUS protocol for AFHDS 3 and latest AFHDS 2a receivers. --- src/main/rx/ibus.c | 15 ++++++++++----- src/test/unit/rx_ibus_unittest.cc | 14 +++++++------- 2 files changed, 17 insertions(+), 12 deletions(-) diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c index da7e9257a3..169ef497ca 100644 --- a/src/main/rx/ibus.c +++ b/src/main/rx/ibus.c @@ -52,7 +52,9 @@ #include "telemetry/ibus.h" #include "telemetry/ibus_shared.h" -#define IBUS_MAX_CHANNEL 14 +#define IBUS_MAX_CHANNEL 18 +//In AFHDS there is 18 channels encoded in 14 slots (each slot is 2 byte long) +#define IBUS_MAX_SLOTS 14 #define IBUS_BUFFSIZE 32 #define IBUS_MODEL_IA6B 0 #define IBUS_MODEL_IA6 1 @@ -137,7 +139,7 @@ static bool isChecksumOkIa6(void) 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) { + for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_SLOTS; i++, offset += 2) { chksum += ibus[offset] + (ibus[offset + 1] << 8); } return chksum == rxsum; @@ -156,9 +158,12 @@ static bool checksumIsOk(void) { 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); + for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_SLOTS; i++, offset += 2) { + ibusChannelData[i] = ibus[offset] + ((ibus[offset + 1] & 0x0F) << 8); + } + //latest IBUS recievers are using prviously not used 4 bits on every channel to incresse total channel count + for (i = IBUS_MAX_SLOTS, offset = ibusChannelOffset + 1; i < IBUS_MAX_CHANNEL; i++, offset += 6) { + ibusChannelData[i] = ((ibus[offset] & 0xF0) >> 4) | (ibus[offset + 2] & 0xF0) | ((ibus[offset + 4] & 0xF0) << 4); } } diff --git a/src/test/unit/rx_ibus_unittest.cc b/src/test/unit/rx_ibus_unittest.cc index 5f1ca6571f..6204fa670d 100644 --- a/src/test/unit/rx_ibus_unittest.cc +++ b/src/test/unit/rx_ibus_unittest.cc @@ -212,7 +212,7 @@ TEST_F(IbusRxInitUnitTest, Test_IbusRxNotEnabled) // EXPECT_EQ(NULL, rxRuntimeConfig.rcReadRawFn); // EXPECT_EQ(NULL, rxRuntimeConfig.rcFrameStatusFn); - EXPECT_EQ(14, rxRuntimeConfig.channelCount); + EXPECT_EQ(18, rxRuntimeConfig.channelCount); EXPECT_EQ(20000, rxRuntimeConfig.rxRefreshRate); EXPECT_FALSE(NULL == rxRuntimeConfig.rcReadRawFn); EXPECT_FALSE(NULL == rxRuntimeConfig.rcFrameStatusFn); @@ -227,7 +227,7 @@ TEST_F(IbusRxInitUnitTest, Test_IbusRxEnabled) EXPECT_TRUE(ibusInit(&initialRxConfig, &rxRuntimeConfig)); - EXPECT_EQ(14, rxRuntimeConfig.channelCount); + EXPECT_EQ(18, rxRuntimeConfig.channelCount); EXPECT_EQ(20000, rxRuntimeConfig.rxRefreshRate); EXPECT_FALSE(NULL == rxRuntimeConfig.rcReadRawFn); EXPECT_FALSE(NULL == rxRuntimeConfig.rcFrameStatusFn); @@ -283,10 +283,10 @@ TEST_F(IbusRxProtocollUnitTest, Test_InitialFrameState) TEST_F(IbusRxProtocollUnitTest, Test_IA6B_OnePacketReceived) { 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 + 0x00, 0xE0, 0x01, 0x00, 0x02, 0x00, 0x03, 0xF0, 0x04, 0x00, //channel 1..5 + 15 + 16 + 0x05, 0x00, 0x06, 0x00, 0x07, 0x10, 0x08, 0x00, 0x09, 0x10, //channel 6..10 + 17 + 18(lsb) + 0x0a, 0x10, 0x0b, 0x00, 0x0c, 0x00, 0x0d, 0x00, //channel 11..14 + 18 + 0x84, 0xfd}; //checksum for (size_t i=0; i < sizeof(packet); i++) { EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig)); @@ -298,7 +298,7 @@ TEST_F(IbusRxProtocollUnitTest, Test_IA6B_OnePacketReceived) EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig)); //check that channel values have been updated - for (int i=0; i<14; i++) { + for (int i=0; i<18; i++) { ASSERT_EQ(i, rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, i)); } }