From bad5fb418dfe364eba50b9847856ad081679b5aa Mon Sep 17 00:00:00 2001 From: PraiseSatan Date: Sat, 6 Jul 2019 17:27:43 +1000 Subject: [PATCH] AP_RCProtocol: added ibus RX support to recieve channel data from ibus transmitters. --- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 4 + libraries/AP_RCProtocol/AP_RCProtocol.h | 1 + .../AP_RCProtocol/AP_RCProtocol_IBUS.cpp | 106 ++++++++++++++++++ libraries/AP_RCProtocol/AP_RCProtocol_IBUS.h | 43 +++++++ .../RCProtocolTest/RCProtocolTest.cpp | 4 + 5 files changed, 158 insertions(+) create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_IBUS.cpp create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_IBUS.h diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 1dcd67e39f..2cd7345b60 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -18,6 +18,7 @@ #include "AP_RCProtocol.h" #include "AP_RCProtocol_PPMSum.h" #include "AP_RCProtocol_DSM.h" +#include "AP_RCProtocol_IBUS.h" #include "AP_RCProtocol_SBUS.h" #include "AP_RCProtocol_SUMD.h" #include "AP_RCProtocol_SRXL.h" @@ -29,6 +30,7 @@ AP_RCProtocol *AP_RCProtocol::_singleton; void AP_RCProtocol::init() { backend[AP_RCProtocol::PPM] = new AP_RCProtocol_PPMSum(*this); + backend[AP_RCProtocol::IBUS] = new AP_RCProtocol_IBUS(*this); backend[AP_RCProtocol::SBUS] = new AP_RCProtocol_SBUS(*this, true); backend[AP_RCProtocol::SBUS_NI] = new AP_RCProtocol_SBUS(*this, false); backend[AP_RCProtocol::DSM] = new AP_RCProtocol_DSM(*this); @@ -205,6 +207,8 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol) switch (protocol) { case PPM: return "PPM"; + case IBUS: + return "IBUS"; case SBUS: case SBUS_NI: return "SBUS"; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index e28c02d2e7..3bd2467857 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -32,6 +32,7 @@ public: enum rcprotocol_t { PPM = 0, + IBUS, SBUS, SBUS_NI, DSM, diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.cpp new file mode 100644 index 0000000000..4138c7615d --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.cpp @@ -0,0 +1,106 @@ +/* + * This file 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. + * + * This file 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 this program. If not, see . + * + */ + +#include "AP_RCProtocol_IBUS.h" + +// constructor +AP_RCProtocol_IBUS::AP_RCProtocol_IBUS(AP_RCProtocol &_frontend) : + AP_RCProtocol_Backend(_frontend) +{} + +// decode a full IBUS frame +bool AP_RCProtocol_IBUS::ibus_decode(const uint8_t frame[IBUS_FRAME_SIZE], uint16_t *values, bool *ibus_failsafe) +{ + uint32_t chksum = 96; + + /* check frame boundary markers to avoid out-of-sync cases */ + if ((frame[0] != 0x20) || (frame[1] != 0x40)) { + return false; + } + + /* use the decoder matrix to extract channel data */ + for (uint8_t channel = 0, pick=2; channel < IBUS_INPUT_CHANNELS; channel++, pick+=2) { + values[channel]=frame[pick]|(frame[pick+1] & 0x0F)<<8; + chksum+=frame[pick]+frame[pick+1]; + } + + chksum += frame[IBUS_FRAME_SIZE-2]|frame[IBUS_FRAME_SIZE-1]<<8; + + if (chksum!=0xFFFF) { + return false; + } + + if ((frame[3]&0xF0) || (frame[9]&0xF0)) { + *ibus_failsafe = true; + } else { + *ibus_failsafe = false; + } + + return true; +} + + +/* + process an IBUS input pulse of the given width + */ +void AP_RCProtocol_IBUS::process_pulse(uint32_t w0, uint32_t w1) +{ + uint8_t b; + if (ss.process_pulse(w0, w1, b)) { + _process_byte(ss.get_byte_timestamp_us(), b); + } +} + +// support byte input +void AP_RCProtocol_IBUS::_process_byte(uint32_t timestamp_us, uint8_t b) +{ + const bool have_frame_gap = (timestamp_us - byte_input.last_byte_us >= 2000U); + byte_input.last_byte_us = timestamp_us; + + if (have_frame_gap) { + // if we have a frame gap then this must be the start of a new + // frame + byte_input.ofs = 0; + } + if (b != 0x20 && byte_input.ofs == 0) { + // definately not IBUS, missing header byte + return; + } + if (byte_input.ofs == 0 && !have_frame_gap) { + // must have a frame gap before the start of a new IBUS frame + return; + } + + byte_input.buf[byte_input.ofs++] = b; + + if (byte_input.ofs == sizeof(byte_input.buf)) { + uint16_t values[IBUS_INPUT_CHANNELS]; + bool ibus_failsafe = false; + if (ibus_decode(byte_input.buf, values, &ibus_failsafe)) { + add_input(IBUS_INPUT_CHANNELS, values, ibus_failsafe); + } + byte_input.ofs = 0; + } +} + +// support byte input +void AP_RCProtocol_IBUS::process_byte(uint8_t b, uint32_t baudrate) +{ + if (baudrate != 115200) { + return; + } + _process_byte(AP_HAL::micros(), b); +} diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.h b/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.h new file mode 100644 index 0000000000..8128c99a66 --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.h @@ -0,0 +1,43 @@ +/* + * This file 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. + * + * This file 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 this program. If not, see . + * + */ + +#pragma once + +#define IBUS_FRAME_SIZE 32 +#define IBUS_INPUT_CHANNELS 14 + +#include "AP_RCProtocol.h" +#include "SoftSerial.h" + +class AP_RCProtocol_IBUS : public AP_RCProtocol_Backend +{ +public: + AP_RCProtocol_IBUS(AP_RCProtocol &_frontend); + void process_pulse(uint32_t width_s0, uint32_t width_s1) override; + void process_byte(uint8_t byte, uint32_t baudrate) override; +private: + void _process_byte(uint32_t timestamp_us, uint8_t byte); + bool ibus_decode(const uint8_t frame[IBUS_FRAME_SIZE], uint16_t *values, bool *ibus_failsafe); + + SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1}; + uint32_t saved_width; + + struct { + uint8_t buf[IBUS_FRAME_SIZE]; + uint8_t ofs; + uint32_t last_byte_us; + } byte_input; +}; diff --git a/libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp b/libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp index 9147602ad4..d1c88bad5c 100644 --- a/libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp +++ b/libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp @@ -209,10 +209,14 @@ void loop() const uint16_t sumd_output[] = {1597, 1076, 1514, 1514, 1100, 1100, 1500, 1500}; const uint16_t sumd_output2[] = {1516, 1500, 1100, 1500, 1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900}; + + const uint8_t ibus_bytes[] = {0x20, 0x40, 0xdc, 0x05, 0xdc, 0x05, 0xe8, 0x03, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0x47, 0xf3}; + const uint16_t ibus_output[] = {1500, 1500, 1000, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500}; test_protocol("SRXL", 115200, srxl_bytes, sizeof(srxl_bytes), srxl_output, ARRAY_SIZE(srxl_output), 1); test_protocol("SUMD", 115200, sumd_bytes, sizeof(sumd_bytes), sumd_output, ARRAY_SIZE(sumd_output), 1); test_protocol("SUMD", 115200, sumd_bytes2, sizeof(sumd_bytes2), sumd_output2, ARRAY_SIZE(sumd_output2), 1); + test_protocol("IBUS", 115200, ibus_bytes, sizeof(ibus_bytes), ibus_output, ARRAY_SIZE(ibus_output), 1); // SBUS needs 3 repeats to pass the RCProtocol 3 frames test test_protocol("SBUS", 100000, sbus_bytes, sizeof(sbus_bytes), sbus_output, ARRAY_SIZE(sbus_output), 3);