esp-fc

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

namespace Espfc::Device::Input {

using namespace Espfc::Rc;

InputCRSF::InputCRSF(): _serial(nullptr), _telemetry(nullptr), _state(CRSF_ADDR), _idx(0), _new_data(false) {}

int InputCRSF::begin(Stream::ReadWritable* serial, TelemetryManager* telemetry)
{
  _serial = serial;
  _telemetry = telemetry;
  _telemetry_next = micros() + TELEMETRY_INTERVAL;
  std::fill_n((uint8_t*)&_frame, sizeof(_frame), 0);
  std::fill_n(_channels, CHANNELS, 0);
  return 1;
}

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

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

  if (_telemetry && micros() > _telemetry_next)
  {
    _telemetry_next = micros() + TELEMETRY_INTERVAL;
    _telemetry->process(*_serial, TELEMETRY_PROTOCOL_CRSF);
  }

  if (_new_data)
  {
    _new_data = false;
    return INPUT_RECEIVED;
  }

  return INPUT_IDLE;
}

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

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

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

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

void FAST_CODE_ATTR InputCRSF::parse(CrsfMessage& msg, int d)
{
  uint8_t* data = reinterpret_cast<uint8_t*>(&msg);
  uint8_t c = (uint8_t)(d & 0xff);
  switch (_state)
  {
    case CRSF_ADDR:
      if (c == CRSF_SYNC_BYTE)
      {
        data[_idx++] = c;
        _state = CRSF_SIZE;
      }
      break;
    case CRSF_SIZE:
      if (c >= 2 && c <= CRSF_FRAME_SIZE_MAX - 2) // allowed size is in range 2-62
      {
        data[_idx++] = c;
        _state = CRSF_TYPE;
      }
      else
      {
        reset();
      }
      break;
    case CRSF_TYPE:
      if (c == CRSF_FRAMETYPE_RC_CHANNELS_PACKED || c == CRSF_FRAMETYPE_LINK_STATISTICS ||
          c == CRSF_FRAMETYPE_MSP_REQ || c == CRSF_FRAMETYPE_MSP_WRITE)
      {
        data[_idx++] = c;
        if (msg.size > 2)
        {
          _state = CRSF_DATA;
        }
        else
        {
          _state = CRSF_CRC; // no payload, next byte is crc
        }
      }
      else
      {
        reset();
      }
      break;
    case CRSF_DATA:
      data[_idx++] = c;
      if (_idx > msg.size) // _idx is incremented here and operator > accounts as size - 2
      {
        _state = CRSF_CRC;
      }
      break;
    case CRSF_CRC:
      data[_idx++] = c;
      reset();
      uint8_t crc = msg.crc();
      if (c == crc)
      {
        apply(msg);
      }
      break;
  }
}

void FAST_CODE_ATTR InputCRSF::reset()
{
  _state = CRSF_ADDR;
  _idx = 0;
}

void FAST_CODE_ATTR InputCRSF::apply(const CrsfMessage& msg)
{
  switch (msg.type)
  {
    case CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
      applyChannels(msg);
      break;

    case CRSF_FRAMETYPE_LINK_STATISTICS:
      applyLinkStats(msg);
      break;

    case CRSF_FRAMETYPE_MSP_REQ:
    case CRSF_FRAMETYPE_MSP_WRITE:
      applyMspReq(msg);
      break;

    default:
      break;
  }
}

void FAST_CODE_ATTR InputCRSF::applyLinkStats(const CrsfMessage& msg)
{
  const auto* stats = reinterpret_cast<const CrsfLinkStats*>(msg.payload);
  (void)stats;
  // TODO:
}

void FAST_CODE_ATTR InputCRSF::applyChannels(const CrsfMessage& msg)
{
  const auto* data = reinterpret_cast<const CrsfData*>(msg.payload);
  Crsf::decodeRcDataShift8(_channels, data);
  // Crsf::decodeRcData(_channels, frame);
  _new_data = true;
}

void FAST_CODE_ATTR InputCRSF::applyMspReq(const CrsfMessage& frame)
{
  if (!_telemetry) return;

  uint8_t origin = 0;

  Crsf::decodeMsp(frame, _msg, origin);

  if (_msg.isCmd() && _msg.isReady())
  {
    _telemetry->processMsp(*_serial, TELEMETRY_PROTOCOL_CRSF, _msg, origin);
  }

  _telemetry_next = micros() + TELEMETRY_INTERVAL;
}

} // namespace Espfc::Device::Input