esp-fc

#include "Device/Input/InputSBUS.hpp"
#include "Hal/FastCode.hpp"
#include <algorithm>

namespace Espfc::Device::Input {

InputSBUS::InputSBUS(): _serial(nullptr), _state(SBUS_START), _idx(0), _new_data(false) {}

int InputSBUS::begin(Stream::ReadWritable* serial)
{
  _serial = serial;
  for (size_t i = 0; i < SBUS_FRAME_SIZE; i++)
  {
    _data[i] = 0;
    if (i < CHANNELS) _channels[i] = 0;
  }
  return 1;
}

InputStatus FAST_CODE_ATTR InputSBUS::update()
{
  if (!_serial) return INPUT_IDLE;

  size_t len = _serial->available();
  if (len)
  {
    uint8_t buff[64] = {0};
    len = std::min(len, (size_t)sizeof(buff));
    _serial->readMany(buff, len);
    size_t i = 0;
    while (i < len)
    {
      parse(buff[i++]);
    }
  }

  if (_new_data)
  {
    _new_data = false;
    if (_flags & SBUS_FLAG_FAILSAFE_ACTIVE) return INPUT_FAILSAFE;
    if (_flags & SBUS_FLAG_SIGNAL_LOSS) return INPUT_LOST;
    return INPUT_RECEIVED;
  }

  return INPUT_IDLE;
}

uint16_t FAST_CODE_ATTR InputSBUS::get(uint8_t i) const
{
  return _channels[i];
}

void FAST_CODE_ATTR InputSBUS::get(uint16_t* data, size_t len) const
{
  const uint16_t* src = _channels;
  while (len--)
  {
    *data++ = *src++;
  }
}

size_t InputSBUS::getChannelCount() const
{
  return CHANNELS;
}

bool InputSBUS::needAverage() const
{
  return false;
}

void FAST_CODE_ATTR InputSBUS::parse(int d)
{
  char c = (char)(d & 0xff);
  switch (_state)
  {
    case SBUS_START:
      if (c == 0x0F)
      {
        _data[_idx++] = c;
        _state = SBUS_DATA;
      }
      break;
    case SBUS_DATA:
      _data[_idx] = c;
      if (++_idx >= SBUS_FRAME_SIZE - 1)
      {
        _state = SBUS_END;
      }
      break;
    case SBUS_END:
      _data[_idx++] = c;
      _state = SBUS_START;
      _idx = 0;
      apply();
      break;
  }
}

void FAST_CODE_ATTR InputSBUS::apply()
{
  /*
  const SbusData* frame = reinterpret_cast<const SbusData*>(_data);
  _channels[0]  = convert(frame->chan0);
  _channels[1]  = convert(frame->chan1);
  _channels[2]  = convert(frame->chan2);
  _channels[3]  = convert(frame->chan3);
  _channels[4]  = convert(frame->chan4);
  _channels[5]  = convert(frame->chan5);
  _channels[6]  = convert(frame->chan6);
  _channels[7]  = convert(frame->chan7);
  _channels[8]  = convert(frame->chan8);
  _channels[9]  = convert(frame->chan9);
  _channels[10] = convert(frame->chan10);
  _channels[11] = convert(frame->chan11);
  _channels[12] = convert(frame->chan12);
  _channels[13] = convert(frame->chan13);
  _channels[14] = convert(frame->chan14);
  _channels[15] = convert(frame->chan15);
  _flags = frame->flags;
  */

  // shifting is more efficient
  _channels[0] = convert((_data[1] | _data[2] << 8) & 0x07FF);
  _channels[1] = convert((_data[2] >> 3 | _data[3] << 5) & 0x07FF);
  _channels[2] = convert((_data[3] >> 6 | _data[4] << 2 | _data[5] << 10) & 0x07FF);
  _channels[3] = convert((_data[5] >> 1 | _data[6] << 7) & 0x07FF);
  _channels[4] = convert((_data[6] >> 4 | _data[7] << 4) & 0x07FF);
  _channels[5] = convert((_data[7] >> 7 | _data[8] << 1 | _data[9] << 9) & 0x07FF);
  _channels[6] = convert((_data[9] >> 2 | _data[10] << 6) & 0x07FF);
  _channels[7] = convert((_data[10] >> 5 | _data[11] << 3) & 0x07FF);
  _channels[8] = convert((_data[12] | _data[13] << 8) & 0x07FF);
  _channels[9] = convert((_data[13] >> 3 | _data[14] << 5) & 0x07FF);
  _channels[10] = convert((_data[14] >> 6 | _data[15] << 2 | _data[16] << 10) & 0x07FF);
  _channels[11] = convert((_data[16] >> 1 | _data[17] << 7) & 0x07FF);
  _channels[12] = convert((_data[17] >> 4 | _data[18] << 4) & 0x07FF);
  _channels[13] = convert((_data[18] >> 7 | _data[19] << 1 | _data[20] << 9) & 0x07FF);
  _channels[14] = convert((_data[20] >> 2 | _data[21] << 6) & 0x07FF);
  _channels[15] = convert((_data[21] >> 5 | _data[22] << 3) & 0x07FF);
  _flags = _data[23];

  _new_data = true;
}

uint16_t FAST_CODE_ATTR InputSBUS::convert(int v)
{
  return std::clamp(((v * 5) / 8) + 880, 800, 2200);
}

} // namespace Espfc::Device::Input