IBUS RX support (cherry-pick)

rtlopez · Feb 4, 2025 · 987a1bb · 987a1bb
1 parent ef36cd9
commit 987a1bb
Show file tree

Hide file tree

Showing 10 changed files with 340 additions and 18 deletions.
diff --git a/lib/Espfc/src/Device/InputIBUS.cpp b/lib/Espfc/src/Device/InputIBUS.cpp
@@ -0,0 +1,121 @@
+#include "InputIBUS.hpp"
+#include "Utils/MemoryHelper.h"
+#include <algorithm>
+
+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<uint8_t*>(&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; }
+
+}
diff --git a/lib/Espfc/src/Device/InputIBUS.hpp 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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;
 };
 
 }