From 987a1bbeaa9bbe703bd0b17fd1fd89e14c00e8e2 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 3 Feb 2025 00:30:21 +0100 Subject: [PATCH] IBUS RX support (cherry-pick) --- lib/Espfc/src/Device/InputIBUS.cpp | 121 +++++++++++++++++++++++ lib/Espfc/src/Device/InputIBUS.hpp | 56 +++++++++++ lib/Espfc/src/Device/InputSBUS.cpp | 2 +- lib/Espfc/src/Input.cpp | 29 ++++-- lib/Espfc/src/Input.h | 2 + lib/Espfc/src/Model.h | 2 +- lib/Espfc/src/Output/OutputIBUS.hpp | 51 ++++++++++ lib/Espfc/src/SerialManager.cpp | 16 ++- lib/Espfc/src/SerialManager.h | 2 + test/test_input_crsf/test_input_crsf.cpp | 77 ++++++++++++++- 10 files changed, 340 insertions(+), 18 deletions(-) create mode 100644 lib/Espfc/src/Device/InputIBUS.cpp create mode 100644 lib/Espfc/src/Device/InputIBUS.hpp create mode 100644 lib/Espfc/src/Output/OutputIBUS.hpp diff --git a/lib/Espfc/src/Device/InputIBUS.cpp b/lib/Espfc/src/Device/InputIBUS.cpp new file mode 100644 index 00000000..735a985b --- /dev/null +++ b/lib/Espfc/src/Device/InputIBUS.cpp @@ -0,0 +1,121 @@ +#include "InputIBUS.hpp" +#include "Utils/MemoryHelper.h" +#include + +namespace Espfc::Device +{ + +InputIBUS::InputIBUS() : _serial(NULL), _state(IBUS_LENGTH), _idx(0), _new_data(false) {} + +int InputIBUS::begin(Device::SerialDevice *serial) +{ + _serial = serial; + + std::fill_n((uint8_t*)&_data, IBUS_FRAME_SIZE, 0); + std::fill_n(_channels, CHANNELS, 0); + + return 1; +} + +InputStatus FAST_CODE_ATTR InputIBUS::update() +{ + if (!_serial) return INPUT_IDLE; + + uint8_t buff[64] = {0}; + size_t len = std::min((size_t)_serial->available(), (size_t)sizeof(buff)); + + if (len) + { + _serial->readMany(buff, len); + uint8_t* ptr = buff; + while(len--) + { + parse(_data, *ptr++); + } + } + + if (_new_data) + { + _new_data = false; + return INPUT_RECEIVED; + } + return INPUT_IDLE; +} + +void FAST_CODE_ATTR InputIBUS::parse(IBusData& frameData, int d) +{ + uint8_t* data = reinterpret_cast(&frameData); + uint8_t c = d & 0xff; + switch(_state) + { + case IBUS_LENGTH: + if(c == IBUS_FRAME_SIZE) + { + data[_idx++] = c; + _state = IBUS_CMD; + } + break; + case IBUS_CMD: + if(c == IBUS_COMMAND) + { + data[_idx++] = c; + _state = IBUS_DATA; + } + else + { + _state = IBUS_LENGTH; + } + break; + case IBUS_DATA: + data[_idx] = c; + if(++_idx >= IBUS_FRAME_SIZE - 2) + { + _state = IBUS_CRC_LO; + } + break; + case IBUS_CRC_LO: + data[_idx++] = c; + _state = IBUS_CRC_HI; + break; + case IBUS_CRC_HI: + data[_idx++] = c; + uint16_t csum = 0xffff; + for(size_t i = 0; i < IBUS_FRAME_SIZE - 2; i++) + { + csum -= data[i]; + } + if(frameData.checksum == csum) apply(frameData); + _state = IBUS_LENGTH; + _idx = 0; + break; + } +} + +void FAST_CODE_ATTR InputIBUS::apply(IBusData& data) +{ + for(size_t i = 0; i < CHANNELS; i++) + { + _channels[i] = data.ch[i]; + } + _new_data = true; +} + +uint16_t FAST_CODE_ATTR InputIBUS::get(uint8_t i) const +{ + return _channels[i]; +} + +void FAST_CODE_ATTR InputIBUS::get(uint16_t *data, size_t len) const +{ + const uint16_t *src = _channels; + while (len--) + { + *data++ = *src++; + } +} + +size_t InputIBUS::getChannelCount() const { return CHANNELS; } + +bool InputIBUS::needAverage() const { return false; } + +} \ No newline at end of file diff --git a/lib/Espfc/src/Device/InputIBUS.hpp b/lib/Espfc/src/Device/InputIBUS.hpp new file mode 100644 index 00000000..0fb123fb --- /dev/null +++ b/lib/Espfc/src/Device/InputIBUS.hpp @@ -0,0 +1,56 @@ +#pragma once + +#include "Device/SerialDevice.h" +#include "Device/InputDevice.h" + +namespace Espfc::Device +{ + +class InputIBUS : public InputDevice +{ +public: + struct IBusData + { + uint8_t len; + uint8_t cmd; + uint16_t ch[14]; + uint16_t checksum; + } __attribute__((__packed__)); + + InputIBUS(); + + int begin(Device::SerialDevice *serial); + InputStatus update() override; + uint16_t get(uint8_t i) const override; + void get(uint16_t *data, size_t len) const override; + size_t getChannelCount() const override; + bool needAverage() const override; + + void parse(IBusData& frame, int d); + +private: + enum IbusState + { + IBUS_LENGTH, + IBUS_CMD, + IBUS_DATA, + IBUS_CRC_LO, + IBUS_CRC_HI, + }; + + void apply(IBusData& frame); + + static constexpr size_t IBUS_FRAME_SIZE = sizeof(IBusData); + static constexpr uint8_t IBUS_COMMAND = 0x40; + static constexpr size_t CHANNELS = 14; + + Device::SerialDevice *_serial; + IbusState _state; + uint8_t _idx = 0; + bool _new_data; + + IBusData _data; + uint16_t _channels[CHANNELS]; +}; + +} diff --git a/lib/Espfc/src/Device/InputSBUS.cpp b/lib/Espfc/src/Device/InputSBUS.cpp index af87b5c7..cb932b48 100644 --- a/lib/Espfc/src/Device/InputSBUS.cpp +++ b/lib/Espfc/src/Device/InputSBUS.cpp @@ -27,7 +27,7 @@ InputStatus FAST_CODE_ATTR InputSBUS::update() if(len) { uint8_t buff[64] = {0}; - len = std::min(len, sizeof(buff)); + len = std::min(len, (size_t)sizeof(buff)); _serial->readMany(buff, len); size_t i = 0; while(i < len) diff --git a/lib/Espfc/src/Input.cpp b/lib/Espfc/src/Input.cpp index a1744894..8728338c 100644 --- a/lib/Espfc/src/Input.cpp +++ b/lib/Espfc/src/Input.cpp @@ -340,17 +340,25 @@ void FAST_CODE_ATTR Input::updateFrameRate() Device::InputDevice * Input::getInputDevice() { Device::SerialDevice * serial = _model.getSerialStream(SERIAL_FUNCTION_RX_SERIAL); - if(serial && _model.isFeatureActive(FEATURE_RX_SERIAL) && _model.config.input.serialRxProvider == SERIALRX_SBUS) + if(serial && _model.isFeatureActive(FEATURE_RX_SERIAL)) { - _sbus.begin(serial); - _model.logger.info().logln(F("RX SBUS")); - return &_sbus; - } - if(serial && _model.isFeatureActive(FEATURE_RX_SERIAL) && _model.config.input.serialRxProvider == SERIALRX_CRSF) - { - _crsf.begin(serial); - _model.logger.info().logln(F("RX CRSF")); - return &_crsf; + switch(_model.config.input.serialRxProvider) + { + case SERIALRX_IBUS: + _ibus.begin(serial); + _model.logger.info().logln(F("RX IBUS")); + return &_ibus; + + case SERIALRX_SBUS: + _sbus.begin(serial); + _model.logger.info().logln(F("RX SBUS")); + return &_sbus; + + case SERIALRX_CRSF: + _crsf.begin(serial); + _model.logger.info().logln(F("RX CRSF")); + return &_crsf; + } } else if(_model.isFeatureActive(FEATURE_RX_PPM) && _model.config.pin[PIN_INPUT_RX] != -1) { @@ -366,6 +374,7 @@ Device::InputDevice * Input::getInputDevice() return &_espnow; } #endif + return nullptr; } diff --git a/lib/Espfc/src/Input.h b/lib/Espfc/src/Input.h index 0a10d58c..1ed9f14c 100644 --- a/lib/Espfc/src/Input.h +++ b/lib/Espfc/src/Input.h @@ -4,6 +4,7 @@ #include "Math/Utils.h" #include "Device/InputDevice.h" #include "Device/InputPPM.h" +#include "Device/InputIBUS.hpp" #include "Device/InputSBUS.h" #include "Device/InputCRSF.h" #if defined(ESPFC_ESPNOW) @@ -59,6 +60,7 @@ class Input Filter _filter[INPUT_CHANNELS]; float _step; Device::InputPPM _ppm; + Device::InputIBUS _ibus; Device::InputSBUS _sbus; Device::InputCRSF _crsf; #if defined(ESPFC_ESPNOW) diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index eb50498b..dccd88b9 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -388,7 +388,7 @@ class Model } // configure serial ports - uint32_t serialFunctionAllowedMask = SERIAL_FUNCTION_MSP | SERIAL_FUNCTION_RX_SERIAL | SERIAL_FUNCTION_BLACKBOX | SERIAL_FUNCTION_TELEMETRY_FRSKY | SERIAL_FUNCTION_TELEMETRY_HOTT; + constexpr uint32_t serialFunctionAllowedMask = SERIAL_FUNCTION_MSP | SERIAL_FUNCTION_RX_SERIAL | SERIAL_FUNCTION_BLACKBOX | SERIAL_FUNCTION_TELEMETRY_FRSKY | SERIAL_FUNCTION_TELEMETRY_HOTT | SERIAL_FUNCTION_TELEMETRY_IBUS; uint32_t featureAllowMask = FEATURE_RX_SERIAL | FEATURE_RX_PPM | FEATURE_RX_SPI | FEATURE_SOFTSERIAL | FEATURE_MOTOR_STOP | FEATURE_TELEMETRY;// | FEATURE_AIRMODE; // allow dynamic filter only above 1k sampling rate diff --git a/lib/Espfc/src/Output/OutputIBUS.hpp b/lib/Espfc/src/Output/OutputIBUS.hpp new file mode 100644 index 00000000..9da49d9b --- /dev/null +++ b/lib/Espfc/src/Output/OutputIBUS.hpp @@ -0,0 +1,51 @@ +#pragma once + +#include "Device/SerialDevice.h" +#include "Utils/Timer.h" + +namespace Espfc::Output { + +class OutputIBUS +{ +public: + OutputIBUS() {} + + int begin(Device::SerialDevice* serial) + { + _serial = serial; + _timer.setInterval(7000); // 7ms + + return 1; + } + + int update() + { + if(!_timer.check()) return 0; + + // const uint8_t data[] = { + // 0x20, 0x40, // preambule (len, cmd) + // 0xDC, 0x05, 0xDC, 0x05, 0xBE, 0x05, 0xDC, 0x05, // channel 1-4 + // 0xD0, 0x07, 0xD0, 0x07, 0xDC, 0x05, 0xDC, 0x05, // channel 5-8 + // 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, // channel 9-12 + // 0xDC, 0x05, 0xDC, 0x05, // channel 13-14 + // 0x83, 0xF3 // checksum + // }; + + const uint8_t data[] = { + 0x20, 0x40, + 0xDB, 0x05, 0xDC, 0x05, 0x54, 0x05, 0xDC, 0x05, 0xE8, 0x03, 0xD0, 0x07, 0xD2, 0x05, 0xE8, 0x03, + 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, + 0xDA, 0xF3, + }; + + _serial->write(data, sizeof(data)); + + return 1; + } + +private: + Device::SerialDevice* _serial; + Utils::Timer _timer; +}; + +} diff --git a/lib/Espfc/src/SerialManager.cpp b/lib/Espfc/src/SerialManager.cpp index 744b9c8f..fe74d80e 100644 --- a/lib/Espfc/src/SerialManager.cpp +++ b/lib/Espfc/src/SerialManager.cpp @@ -81,11 +81,11 @@ int SerialManager::begin() sdc.stop_bits = SDC_SERIAL_STOP_BITS_2; sdc.inverted = true; break; + case SERIALRX_IBUS: + sdc.baud = 115200ul; + break; case SERIALRX_CRSF: sdc.baud = 420000ul; - //sdc.parity = SDC_SERIAL_PARITY_EVEN; - //sdc.stop_bits = SDC_SERIAL_STOP_BITS_2; - //sdc.inverted = true; break; default: break; @@ -99,6 +99,11 @@ int SerialManager::begin() sdc.stop_bits = SDC_SERIAL_STOP_BITS_2; } } + else if(spc.functionMask & SERIAL_FUNCTION_TELEMETRY_IBUS) + { + sdc.baud = 115200; + _ibus.begin(port); + } /*if(spc.functionMask & SERIAL_FUNCTION_TELEMETRY_FRSKY) { @@ -178,6 +183,11 @@ int FAST_CODE_ATTR SerialManager::update() { _telemetry.process(*stream); } + } + + if(sc.functionMask & SERIAL_FUNCTION_TELEMETRY_IBUS) + { + _ibus.update(); #ifdef ESPFC_SERIAL_SOFT_0_WIFI if(_current == SERIAL_SOFT_0) diff --git a/lib/Espfc/src/SerialManager.h b/lib/Espfc/src/SerialManager.h index d0e0721a..2e116236 100644 --- a/lib/Espfc/src/SerialManager.h +++ b/lib/Espfc/src/SerialManager.h @@ -2,6 +2,7 @@ #include "Model.h" #include "Device/SerialDevice.h" +#include "Output/OutputIBUS.hpp" #ifdef ESPFC_SERIAL_SOFT_0_WIFI #include "Wireless.h" #endif @@ -36,6 +37,7 @@ class SerialManager #endif Telemetry _telemetry; size_t _current; + Output::OutputIBUS _ibus; }; } diff --git a/test/test_input_crsf/test_input_crsf.cpp b/test/test_input_crsf/test_input_crsf.cpp index 6103b5a4..b0cf0b06 100644 --- a/test/test_input_crsf/test_input_crsf.cpp +++ b/test/test_input_crsf/test_input_crsf.cpp @@ -1,6 +1,7 @@ #include #include #include "Device/InputCRSF.h" +#include "Device/InputIBUS.hpp" #include #include #include @@ -27,7 +28,7 @@ void test_input_crsf_rc_valid() input.parse(frame, data[i]); } - for (size_t i; i < sizeof(data); i++) { + for (size_t i = 0; i < sizeof(data); i++) { TEST_ASSERT_EQUAL_UINT8(data[i], frame.data[i]); } @@ -290,6 +291,76 @@ void test_crsf_decode_rc_shift8() TEST_ASSERT_EQUAL_UINT16(1500, channels[15]); }*/ +void test_input_ibus_rc_valid() +{ + InputIBUS input; + InputIBUS::IBusData frame; + memset(&frame, 0, sizeof(frame)); + uint8_t * frame_data = reinterpret_cast(&frame); + + When(Method(ArduinoFake(), micros)).Return(0); + + input.begin(nullptr); + + // const uint8_t data[] = { + // 0x20, 0x40, // preambule (len, cmd) + // 0xDC, 0x05, 0xDC, 0x05, 0xBE, 0x05, 0xDC, 0x05, // channel 1-4 + // 0xD0, 0x07, 0xD0, 0x07, 0xDC, 0x05, 0xDC, 0x05, // channel 5-8 + // 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, // channel 9-12 + // 0xDC, 0x05, 0xDC, 0x05, // channel 13-14 + // 0x83, 0xF3 // checksum + // }; + + const uint8_t data[] = { + 0x20, 0x40, + 0xDB, 0x05, 0xDC, 0x05, 0x54, 0x05, 0xDC, 0x05, 0xE8, 0x03, 0xD0, 0x07, 0xD2, 0x05, 0xE8, 0x03, + 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, + 0xDA, 0xF3, + }; + for (size_t i = 0; i < sizeof(data); i++) { + input.parse(frame, data[i]); + } + + for (size_t i = 0; i < sizeof(data); i++) { + TEST_ASSERT_EQUAL_HEX8(data[i], frame_data[i]); + } + + TEST_ASSERT_EQUAL_HEX16(0xF3DA, frame.checksum); + + TEST_ASSERT_EQUAL_HEX8(0x20, frame.len); + TEST_ASSERT_EQUAL_HEX8(0x40, frame.cmd); + + TEST_ASSERT_EQUAL_UINT16(1499, frame.ch[0]); + TEST_ASSERT_EQUAL_UINT16(1500, frame.ch[1]); + TEST_ASSERT_EQUAL_UINT16(1364, frame.ch[2]); + TEST_ASSERT_EQUAL_UINT16(1500, frame.ch[3]); + TEST_ASSERT_EQUAL_UINT16(1000, frame.ch[4]); + TEST_ASSERT_EQUAL_UINT16(2000, frame.ch[5]); + TEST_ASSERT_EQUAL_UINT16(1490, frame.ch[6]); + TEST_ASSERT_EQUAL_UINT16(1000, frame.ch[7]); + TEST_ASSERT_EQUAL_UINT16(1500, frame.ch[8]); + TEST_ASSERT_EQUAL_UINT16(1500, frame.ch[9]); + TEST_ASSERT_EQUAL_UINT16(1500, frame.ch[10]); + TEST_ASSERT_EQUAL_UINT16(1500, frame.ch[11]); + TEST_ASSERT_EQUAL_UINT16(1500, frame.ch[12]); + TEST_ASSERT_EQUAL_UINT16(1500, frame.ch[13]); + + TEST_ASSERT_EQUAL_UINT16(1499, input.get(0)); + TEST_ASSERT_EQUAL_UINT16(1500, input.get(1)); + TEST_ASSERT_EQUAL_UINT16(1364, input.get(2)); + TEST_ASSERT_EQUAL_UINT16(1500, input.get(3)); + TEST_ASSERT_EQUAL_UINT16(1000, input.get(4)); + TEST_ASSERT_EQUAL_UINT16(2000, input.get(5)); + TEST_ASSERT_EQUAL_UINT16(1490, input.get(6)); + TEST_ASSERT_EQUAL_UINT16(1000, input.get(7)); + TEST_ASSERT_EQUAL_UINT16(1500, input.get(8)); + TEST_ASSERT_EQUAL_UINT16(1500, input.get(9)); + TEST_ASSERT_EQUAL_UINT16(1500, input.get(10)); + TEST_ASSERT_EQUAL_UINT16(1500, input.get(11)); + TEST_ASSERT_EQUAL_UINT16(1500, input.get(12)); + TEST_ASSERT_EQUAL_UINT16(1500, input.get(13)); +} + int main(int argc, char **argv) { UNITY_BEGIN(); @@ -299,7 +370,7 @@ int main(int argc, char **argv) RUN_TEST(test_crsf_decode_rc_struct); RUN_TEST(test_crsf_decode_rc_shift8); //RUN_TEST(test_crsf_decode_rc_shift32); - UNITY_END(); + RUN_TEST(test_input_ibus_rc_valid); - return 0; + return UNITY_END(); } \ No newline at end of file