esp-fc

#include "Connect/MspProcessor.hpp"
#include "Hardware.h"
#include "EspProtocol.hpp"
#include <platform.h>
#include <algorithm>
#include <limits>
#if defined(ESPFC_MULTI_CORE) && defined(ESPFC_FREE_RTOS)
#include <driver/timer.h>
#endif

#define VTXCOMMON_MSP_BANDCHAN_CHKVAL ((uint16_t)((7 << 3) + 7))

extern "C" {
  #include "msp/msp_protocol.h"
  #include "msp/msp_protocol_v2_common.h"
  #include "msp/msp_protocol_v2_betaflight.h"
  #include "io/serial_4way.h"
  #include "blackbox/blackbox_io.h"
  int blackboxCalculatePDenom(int rateNum, int rateDenom);
  uint8_t blackboxCalculateSampleRate(uint16_t pRatio);
  uint8_t blackboxGetRateDenom(void);
  uint16_t blackboxGetPRatio(void);
}

namespace {

enum SerialSpeedIndex {
  SERIAL_SPEED_INDEX_AUTO = 0,
  SERIAL_SPEED_INDEX_9600,
  SERIAL_SPEED_INDEX_19200,
  SERIAL_SPEED_INDEX_38400,
  SERIAL_SPEED_INDEX_57600,
  SERIAL_SPEED_INDEX_115200,
  SERIAL_SPEED_INDEX_230400,
  SERIAL_SPEED_INDEX_250000,
  SERIAL_SPEED_INDEX_400000,
  SERIAL_SPEED_INDEX_460800,
  SERIAL_SPEED_INDEX_500000,
  SERIAL_SPEED_INDEX_921600,
  SERIAL_SPEED_INDEX_1000000,
  SERIAL_SPEED_INDEX_1500000,
  SERIAL_SPEED_INDEX_2000000,
  SERIAL_SPEED_INDEX_2470000,
};

static SerialSpeedIndex toBaudIndex(int32_t speed)
{
  using namespace Espfc;
  if(speed >= SERIAL_SPEED_2470000) return SERIAL_SPEED_INDEX_2470000;
  if(speed >= SERIAL_SPEED_2000000) return SERIAL_SPEED_INDEX_2000000;
  if(speed >= SERIAL_SPEED_1500000) return SERIAL_SPEED_INDEX_1500000;
  if(speed >= SERIAL_SPEED_1000000) return SERIAL_SPEED_INDEX_1000000;
  if(speed >= SERIAL_SPEED_921600)  return SERIAL_SPEED_INDEX_921600;
  if(speed >= SERIAL_SPEED_500000)  return SERIAL_SPEED_INDEX_500000;
  if(speed >= SERIAL_SPEED_460800)  return SERIAL_SPEED_INDEX_460800;
  if(speed >= SERIAL_SPEED_400000)  return SERIAL_SPEED_INDEX_400000;
  if(speed >= SERIAL_SPEED_250000)  return SERIAL_SPEED_INDEX_250000;
  if(speed >= SERIAL_SPEED_230400)  return SERIAL_SPEED_INDEX_230400;
  if(speed >= SERIAL_SPEED_115200)  return SERIAL_SPEED_INDEX_115200;
  if(speed >= SERIAL_SPEED_57600)   return SERIAL_SPEED_INDEX_57600;
  if(speed >= SERIAL_SPEED_38400)   return SERIAL_SPEED_INDEX_38400;
  if(speed >= SERIAL_SPEED_19200)   return SERIAL_SPEED_INDEX_19200;
  if(speed >= SERIAL_SPEED_9600)    return SERIAL_SPEED_INDEX_9600;
  return SERIAL_SPEED_INDEX_AUTO;
}

static Espfc::SerialSpeed fromBaudIndex(SerialSpeedIndex index)
{
  using namespace Espfc;
  switch(index)
  {
    case SERIAL_SPEED_INDEX_9600:    return SERIAL_SPEED_9600;
    case SERIAL_SPEED_INDEX_19200:   return SERIAL_SPEED_19200;
    case SERIAL_SPEED_INDEX_38400:   return SERIAL_SPEED_38400;
    case SERIAL_SPEED_INDEX_57600:   return SERIAL_SPEED_57600;
    case SERIAL_SPEED_INDEX_115200:  return SERIAL_SPEED_115200;
    case SERIAL_SPEED_INDEX_230400:  return SERIAL_SPEED_230400;
    case SERIAL_SPEED_INDEX_250000:  return SERIAL_SPEED_250000;
    case SERIAL_SPEED_INDEX_400000:  return SERIAL_SPEED_400000;
    case SERIAL_SPEED_INDEX_460800:  return SERIAL_SPEED_460800;
    case SERIAL_SPEED_INDEX_500000:  return SERIAL_SPEED_500000;
    case SERIAL_SPEED_INDEX_921600:  return SERIAL_SPEED_921600;
    case SERIAL_SPEED_INDEX_1000000: return SERIAL_SPEED_1000000;
    case SERIAL_SPEED_INDEX_1500000: return SERIAL_SPEED_1500000;
    case SERIAL_SPEED_INDEX_2000000: return SERIAL_SPEED_2000000;
    case SERIAL_SPEED_INDEX_2470000: return SERIAL_SPEED_2470000;
    case SERIAL_SPEED_INDEX_AUTO:
    default:
      return SERIAL_SPEED_NONE;
  }
}

static uint8_t toFilterTypeDerivative(uint8_t t)
{
  switch(t) {
    case 0: return Espfc::FILTER_NONE;
    case 1: return Espfc::FILTER_PT3;
    case 2: return Espfc::FILTER_BIQUAD;
    default: return Espfc::FILTER_PT3;
  }
}

static uint8_t fromFilterTypeDerivative(uint8_t t)
{
  switch(t) {
    case Espfc::FILTER_NONE: return 0;
    case Espfc::FILTER_PT3: return 1;
    case Espfc::FILTER_BIQUAD: return 2;
    default: return 1;
  }
}

static uint8_t fromGyroDlpf(uint8_t t)
{
  switch(t) {
    case Espfc::GYRO_DLPF_256: return 0;
    case Espfc::GYRO_DLPF_EX: return 1;
    default: return 2;
  }
}

static int8_t toVbatSource(uint8_t t)
{
  switch(t) {
    case 0: return 0; // none
    case 1: return 1; // internal adc
    default: return 0;
  }
}

static int8_t toIbatSource(uint8_t t)
{
  switch(t) {
    case 0: return 0; // none
    case 1: return 1; // internal adc
    default: return 0;
  }
}

static uint8_t toVbatVoltageLegacy(float voltage)
{
  return constrain(lrintf(voltage * 10.0f), 0, 255);
}

static uint16_t toVbatVoltage(float voltage)
{
  return constrain(lrintf(voltage * 100.0f), 0, 32000);
}

static uint16_t toIbatCurrent(float current)
{
  return constrain(lrintf(current * 100.0f), -32000, 32000);
}

constexpr uint8_t MSP_PASSTHROUGH_ESC_4WAY = 0xff;

}

namespace Espfc::Connect {

static EspCmdInputType toEspInputType(const Model& _model)
{
  if(_model.config.featureMask & FEATURE_RX_SERIAL)
  {
    if(_model.config.input.serialRxProvider == SERIALRX_IBUS) return RX_SERIAL_IBUS;
    else if(_model.config.input.serialRxProvider == SERIALRX_SBUS) return RX_SERIAL_SBUS;
    else if(_model.config.input.serialRxProvider == SERIALRX_CRSF) return RX_SERIAL_CRSF;
    return RX_NONE;
  }
  else if(_model.config.featureMask & FEATURE_RX_SPI)
  {
    return RX_ESPNOW;
  }
  else if(_model.config.featureMask & FEATURE_RX_PPM)
  {
    return RX_PPM;
  }
  return RX_NONE;
}

static void fromEspInputType(Model& _model, EspCmdInputType type)
{
  _model.config.featureMask &= ~(FEATURE_RX_SERIAL | FEATURE_RX_SPI | FEATURE_RX_PPM);
  switch(type)
  {
    case RX_SERIAL_IBUS:
      _model.config.featureMask |= FEATURE_RX_SERIAL;
      _model.config.input.serialRxProvider = SERIALRX_IBUS;
      break;
    case RX_SERIAL_SBUS:
      _model.config.featureMask |= FEATURE_RX_SERIAL;
      _model.config.input.serialRxProvider = SERIALRX_SBUS;
      break;
    case RX_SERIAL_CRSF:
      _model.config.featureMask |= FEATURE_RX_SERIAL;
      _model.config.input.serialRxProvider = SERIALRX_CRSF;
      break;
    case RX_ESPNOW:
      _model.config.featureMask |= FEATURE_RX_SPI;
      break;
    case RX_PPM:
      _model.config.featureMask |= FEATURE_RX_PPM;
      break;
    case RX_NONE:
    default:
      break;
  }
}

void updatePidValues(EspCmdPidTuning& tune)
{
  constexpr static PidConfig ref[3] = {
    [FC_PID_ROLL]  = { .P = 42, .I = 85, .D = 24, .F = 72 },  // ROLL
    [FC_PID_PITCH] = { .P = 46, .I = 90, .D = 26, .F = 76 },  // PITCH
    [FC_PID_YAW]   = { .P = 45, .I = 90, .D =  0, .F = 72 },  // YAW
  };

  float rpGain = 0.01f * tune.rpGain;
  float rpStability = 0.01f * tune.rpStability;
  float rpAgility = 0.01f * tune.rpAgility;
  float rpAgilityInv = 2.0f - rpAgility;
  float pBalance = 0.01f * tune.rpBalance;
  float rBalance = 2.0f - pBalance;
  float yawGain = 0.01f * tune.yawGain;
  float yawStability = 0.01f * tune.yawStability;


  tune.pids[FC_PID_ROLL].p = std::clamp(lrintf(ref[FC_PID_ROLL].P * rBalance * rpGain), 0l, 255l);
  tune.pids[FC_PID_ROLL].i = std::clamp(lrintf(ref[FC_PID_ROLL].I * rBalance * rpGain * rpStability), 0l, 255l);
  tune.pids[FC_PID_ROLL].d = std::clamp(lrintf(ref[FC_PID_ROLL].D * rBalance * rpGain * rpAgilityInv), 0l, 255l);
  tune.pids[FC_PID_ROLL].f = std::clamp(lrintf(ref[FC_PID_ROLL].F * rBalance * rpGain * rpAgility), 0l, 1000l);

  tune.pids[FC_PID_PITCH].p = std::clamp(lrintf(ref[FC_PID_PITCH].P * pBalance * rpGain), 0l, 255l);
  tune.pids[FC_PID_PITCH].i = std::clamp(lrintf(ref[FC_PID_PITCH].I * pBalance * rpGain * rpStability), 0l, 255l);
  tune.pids[FC_PID_PITCH].d = std::clamp(lrintf(ref[FC_PID_PITCH].D * pBalance * rpGain * rpAgilityInv), 0l, 255l);
  tune.pids[FC_PID_PITCH].f = std::clamp(lrintf(ref[FC_PID_PITCH].F * pBalance * rpGain * rpAgility), 0l, 1000l);

  tune.pids[FC_PID_YAW].p = std::clamp(lrintf(ref[FC_PID_YAW].P * yawGain), 0l, 255l);
  tune.pids[FC_PID_YAW].i = std::clamp(lrintf(ref[FC_PID_YAW].I * yawGain * yawStability), 0l, 255l);
  tune.pids[FC_PID_YAW].d = std::clamp(lrintf(ref[FC_PID_YAW].D * yawGain), 0l, 255l);
  tune.pids[FC_PID_YAW].f = std::clamp(lrintf(ref[FC_PID_YAW].F * yawGain), 0l, 1000l);
}

MspProcessor::MspProcessor(Model& model): _model(model) {}

bool MspProcessor::parse(char c, MspMessage& msg)
{
  _parser.parse(c, msg);

  if(msg.isReady()) debugMessage(msg);

  return !msg.isIdle();
}

void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialDevice& s)
{
  r.cmd = m.cmd;
  r.version = m.version;
  r.variant = m.variant;
  r.result = 1;
  switch(m.variant)
  {
    case MSP_BF:
      processCommandBF(m, r, s);
      break;
    case MSP_ESP:
      processCommandESP(m, r, s);
      break;
    default:
      r.cmd = m.cmd;
      r.version = m.version;
      r.variant = m.variant;
      r.result = -1; // unsupported variant
      break;
  }
}

void MspProcessor::processCommandESP(MspMessage& m, MspResponse& r, Device::SerialDevice& s)
{
  switch(m.cmd)
  {
    case ESP_CMD_VERSION:
      {
        EspCmdVersion version = {
          .apiMajor = ESP_API_VERSION_MAJOR,
          .apiMinor = ESP_API_VERSION_MINOR,
          .hwType = ESP_HW_TYPE_UNKNOWN,
          .capabilities = 0,
          .fwVersion = {0},
          .fwRevision = {0}
        };
        char ver[] = STR(ESPFC_VERSION);
        char rev[] = STR(ESPFC_REVISION);
        std::copy_n(ver, std::min(sizeof(version.fwVersion), sizeof(ver)), version.fwVersion);
        std::copy_n(rev, std::min(sizeof(version.fwRevision), sizeof(rev)), version.fwRevision);
        r.write(version);
      }
      break;

    case ESP_CMD_STATUS:
      {
        EspCmdStatus status = {
          .sensors = 0,
          .gyroTimeUs = (uint16_t)_model.state.gyro.timer.interval,
          .loopTimeUs = (uint16_t)_model.state.loopTimer.interval,
          .modeSwitchMask = _model.state.mode.maskSwitch,
          .modeActiveMask = _model.state.mode.mask,
          .armingDisableFlags = _model.state.mode.armingDisabledFlags,
        };
        if(_model.state.gyro.present) status.sensors |= 1 << 0;
        if(_model.state.accel.present) status.sensors |= 1 << 1;
        if(_model.state.baro.present) status.sensors |= 1 << 2;
        if(_model.state.mag.present) status.sensors |= 1 << 3;
        if(_model.state.gps.present) status.sensors |= 1 << 4;
        r.write(status);
      }
      break;

    case ESP_CMD_STATISTICS:
      {
        EspCmdStatistics stats = {
          .uptimeMs = static_cast<uint32_t>(millis()),
          .loopTimeUs = static_cast<uint16_t>(std::min((uint32_t)0xffff, _model.state.stats.loopTime())),
          .cpuLoad = (uint8_t)lrintf(_model.state.stats.getCpuLoad()),
          .cpu0Load = (uint8_t)lrintf(_model.state.stats.getLoad(COUNTER_CPU_0)),
          .cpu1Load = (uint8_t)lrintf(_model.state.stats.getLoad(COUNTER_CPU_1)),
          .heapTotal = targetTotalHeap(),
          .heapFree = targetFreeHeap(),
#ifdef USE_FLASHFS
          .flashTotal = flashfsGetSize(),
          .flashUsed = flashfsGetOffset()
#else
          .flashTotal = 0,
          .flashUsed = 0
#endif
        };
        r.write(stats);
      }
      break;

    case ESP_CMD_ATTITUDE:
      {
        EspCmdAttitude attitude = {
          .x = (int16_t)lrintf(_model.state.attitude.quaternion.x * 1000.0f),
          .y = (int16_t)lrintf(_model.state.attitude.quaternion.y * 1000.0f),
          .z = (int16_t)lrintf(_model.state.attitude.quaternion.z * 1000.0f),
          .w = (int16_t)lrintf(_model.state.attitude.quaternion.w * 1000.0f),
        };
        r.write(attitude);
      }
      break;

    case ESP_CMD_SENSORS:
      {
        EspCmdSensors sensors = {
          .gyro = {
            (int16_t)lrintf(_model.state.gyro.adc.x * 100.0f),
            (int16_t)lrintf(_model.state.gyro.adc.y * 100.0f),
            (int16_t)lrintf(_model.state.gyro.adc.z * 100.0f),
          },
          .accel = {
            (int16_t)lrintf(_model.state.accel.adc.x * 100.0f),
            (int16_t)lrintf(_model.state.accel.adc.y * 100.0f),
            (int16_t)lrintf(_model.state.accel.adc.z * 100.0f),
          },
          .mag = {
            (int16_t)lrintf(_model.state.mag.adc.x * 100.0f),
            (int16_t)lrintf(_model.state.mag.adc.y * 100.0f),
            (int16_t)lrintf(_model.state.mag.adc.z * 100.0f),
          },
          .baroAlt = (int16_t)lrintf(_model.state.baro.altitude * 100.0f),
        };
        r.write(sensors);
      }
      break;

    case ESP_CMD_INPUT:
      {
        EspCmdInput input = {
          .channelCount = (uint8_t)_model.state.input.channelCount,
          .channels = {0}
        };
        for(size_t i = 0; i < _model.state.input.channelCount; i++) input.channels[i] = _model.state.input.us[i];
        r.write(input);
      }
      break;

    case ESP_CMD_OUTPUT:
      {
        EspCmdOutput output = {
          .channelCount = OUTPUT_CHANNELS,
          .channels = {0}
        };
        for(size_t i = 0; i < OUTPUT_CHANNELS; i++) output.channels[i] = _model.state.output.us[i];
        r.write(output);
      }
      break;

    case ESP_CMD_VOLTAGE:
      {
        EspCmdVoltage voltage = {
          .count = 1,
          .items = {
            {
              .source = _model.config.vbat.source,
              .voltage = (uint16_t)std::clamp(lrintf(_model.state.battery.voltage * 100.0f), 0l, (long)UINT16_MAX),
              .cellCount = _model.state.battery.cells,
            }
          }
        };
        r.write(voltage);
      }
      break;

    case ESP_CMD_CURRENT:
      {
        EspCmdCurrent current = {
          .count = 1,
          .items = {
            {
              .source = _model.config.ibat.source,
              .current = (uint16_t)std::clamp(lrintf(_model.state.battery.current * 100.0f), 0l, (long)UINT16_MAX),
              .consumption = (uint32_t)lrintf(_model.state.battery.consumption),
            }
          }
        };
        r.write(current);
      }
      break;

    case ESP_CMD_GPS:
      {
        EspCmdGps gps = {
          .time = _model.state.gps.dateTime.toUnixTimestamp(),
          .fixType = _model.state.gps.fixType,
          .sats = _model.state.gps.numSats,
          .latitude = _model.state.gps.location.raw.lat,
          .longitude = _model.state.gps.location.raw.lon,
          .altitude = _model.state.gps.location.raw.height,
          .speed = _model.state.gps.velocity.raw.groundSpeed,
          .course = _model.state.gps.velocity.raw.heading,
          .pDop = _model.state.gps.accuracy.pDop,
          .hAccu = Utils::scast<uint16_t>(_model.state.gps.accuracy.horizontal / 10),
          .vAccu = Utils::scast<uint16_t>(_model.state.gps.accuracy.vertical / 10),
        };
        r.write(gps);
      }
      break;

    case ESP_CMD_GPS_INFO:
      {
        EspCmdGpsInfo info = {
          .numSats = _model.state.gps.numCh,
          .svs = {}
        };
        for(size_t i = 0; i < std::min((size_t)_model.state.gps.numCh, std::size(info.svs)); i++)
        {
          info.svs[i].gnssId = _model.state.gps.svinfo[i].gnssId;
          info.svs[i].id = _model.state.gps.svinfo[i].id;
          info.svs[i].quality = static_cast<uint8_t>(_model.state.gps.svinfo[i].quality.value & 0xff);
          info.svs[i].cno = _model.state.gps.svinfo[i].cno;
        }
        r.write(info);
      }
      break;

    case ESP_CMD_RPM_TLM:
      {
        EspCmdRpmTlm rpm = {
          .channelCount = 4,
          .channels = {0}
        };
        for(size_t i = 0; i < 4; i++)
        {
          rpm.channels[i].rpm = lrintf(_model.state.output.telemetry.rpm[i]);
          rpm.channels[i].errors = _model.state.output.telemetry.errors[i] * 2 / 100;
          rpm.channels[i].temperature = _model.state.output.telemetry.temperature[i];
          rpm.channels[i].voltage = _model.state.output.telemetry.voltage[i];
          rpm.channels[i].current = _model.state.output.telemetry.current[i];
        }
        r.write(rpm);
      }
      break;

    case ESP_CMD_PID_INNER:
      {
        const auto& inner = _model.state.innerPid;
        EspCmdPidInner res = {
          .roll = {
            .p = Utils::fcast<int16_t>(inner[AXIS_ROLL].pTerm * 1000.0f),
            .i = Utils::fcast<int16_t>(inner[AXIS_ROLL].iTerm * 1000.0f),
            .d = Utils::fcast<int16_t>(inner[AXIS_ROLL].dTerm * 1000.0f),
            .f = Utils::fcast<int16_t>(inner[AXIS_ROLL].fTerm * 1000.0f),
          },
          .pitch = {
            .p = Utils::fcast<int16_t>(inner[AXIS_PITCH].pTerm * 1000.0f),
            .i = Utils::fcast<int16_t>(inner[AXIS_PITCH].iTerm * 1000.0f),
            .d = Utils::fcast<int16_t>(inner[AXIS_PITCH].dTerm * 1000.0f),
            .f = Utils::fcast<int16_t>(inner[AXIS_PITCH].fTerm * 1000.0f),
          },
          .yaw = {
            .p = Utils::fcast<int16_t>(inner[AXIS_YAW].pTerm * 1000.0f),
            .i = Utils::fcast<int16_t>(inner[AXIS_YAW].iTerm * 1000.0f),
            .d = Utils::fcast<int16_t>(inner[AXIS_YAW].dTerm * 1000.0f),
            .f = Utils::fcast<int16_t>(inner[AXIS_YAW].fTerm * 1000.0f),
          },
          .alt = {
            .p = Utils::fcast<int16_t>(inner[AXIS_THRUST].pTerm * 1000.0f),
            .i = Utils::fcast<int16_t>(inner[AXIS_THRUST].iTerm * 1000.0f),
            .d = Utils::fcast<int16_t>(inner[AXIS_THRUST].dTerm * 1000.0f),
            .f = Utils::fcast<int16_t>(inner[AXIS_THRUST].fTerm * 1000.0f),
          }
        };
        r.write(res);
      }
      break;

    case ESP_CMD_PID_OUTER:
      {
        const auto& outer = _model.state.outerPid;
        EspCmdPidOuter res{
          .roll = {
            .p = Utils::fcast<int16_t>(outer[AXIS_ROLL].pTerm * 1000.0f),
            .i = Utils::fcast<int16_t>(outer[AXIS_ROLL].iTerm * 1000.0f),
            .d = Utils::fcast<int16_t>(outer[AXIS_ROLL].dTerm * 1000.0f),
            .f = Utils::fcast<int16_t>(outer[AXIS_ROLL].fTerm * 1000.0f),
          },
          .pitch = {
            .p = Utils::fcast<int16_t>(outer[AXIS_PITCH].pTerm * 1000.0f),
            .i = Utils::fcast<int16_t>(outer[AXIS_PITCH].iTerm * 1000.0f),
            .d = Utils::fcast<int16_t>(outer[AXIS_PITCH].dTerm * 1000.0f),
            .f = Utils::fcast<int16_t>(outer[AXIS_PITCH].fTerm * 1000.0f),
          },
        };
        r.write(res);
      }
      break;

    case ESP_CMD_DEBUG:
      {
        EspCmdDebug debug = {};
        for(size_t i = 0; i < DEBUG_VALUE_COUNT; i++) debug.debug[i] = _model.state.debug[i];
        r.write(debug);
      }
      break;

    case ESP_CMD_INPUT_CONFIG:
      {
        InputConfig& c = _model.config.input;
        if(m.received >= sizeof(EspCmdInputConfig))
        {
          fromEspInputType(_model, (EspCmdInputType)m.readU8());
          c.deadband = m.readU8();
          c.filterAutoFactor = m.readU8();
          c.midRc = m.readU16();
          c.minRc = m.readU16();
          c.maxRc = m.readU16();
        }
        EspCmdInputConfig res = {
          .type = toEspInputType(_model),
          .deadband = (uint8_t)c.deadband,
          .smoothing = (uint8_t)c.filterAutoFactor,
          .mid = c.midRc,
          .min = c.minRc,
          .max = c.maxRc,
        };
        r.write(res);
      }
      break;

    case ESP_CMD_INPUT_CHANNEL_CONFIG:
      {
        if(m.received >= sizeof(EspCmdInputChannelConfigResponse))
        {
          m.advance(1);
          for(size_t i = 0; i < INPUT_CHANNELS; i++)
          {
            InputChannelConfig& c = _model.config.input.channel[i];
            c.map = m.readU8();
            c.min = m.readU16();
            c.max = m.readU16();
            c.fsMode = m.readU8();
            c.fsValue = m.readU16();
          }
        }
        EspCmdInputChannelConfigResponse res;
        res.count = (uint8_t)_model.state.input.channelCount;
        for(size_t i = 0; i < INPUT_CHANNELS; i++)
        {
          InputChannelConfig& c = _model.config.input.channel[i];
          res.configs[i].map = c.map;
          res.configs[i].min = c.min;
          res.configs[i].max = c.max;
          res.configs[i].fsMode = c.fsMode;
          res.configs[i].fsValue = c.fsValue;
        };
        r.write(res);
      }
      break;

    case ESP_CMD_OUTPUT_CONFIG:
      {
        OutputConfig& c = _model.config.output;
        if(m.received >= sizeof(EspCmdOutputConfig))
        {
          c.protocol = m.readU8();
          c.async = m.readU8();
          c.rate = m.readU16();
          c.servoRate = m.readU16();
          c.minCommand = m.readU16();
          c.minThrottle = m.readU16();
          c.maxThrottle = m.readU16();
          c.dshotIdle = m.readU16();
          c.dshotTelemetry = m.readU8();
          c.motorPoles = m.readU8();
          c.motorLimit = m.readU8();
          c.throttleLimitType = m.readU8();
          c.throttleLimitPercent = m.readU8();
        }
        EspCmdOutputConfig res = {
          .protocol = (uint8_t)c.protocol,
          .async = (uint8_t)c.async,
          .rate = (uint16_t)c.rate,
          .servoRate = (uint16_t)c.servoRate,
          .minCommand = (uint16_t)c.minCommand,
          .minThrottle = (uint16_t)c.minThrottle,
          .maxThrottle = (uint16_t)c.maxThrottle,
          .digitalIdle = (uint16_t)c.dshotIdle,
          .digitalTlm = (uint8_t)c.dshotTelemetry,
          .motorPoles = (uint8_t)c.motorPoles,
          .motorLimit = (uint8_t)c.motorLimit,
          .throttleLimitType = (uint8_t)c.throttleLimitType,
          .throttleLimitPercent = (uint8_t)c.throttleLimitPercent,
        };
        r.write(res);
      }
      break;

      case ESP_CMD_OUTPUT_CHANNEL_CONFIG:
      {
        if(m.received >= sizeof(EspCmdOutputChannelConfigResponse))
        {
          m.advance(1);
          for(size_t i = 0; i < OUTPUT_CHANNELS; i++)
          {
            OutputChannelConfig& c = _model.config.output.channel[i];
            c.min = m.readU16();
            c.neutral = m.readU16();
            c.max = m.readU16();
            c.servo = m.readU8();
            c.reverse = m.readU8();
          }
        }
        EspCmdOutputChannelConfigResponse res;
        res.count = OUTPUT_CHANNELS;
        for(size_t i = 0; i < OUTPUT_CHANNELS; i++)
        {
          OutputChannelConfig& c = _model.config.output.channel[i];
          res.configs[i].min = c.min;
          res.configs[i].neutral = c.neutral;
          res.configs[i].max = c.max;
          res.configs[i].servo = c.servo;
          res.configs[i].reverse = c.reverse;
        };
        r.write(res);
      }
      break;

    case ESP_CMD_GYRO_CONFIG:
      {
        GyroConfig& c = _model.config.gyro;
        if(m.received >= sizeof(EspCmdGyroConfig))
        {
          c.align = m.readU8();
          c.filter.type = m.readU8();
          c.filter.freq = m.readU16();
          c.filter2.type = m.readU8();
          c.filter2.freq = m.readU16();
          c.filter3.type = m.readU8();
          c.filter3.freq = m.readU16();
          c.dynamicFilter.count = m.readU8();
          c.dynamicFilter.q = m.readU8() * 10;
          c.dynamicFilter.min_freq = m.readU16();
          c.dynamicFilter.max_freq = m.readU16();
          c.rpmFilter.harmonics = m.readU8();
          c.rpmFilter.q = m.readU8() * 10;
          c.rpmFilter.minFreq = m.readU16();
        }
        EspCmdGyroConfig res = {
          .align = (uint8_t)c.align,
          .lpf = {
            [0] = {
              .type = c.filter.type,
              .freq = c.filter.freq,
            },
            [1] = {
              .type = c.filter2.type,
              .freq = c.filter2.freq,
            },
            [2] = {
              .type = c.filter3.type,
              .freq = c.filter3.freq,
            }
          },
          .dynNotch = {
            .count = c.dynamicFilter.count,
            .q = (int8_t)((c.dynamicFilter.q + 5) / 10),
            .minFreq = c.dynamicFilter.min_freq,
            .maxFreq = c.dynamicFilter.max_freq,
          },
          .rpmNotch = {
            .harmonics = c.rpmFilter.harmonics,
            .q = (int8_t)((c.rpmFilter.q + 5) / 10),
            .minFreq = c.rpmFilter.minFreq,
          },
        };
        r.write(res);
      }
      break;

    case ESP_CMD_ACCEL_CONFIG:
      {
        AccelConfig& c = _model.config.accel;
        if(m.received >= sizeof(EspCmdAccelConfig))
        {
          c.filter.type = m.readU8();
          c.filter.freq = m.readU16();
        }
        EspCmdAccelConfig res = {
          .lpf = {
            .type = c.filter.type,
            .freq = c.filter.freq,
          }
        };
        r.write(res);
      }
      break;

     case ESP_CMD_BARO_CONFIG:
      {
        EspCmdBaroConfig res;
        BaroConfig& c = _model.config.baro;
        if(m.received >= sizeof(EspCmdBaroConfig))
        {
          m.readTo(res);
        }
        res = {
          .lpf = {
            .type = c.filter.type,
            .freq = c.filter.freq,
          }
        };
        r.write(res);
      }
      break;

    case ESP_CMD_MAG_CONFIG:
      {
        EspCmdMagConfig res;
        MagConfig& c = _model.config.mag;
        if(m.received >= sizeof(EspCmdMagConfig))
        {
          m.readTo(res);
        }
        res = {
          .align = c.align,
          .lpf = {
            .type = c.filter.type,
            .freq = c.filter.freq,
          }
        };
        r.write(res);
      }
      break;

    case ESP_CMD_PIN_CONFIG:
      {
        size_t count = m.received / 2;
        while(count--)
        {
          const uint8_t id = m.readU8();
          const uint8_t pin = (int8_t)m.readU8();
          const uint8_t index = id & ESP_PIN_IDX_MASK;
          switch(id & ESP_PIN_FN_MASK)
          {
#ifdef ESPFC_SERIAL_0
            case ESP_PIN_SERIAL:
              _model.config.pin[PIN_SERIAL_0_RX + index] = pin;
              break;
#endif
            case ESP_PIN_OUTPUT:
              _model.config.pin[PIN_OUTPUT_0 + index] = pin;
              break;
#ifdef ESPFC_INPUT
            case ESP_PIN_INPUT:
              _model.config.pin[PIN_INPUT_RX] = pin;
              break;
#endif
#ifdef ESPFC_I2C_0
            case ESP_PIN_I2C:
              _model.config.pin[PIN_I2C_0_SCL + index] = pin;
              break;
#endif
#ifdef ESPFC_SPI_0
            case ESP_PIN_SPI:
              _model.config.pin[PIN_SPI_0_SCK + index] = pin;
              break;
#endif
#ifdef ESPFC_ADC_VBAT
            case ESP_PIN_ADC:
              _model.config.pin[PIN_INPUT_ADC_VBAT + index] = pin;
              break;
#endif
            case ESP_PIN_BUTTON:
              _model.config.pin[PIN_BUTTON] = pin;
              break;
            case ESP_PIN_BUZZER:
              _model.config.pin[PIN_BUZZER] = pin;
              break;
            case ESP_PIN_LED:
              _model.config.pin[PIN_LED_BLINK] = pin;
              break;
            default:
              break;
          }
        }
        r.writeU8(ESP_PIN_OUTPUT | ESP_PIN_0);
        r.writeU8(_model.config.pin[PIN_OUTPUT_0]);
        r.writeU8(ESP_PIN_OUTPUT | ESP_PIN_1);
        r.writeU8(_model.config.pin[PIN_OUTPUT_1]);
        r.writeU8(ESP_PIN_OUTPUT | ESP_PIN_2);
        r.writeU8(_model.config.pin[PIN_OUTPUT_2]);
        r.writeU8(ESP_PIN_OUTPUT | ESP_PIN_3);
        r.writeU8(_model.config.pin[PIN_OUTPUT_3]);
#if ESPFC_OUTPUT_COUNT > 4
        r.writeU8(ESP_PIN_OUTPUT | ESP_PIN_4);
        r.writeU8(_model.config.pin[PIN_OUTPUT_4]);
#endif
#if ESPFC_OUTPUT_COUNT > 5
        r.writeU8(ESP_PIN_OUTPUT | ESP_PIN_5);
        r.writeU8(_model.config.pin[PIN_OUTPUT_5]);
#endif
#if ESPFC_OUTPUT_COUNT > 6
        r.writeU8(ESP_PIN_OUTPUT | ESP_PIN_6);
        r.writeU8(_model.config.pin[PIN_OUTPUT_6]);
#endif
#if ESPFC_OUTPUT_COUNT > 7
        r.writeU8(ESP_PIN_OUTPUT | ESP_PIN_7);
        r.writeU8(_model.config.pin[PIN_OUTPUT_7]);
#endif
#ifdef ESPFC_SERIAL_0
        r.writeU8(ESP_PIN_SERIAL | ESP_PIN_0);
        r.writeU8(_model.config.pin[PIN_SERIAL_0_RX]);
        r.writeU8(ESP_PIN_SERIAL | ESP_PIN_1);
        r.writeU8(_model.config.pin[PIN_SERIAL_0_TX]);
#endif
#ifdef ESPFC_SERIAL_1
        r.writeU8(ESP_PIN_SERIAL | ESP_PIN_2);
        r.writeU8(_model.config.pin[PIN_SERIAL_1_RX]);
        r.writeU8(ESP_PIN_SERIAL | ESP_PIN_3);
        r.writeU8(_model.config.pin[PIN_SERIAL_1_TX]);
#endif
#ifdef ESPFC_SERIAL_2
        r.writeU8(ESP_PIN_SERIAL | ESP_PIN_4);
        r.writeU8(_model.config.pin[PIN_SERIAL_2_RX]);
        r.writeU8(ESP_PIN_SERIAL | ESP_PIN_5);
        r.writeU8(_model.config.pin[PIN_SERIAL_2_TX]);
#endif
        r.writeU8(ESP_PIN_BUZZER | ESP_PIN_0);
        r.writeU8(_model.config.pin[PIN_BUZZER]);
        r.writeU8(ESP_PIN_LED | ESP_PIN_0);
        r.writeU8(_model.config.pin[PIN_LED_BLINK]);
#ifdef ESPFC_INPUT
        r.writeU8(ESP_PIN_INPUT | ESP_PIN_0);
        r.writeU8(_model.config.pin[PIN_INPUT_RX]);
#endif
        r.writeU8(ESP_PIN_BUTTON | ESP_PIN_0);
        r.writeU8(_model.config.pin[PIN_BUTTON]);
#ifdef ESPFC_ADC_VBAT
        r.writeU8(ESP_PIN_ADC | ESP_PIN_0);
        r.writeU8(_model.config.pin[PIN_INPUT_ADC_VBAT]);
#endif
#ifdef ESPFC_ADC_IBAT
        r.writeU8(ESP_PIN_ADC | ESP_PIN_1);
        r.writeU8(_model.config.pin[PIN_INPUT_ADC_IBAT]);
#endif
#ifdef ESPFC_I2C_0
        r.writeU8(ESP_PIN_I2C | ESP_PIN_SCL);
        r.writeU8(_model.config.pin[PIN_I2C_0_SCL]);
        r.writeU8(ESP_PIN_I2C | ESP_PIN_SDA);
        r.writeU8(_model.config.pin[PIN_I2C_0_SDA]);
#endif
#ifdef ESPFC_SPI_0
        r.writeU8(ESP_PIN_SPI | ESP_PIN_SCK);
        r.writeU8(_model.config.pin[PIN_SPI_0_SCK]);
        r.writeU8(ESP_PIN_SPI | ESP_PIN_MOSI);
        r.writeU8(_model.config.pin[PIN_SPI_0_MOSI]);
        r.writeU8(ESP_PIN_SPI | ESP_PIN_MISO);
        r.writeU8(_model.config.pin[PIN_SPI_0_MISO]);
        r.writeU8(ESP_PIN_SPI | ESP_PIN_CS0);
        r.writeU8(_model.config.pin[PIN_SPI_CS0]);
        r.writeU8(ESP_PIN_SPI | ESP_PIN_CS1);
        r.writeU8(_model.config.pin[PIN_SPI_CS1]);
        r.writeU8(ESP_PIN_SPI | ESP_PIN_CS2);
        r.writeU8(_model.config.pin[PIN_SPI_CS2]);
#endif
      }
      break;

    case ESP_CMD_MODE_NAMES:
      {
        r.writeU8(MODE_ARMED);
        r.writeString("ARM");
        r.writeU8(0);
        r.writeU8(MODE_ANGLE);
        r.writeString("ANGLE");
        r.writeU8(0);
        r.writeU8(MODE_AIRMODE);
        r.writeString("AIRMODE");
        r.writeU8(0);
        r.writeU8(MODE_BUZZER);
        r.writeString("BUZZER");
        r.writeU8(0);
        r.writeU8(MODE_FAILSAFE);
        r.writeString("FAILSAFE");
        r.writeU8(0);
        r.writeU8(MODE_BLACKBOX);
        r.writeString("BLACKBOX");
        r.writeU8(0);
        r.writeU8(MODE_BLACKBOX_ERASE);
        r.writeString("BLACKBOX_ERASE");
        r.writeU8(0);
      }
      break;

    case ESP_CMD_MODES_CONFIG:
      {
        if(m.received >= sizeof(EspCmdModesConfig))
        {
          m.advance(1);
          for(size_t i = 0; i < ACTUATOR_CONDITIONS; i++)
          {
            ActuatorCondition& c = _model.config.conditions[i];
            c.id = m.readU8();
            c.ch = m.readU8();
            c.min = m.readU16();
            c.max = m.readU16();
          }
        }
        EspCmdModesConfig res;
        res.modeCount = ACTUATOR_CONDITIONS;
        for(size_t i = 0; i < ACTUATOR_CONDITIONS; i++)
        {
          ActuatorCondition& c = _model.config.conditions[i];
          res.modes[i].id = c.id;
          res.modes[i].ch = c.ch;
          res.modes[i].min = c.min;
          res.modes[i].max = c.max;
        };
        r.write(res);
      }
      break;

    case ESP_CMD_FEATURE_NAMES:
      {
        r.writeU8(6);
        r.writeString("WIFI");
        r.writeU8(0);

        r.writeU8(7);
        r.writeString("GPS");
        r.writeU8(0);

        r.writeU8(10);
        r.writeString("TELEMETRY");
        r.writeU8(0);

        r.writeU8(22);
        r.writeString("AIRMODE");
        r.writeU8(0);

        r.writeU8(29);
        r.writeString("DYN-NOTCH");
        r.writeU8(0);
      }
      break;

    case ESP_CMD_FEATURE_CONFIG:
      {
        if(m.received >= sizeof(EspCmdFeatureConfig))
        {
          _model.config.featureMask = m.readU32();
        }
        EspCmdFeatureConfig res = {
          .features = (uint32_t)_model.config.featureMask,
        };
        r.write(res);
      }
      break;

    case ESP_CMD_SERIAL_NAMES:
      {
#ifdef ESPFC_SERIAL_USB
        r.writeU8(SERIAL_USB);
        r.writeString("USB");
        r.writeU8(0);
#endif
#ifdef ESPFC_SERIAL_0
        r.writeU8(SERIAL_UART_0);
        r.writeString("UART1");
        r.writeU8(0);
#endif
#ifdef ESPFC_SERIAL_1
        r.writeU8(SERIAL_UART_1);
        r.writeString("UART2");
        r.writeU8(0);
#endif
#ifdef ESPFC_SERIAL_2
        r.writeU8(SERIAL_UART_2);
        r.writeString("UART3");
        r.writeU8(0);
#endif
#ifdef ESPFC_SERIAL_SOFT_0
        r.writeU8(SERIAL_SOFT_0);
        r.writeString("WIFI");
        r.writeU8(0);
#endif
      }
      break;

    case ESP_CMD_SERIAL_CONFIG:
      {
        if(m.received >= sizeof(EspCmdSerialConfigResponse))
        {
          m.advance(1);
          for(size_t i = 0; i < SERIAL_UART_COUNT; i++)
          {
            SerialPortConfig& c = _model.config.serial[i];
            c.baud = m.readU32();
            c.functionMask = m.readU32();
          }
        }
        EspCmdSerialConfigResponse res = {
          .serialCount = SERIAL_UART_COUNT,
        };
        for(size_t i = 0; i < SERIAL_UART_COUNT; i++)
        {
          SerialPortConfig& c = _model.config.serial[i];
          res.serial[i].baud = c.baud;
          res.serial[i].func = c.functionMask;
        }
        r.write(res);
      }
      break;

    case ESP_CMD_VOLTAGE_CONFIG:
      {
        VBatConfig& c = _model.config.vbat;
        EspCmdVoltageConfig res;
        if(m.received >= sizeof(EspCmdVoltageConfig))
        {
          m.readTo(res);
          c.source = res.items[0].source;
          c.scale = res.items[0].scale;
          c.resDiv = 1; //res.items[0].resDiv;
          c.resMult = 1; //res.items[0].resMult;
          c.cellWarning = res.items[0].cellWarning;
        }
        res = {
          .count = 1,
          .items = {
            {
              .source = c.source,
              .scale = c.scale,
              .cellWarning = c.cellWarning,
            }
          }
        };
        r.write(res);
      }
      break;

    case ESP_CMD_CURRENT_CONFIG:
      {
        IBatConfig& c = _model.config.ibat;
        EspCmdCurrentConfig res;
        if(m.received >= sizeof(EspCmdCurrentConfig))
        {
          m.readTo(res);
          c.source = res.items[0].source;
          c.scale = res.items[0].scale;
          c.offset = res.items[0].offset;
        }
        res = {
          .count = 1,
          .items = {
            {
              .source = c.source,
              .scale = c.scale,
              .offset = c.offset,
            }
          }
        };
        r.write(res);
      }
      break;

    case ESP_CMD_SENSOR_CONFIG:
      {
        if (m.received >= sizeof(EspCmdSensorConfig))
        {
          _model.config.loopSync = m.readU8();
          _model.config.accel.dev = m.readU8();
          _model.config.baro.dev = m.readU8();
          _model.config.mag.dev = m.readU8();
          _model.config.boardAlignment[0] = m.readU16();
          _model.config.boardAlignment[1] = m.readU16();
          _model.config.boardAlignment[2] = m.readU16();
        }
        EspCmdSensorConfig ret = {
          .loopSync = (uint8_t)_model.config.loopSync,
          .accelDev = (uint8_t)_model.config.accel.dev,
          .baroDev = (uint8_t)_model.config.baro.dev,
          .magDev = (uint8_t)_model.config.mag.dev,
          .alignment = {
            [0] = _model.config.boardAlignment[0],
            [1] = _model.config.boardAlignment[1],
            [2] = _model.config.boardAlignment[2],
          }
        };
        r.write(ret);
      }
      break;

    case ESP_CMD_PID_NAMES:
      {
        r.writeU8(FC_PID_ROLL);
        r.writeString("Roll");
        r.writeU8(0);
        r.writeU8(FC_PID_PITCH);
        r.writeString("Pitch");
        r.writeU8(0);
        r.writeU8(FC_PID_YAW);
        r.writeString("Yaw");
        r.writeU8(0);
        r.writeU8(FC_PID_ALT);
        r.writeString("Alt Hold");
        r.writeU8(0);
        r.writeU8(FC_PID_POS);
        r.writeString("Pos");
        r.writeU8(0);
        r.writeU8(FC_PID_POSR);
        r.writeString("Pos Rate");
        r.writeU8(0);
        r.writeU8(FC_PID_NAVR);
        r.writeString("Nav Rate");
        r.writeU8(0);
        r.writeU8(FC_PID_LEVEL);
        r.writeString("Angle");
        r.writeU8(0);
        r.writeU8(FC_PID_MAG);
        r.writeString("Mag");
        r.writeU8(0);
        r.writeU8(FC_PID_VEL);
        r.writeString("Vel");
        r.writeU8(0);
      }
      break;

    case ESP_CMD_PID_CONFIG:
      {
        EspCmdPidConfigResponse res;
        if (m.received >= sizeof(EspCmdPidConfigResponse))
        {
          m.readTo(res);
          for (size_t i = 0; i < res.pidCount; i++)
          {
            const auto& pid = res.config[i];
            const size_t idx = pid.index;
            if (idx < FC_PID_ITEM_COUNT)
            {
              _model.config.pid[idx].P = pid.p;
              _model.config.pid[idx].I = pid.i;
              _model.config.pid[idx].D = pid.d;
              _model.config.pid[idx].F = pid.f;
            }
          }
        }
        const size_t count = std::size(res.config);
        res.pidCount = count;
        for (size_t i = 0; i < count; i++)
        {
          const size_t idx = i + FC_PID_ALT;
          auto& pid = res.config[i];
          pid.index = static_cast<uint8_t>(idx);
          pid.p = _model.config.pid[idx].P;
          pid.i = _model.config.pid[idx].I;
          pid.d = _model.config.pid[idx].D;
          pid.f = _model.config.pid[idx].F;
        }
        r.write(res);
      }
      break;

    case ESP_CMD_PID_TUNING:
      {
        EspCmdPidTuning res = { .mode = 0 };
        if (m.received >= sizeof(EspCmdPidTuning))
        {
          m.readTo(res);
          if(res.mode & ESP_CMD_PID_TUNING_MODE_CALC)
          {
            updatePidValues(res);
          }
          if(res.mode & ESP_CMD_PID_TUNING_MODE_SAVE)
          {
            res.mode &= ESP_CMD_PID_TUNING_MODE_CALC; // keep only calc flag
            _model.config.pidTuning.mode = res.mode;
            _model.config.pidTuning.rpGain = res.rpGain;
            _model.config.pidTuning.rpStability = res.rpStability;
            _model.config.pidTuning.rpAgility = res.rpAgility;
            _model.config.pidTuning.rpBalance = res.rpBalance;
            _model.config.pidTuning.yawGain = res.yawGain;
            _model.config.pidTuning.yawStability = res.yawStability;
            _model.config.pid[FC_PID_ROLL].P = res.pids[FC_PID_ROLL].p;
            _model.config.pid[FC_PID_ROLL].I = res.pids[FC_PID_ROLL].i;
            _model.config.pid[FC_PID_ROLL].D = res.pids[FC_PID_ROLL].d;
            _model.config.pid[FC_PID_ROLL].F = res.pids[FC_PID_ROLL].f;
            _model.config.pid[FC_PID_PITCH].P = res.pids[FC_PID_PITCH].p;
            _model.config.pid[FC_PID_PITCH].I = res.pids[FC_PID_PITCH].i;
            _model.config.pid[FC_PID_PITCH].D = res.pids[FC_PID_PITCH].d;
            _model.config.pid[FC_PID_PITCH].F = res.pids[FC_PID_PITCH].f;
            _model.config.pid[FC_PID_YAW].P = res.pids[FC_PID_YAW].p;
            _model.config.pid[FC_PID_YAW].I = res.pids[FC_PID_YAW].i;
            _model.config.pid[FC_PID_YAW].D = res.pids[FC_PID_YAW].d;
            _model.config.pid[FC_PID_YAW].F = res.pids[FC_PID_YAW].f;
          }
        }

        if(res.mode == 0)
        {
          res.mode = _model.config.pidTuning.mode;
          res.rpGain = _model.config.pidTuning.rpGain;
          res.rpStability = _model.config.pidTuning.rpStability;
          res.rpAgility = _model.config.pidTuning.rpAgility;
          res.rpBalance = _model.config.pidTuning.rpBalance;
          res.yawGain = _model.config.pidTuning.yawGain;
          res.yawStability = _model.config.pidTuning.yawStability;
          res.pids[FC_PID_ROLL].p = _model.config.pid[FC_PID_ROLL].P;
          res.pids[FC_PID_ROLL].i = _model.config.pid[FC_PID_ROLL].I;
          res.pids[FC_PID_ROLL].d = _model.config.pid[FC_PID_ROLL].D;
          res.pids[FC_PID_ROLL].f = _model.config.pid[FC_PID_ROLL].F;
          res.pids[FC_PID_PITCH].p = _model.config.pid[FC_PID_PITCH].P;
          res.pids[FC_PID_PITCH].i = _model.config.pid[FC_PID_PITCH].I;
          res.pids[FC_PID_PITCH].d = _model.config.pid[FC_PID_PITCH].D;
          res.pids[FC_PID_PITCH].f = _model.config.pid[FC_PID_PITCH].F;
          res.pids[FC_PID_YAW].p = _model.config.pid[FC_PID_YAW].P;
          res.pids[FC_PID_YAW].i = _model.config.pid[FC_PID_YAW].I;
          res.pids[FC_PID_YAW].d = _model.config.pid[FC_PID_YAW].D;
          res.pids[FC_PID_YAW].f = _model.config.pid[FC_PID_YAW].F;
        }
        r.write(res);
      }
      break;

    case ESP_CMD_DEBUG_NAMES:
      {
        r.writeU8(DEBUG_NONE); r.writeString("NONE"); r.writeU8(0);
        r.writeU8(DEBUG_ACCELEROMETER); r.writeString("ACCELEROMETER"); r.writeU8(0);
        r.writeU8(DEBUG_ALTITUDE); r.writeString("ALTITUDE"); r.writeU8(0);
        r.writeU8(DEBUG_ANGLERATE); r.writeString("ANGLERATE"); r.writeU8(0);
        r.writeU8(DEBUG_BARO); r.writeString("BAROMETER"); r.writeU8(0);
        r.writeU8(DEBUG_BATTERY); r.writeString("BATTERY"); r.writeU8(0);
        r.writeU8(DEBUG_CYCLETIME); r.writeString("CYCLETIME"); r.writeU8(0);
        r.writeU8(DEBUG_CURRENT_SENSOR); r.writeString("CURRENT_SENSOR"); r.writeU8(0);
        r.writeU8(DEBUG_DSHOT_RPM_ERRORS); r.writeString("DSHOT_RPM_ERRORS"); r.writeU8(0);
        r.writeU8(DEBUG_FFT_FREQ); r.writeString("FFT_FREQUENCY"); r.writeU8(0);
        r.writeU8(DEBUG_FFT_TIME); r.writeString("FFT_TIME"); r.writeU8(0);
        r.writeU8(DEBUG_PIDLOOP); r.writeString("PIDLOOP"); r.writeU8(0);
        r.writeU8(DEBUG_RX_TIMING); r.writeString("RX_TIMING"); r.writeU8(0);
        r.writeU8(DEBUG_RC_SMOOTHING); r.writeString("RC_SMOOTHING_RATE"); r.writeU8(0);
      }
      break;

    case ESP_CMD_BLACKBOX_NAMES:
      {
        r.writeU8(BLACKBOX_FIELD_PID); r.writeString("PID"); r.writeU8(0);
        r.writeU8(BLACKBOX_FIELD_RC_COMMANDS); r.writeString("RC_COMMAND"); r.writeU8(0);
        r.writeU8(BLACKBOX_FIELD_SETPOINT); r.writeString("SETPOINT"); r.writeU8(0);
        r.writeU8(BLACKBOX_FIELD_BATTERY); r.writeString("BATTERY"); r.writeU8(0);
        r.writeU8(BLACKBOX_FIELD_MAG); r.writeString("MAGNETOMETER"); r.writeU8(0);
        r.writeU8(BLACKBOX_FIELD_ALTITUDE); r.writeString("ALTITUDE"); r.writeU8(0);
        r.writeU8(BLACKBOX_FIELD_RSSI); r.writeString("RSSI"); r.writeU8(0);
        r.writeU8(BLACKBOX_FIELD_GYRO); r.writeString("GYRO"); r.writeU8(0);
        r.writeU8(BLACKBOX_FIELD_ACC); r.writeString("ACCELEROMETER"); r.writeU8(0);
        r.writeU8(BLACKBOX_FIELD_DEBUG_LOG); r.writeString("DEBUG"); r.writeU8(0);
        r.writeU8(BLACKBOX_FIELD_MOTOR); r.writeString("MOTOR"); r.writeU8(0);
        r.writeU8(BLACKBOX_FIELD_GPS); r.writeString("GPS"); r.writeU8(0);
        r.writeU8(BLACKBOX_FIELD_RPM); r.writeString("RPM"); r.writeU8(0);
        r.writeU8(BLACKBOX_FIELD_GYROUNFILT); r.writeString("GYRO_RAW"); r.writeU8(0);
      }
      break;

    case ESP_CMD_BLACKBOX_CONFIG:
      {
        BlackboxConfig& c = _model.config.blackbox;
        DebugConfig& d = _model.config.debug;
        if(m.received >= sizeof(EspCmdBlackboxConfig))
        {
          c.dev = m.readU8();
          c.pDenom = m.readU8();
          c.mode = m.readU8();
          c.fieldsMask = m.readU32();
          d.mode = m.readU8();
          d.axis = m.readU8();
        }
        EspCmdBlackboxConfig res = {
          .device = c.dev,
          .denom = (uint8_t)c.pDenom,
          .mode = c.mode,
          .fieldMask = c.fieldsMask,
          .debugMode = d.mode,
          .debugAxis = d.axis,
        };
        r.write(res);
      }
      break;

    case ESP_CMD_CALIBRATE:
      {
        uint8_t result = 0;
        if(m.received > 0)
        {
          const uint8_t mode = m.readU8();
          if(!_model.isModeActive(MODE_ARMED))
          {
            if(mode == 1) _model.calibrateGyro();
            if(mode == 2) _model.calibrateMag();
            result = mode;
          }
        }
        r.writeU8(result);
      }
      break;

    case ESP_CMD_MIXER_NAMES:
      {
        r.writeU8(FC_MIXER_QUADX); r.writeString("Quad X"); r.writeU8(0);
        r.writeU8(FC_MIXER_TRI); r.writeString("Tricopter"); r.writeU8(0);
        r.writeU8(FC_MIXER_CUSTOM); r.writeString("Custom"); r.writeU8(0);
      }
      break;

    case ESP_CMD_MIXER_CONFIG:
      {
        if (m.received >= sizeof(EspCmdMixerConfig))
        {
          _model.config.mixer.type = m.readU8();
          _model.config.mixer.yawReverse = m.readU8();
          _model.config.mixerSync = m.readU8();
        }
        EspCmdMixerConfig ret = {
          .type = (uint8_t)_model.config.mixer.type,
          .yawReverse = (uint8_t)_model.config.mixer.yawReverse,
          .sync = (uint8_t)_model.config.mixerSync,
        };
        r.write(ret);
      }
      break;

    case ESP_CMD_FLASH_LOGS:
      {
#ifdef USE_FLASHFS
        EspCmdFlashLogs res = {
          .totalSize = flashfsGetSize(),
          .usedSize = flashfsGetOffset(),
        };
        const FlashfsRuntime* flashfs = flashfsGetRuntime();
        for (size_t i = 0; i < 16; ++i)
        {
          const auto& it = flashfs->journal[i];
          res.logs[i].address = it.logBegin;
          res.logs[i].size = it.logEnd > it.logBegin ? it.logEnd - it.logBegin : 0;
        }
        r.write(res);
#endif
      }
      break;

    case ESP_CMD_FLASH_READ:
      {
#ifdef USE_FLASHFS
        EspCmdFlashReadRequest req;
        if (m.received >= sizeof(EspCmdFlashReadRequest))
        {
          // load request data
          m.readTo(req);

          // calc allowed read length
          const uint32_t allowedToRead = r.remain() - sizeof(EspCmdFlashReadResponse) - 16;
          const uint32_t flashLeft = flashfsGetSize() - req.address;
          const auto readLen = std::min(std::min((uint32_t)req.size, allowedToRead), flashLeft);

          // calc write pointers
          auto resPtr = reinterpret_cast<uint8_t*>(&r.data[r.len]); 
          r.advance(sizeof(EspCmdFlashReadResponse));
          auto dstPtr = reinterpret_cast<uint8_t*>(&r.data[r.len]); 

          // init response header
          EspCmdFlashReadResponse res;
          res.address = req.address;
          res.flags = 0;

          // read flash data
          res.size = flashfsReadAbs(req.address, dstPtr, readLen);

          // fill response headers
          std::memcpy(resPtr, &res, sizeof(res));

          // advance read data size in response
          r.advance(res.size);
        }
#endif
      }
      break;

    case ESP_CMD_FLASH_ERASE:
      {
#ifdef USE_FLASHFS
        blackboxEraseAll();
        r.write(1);
#endif
      }
      break;

    case ESP_CMD_OUTPUT_OVERRIDE:
      {
        EspCmdOutputOverride req;
        if (m.received >= sizeof(EspCmdOutputOverride))
        {
          m.readTo(req);
          for (size_t i = 0; i < OUTPUT_CHANNELS; i++)
          {
            _model.state.output.disarmed[i] = req.overrides[i];
          }
          _model.state.output.overrideTimeout = millis() + 2000;
        }
        req.count = OUTPUT_CHANNELS;
        for (size_t i = 0; i < OUTPUT_CHANNELS; i++)
        {
          req.overrides[i] = (uint16_t)_model.state.output.disarmed[i];
        }
        r.write(req);
      }
      break;

    case ESP_CMD_DISABLE_ARM:
      {
        const uint8_t cmd = m.readU8();
        _model.setArmingDisabled(ARMING_DISABLED_MSP, cmd);
        if (_model.isModeActive(MODE_ARMED)) _model.disarm(DISARM_REASON_ARMING_DISABLED);
      }
      break;

    case ESP_CMD_DEFAULTS:
      if(!_model.isModeActive(MODE_ARMED))
      {
        _model.reset();
        _model.save();
      }
      break;

    case ESP_CMD_SAVE:
      {
        _model.save();
      }
      break;

    case ESP_CMD_REBOOT:
      {
        r.writeU8(0); // reboot to firmware
        _postCommand = std::bind(&MspProcessor::processRestart, this);
      }
      break;

    default:
      r.result = -1; // unsupported command
      break;
  }
}

void MspProcessor::processCommandBF(MspMessage& m, MspResponse& r, Device::SerialDevice& s)
{
  switch(m.cmd)
  {
    case MSP_API_VERSION:
      r.writeU8(MSP_PROTOCOL_VERSION);
      r.writeU8(API_VERSION_MAJOR);
      r.writeU8(API_VERSION_MINOR);
      r.writeU8(0xff);
      break;

    case MSP_FC_VARIANT:
      r.writeData(flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
      break;

    case MSP_FC_VERSION:
      r.writeU8(FC_VERSION_MAJOR);
      r.writeU8(FC_VERSION_MINOR);
      r.writeU8(FC_VERSION_PATCH_LEVEL);
      break;

    case MSP_BOARD_INFO:
      r.writeData(boardIdentifier, BOARD_IDENTIFIER_LENGTH);
      r.writeU16(0); // No other build targets currently have hardware revision detection.
      r.writeU8(0);  // 0 == FC
      r.writeU8(0);  // target capabilities
      r.writeU8(strlen(targetName));  // target name
      r.writeData(targetName, strlen(targetName));
      r.writeU8(0);  // board name
      r.writeU8(0);  // manufacturer name
      for(size_t i = 0; i < 32; i++) r.writeU8(0); // signature
      r.writeU8(255); // mcu id: unknown
      // 1.42
      r.writeU8(2);  // configuration state: configured
      // 1.43
      r.writeU16(_model.state.gyro.present ? _model.state.gyro.timer.rate : 0); // sample rate
      {
        uint32_t problems = 0;
        if(_model.state.accel.bias.x == 0 && _model.state.accel.bias.y == 0 && _model.state.accel.bias.z == 0) {
          problems |= 1 << 0; // acc calibration required
        }
        if(_model.config.output.protocol == ESC_PROTOCOL_DISABLED) {
          problems |= 1 << 1; // no motor protocol
        }
        r.writeU32(problems); // configuration problems
      }
      // 1.44
      r.writeU8(0);  // spi dev count
      r.writeU8(0);  // i2c dev count
      break;

    case MSP_BUILD_INFO:
      r.writeData(buildDate, BUILD_DATE_LENGTH);
      r.writeData(buildTime, BUILD_TIME_LENGTH);
      r.writeData(shortGitRevision, GIT_SHORT_REVISION_LENGTH);
      break;

    case MSP_UID:
      r.writeU32(getBoardId0());
      r.writeU32(getBoardId1());
      r.writeU32(getBoardId2());
      break;

    case MSP_STATUS_EX:
    case MSP_STATUS:
      //r.writeU16(_model.state.loopTimer.delta);
      r.writeU16(_model.state.stats.loopTime());
      r.writeU16(_model.state.i2cErrorCount); // i2c error count
      //         acc,     baro,    mag,     gps,     sonar,   gyro
      r.writeU16(_model.accelActive() | _model.baroActive() << 1 | _model.magActive() << 2 | _model.gpsActive() << 3 | 0 << 4 | _model.gyroActive() << 5);
      r.writeU32(_model.state.mode.mask); // flight mode flags
      r.writeU8(0); // pid profile
      r.writeU16(lrintf(_model.state.stats.getCpuLoad()));
      if (m.cmd == MSP_STATUS_EX) {
        r.writeU8(1); // max profile count
        r.writeU8(0); // current rate profile index
      } else {  // MSP_STATUS
        //r.writeU16(_model.state.gyro.timer.interval); // gyro cycle time
        r.writeU16(0);
      }

      // flight mode flags (above 32 bits)
      r.writeU8(0); // count

      // Write arming disable flags
      r.writeU8(ARMING_DISABLED_FLAGS_COUNT);  // 1 byte, flag count
      r.writeU32(_model.state.mode.armingDisabledFlags);  // 4 bytes, flags
      r.writeU8(0); // reboot required
      break;

    case MSP_NAME:
      r.writeString(_model.config.modelName);
      break;

    case MSP_SET_NAME:
      memset(&_model.config.modelName, 0, MODEL_NAME_LEN + 1);
      for(size_t i = 0; i < std::min((size_t)m.received, MODEL_NAME_LEN); i++)
      {
        _model.config.modelName[i] = m.readU8();
      }
      break;

    case MSP_BOXNAMES:
      r.writeString(F("ARM;AIRMODE;ANGLE;ALTHOLD;BEEPER;FAILSAFE;BLACKBOX;BLACKBOXERASE;"));
      break;

    case MSP_BOXIDS:
      r.writeU8(MODE_ARMED);
      r.writeU8(MODE_AIRMODE);
      r.writeU8(MODE_ANGLE);
      r.writeU8(MODE_ALTHOLD);
      r.writeU8(MODE_BUZZER);
      r.writeU8(MODE_FAILSAFE);
      r.writeU8(MODE_BLACKBOX);
      r.writeU8(MODE_BLACKBOX_ERASE);
      break;

    case MSP_MODE_RANGES:
      for(size_t i = 0; i < ACTUATOR_CONDITIONS; i++)
      {
        r.writeU8(_model.config.conditions[i].id);
        r.writeU8(_model.config.conditions[i].ch - AXIS_AUX_1);
        r.writeU8((_model.config.conditions[i].min - 900) / 25);
        r.writeU8((_model.config.conditions[i].max - 900) / 25);
      }
      break;

    case MSP_MODE_RANGES_EXTRA:
      r.writeU8(ACTUATOR_CONDITIONS);
      for(size_t i = 0; i < ACTUATOR_CONDITIONS; i++)
      {
        r.writeU8(_model.config.conditions[i].id);
        r.writeU8(_model.config.conditions[i].logicMode);
        r.writeU8(_model.config.conditions[i].linkId);
      }

      break;

    case MSP_SET_MODE_RANGE:
      {
        size_t i = m.readU8();
        if(i < ACTUATOR_CONDITIONS)
        {
          _model.config.conditions[i].id = m.readU8();
          _model.config.conditions[i].ch = m.readU8() + AXIS_AUX_1;
          _model.config.conditions[i].min = m.readU8() * 25 + 900;
          _model.config.conditions[i].max = m.readU8() * 25 + 900;
          if(m.remain() >= 2) {
            _model.config.conditions[i].logicMode = m.readU8(); // mode logic
            _model.config.conditions[i].linkId = m.readU8(); // link to
          }
        }
        else
        {
          r.result = -1;
        }
      }
      break;

    case MSP_ANALOG:
      r.writeU8(toVbatVoltageLegacy(_model.state.battery.voltage));  // voltage in 0.1V
      r.writeU16(0); // mah drawn
      r.writeU16(_model.getRssi()); // rssi
      r.writeU16(toIbatCurrent(_model.state.battery.current));  // amperage in 0.01A
      r.writeU16(toVbatVoltage(_model.state.battery.voltage));  // voltage in 0.01V
      break;

    case MSP_FEATURE_CONFIG:
      r.writeU32(_model.config.featureMask);
      break;

    case MSP_SET_FEATURE_CONFIG:
      _model.config.featureMask = m.readU32();
      _model.reload();
      break;

    case MSP_BATTERY_CONFIG:
      r.writeU8(34);  // vbatmincellvoltage
      r.writeU8(42);  // vbatmaxcellvoltage
      r.writeU8((_model.config.vbat.cellWarning + 5) / 10);  // vbatwarningcellvoltage
      r.writeU16(0); // batteryCapacity
      r.writeU8(_model.config.vbat.source);  // voltageMeterSource
      r.writeU8(_model.config.ibat.source);  // currentMeterSource
      r.writeU16(340); // vbatmincellvoltage
      r.writeU16(420); // vbatmaxcellvoltage
      r.writeU16(_model.config.vbat.cellWarning); // vbatwarningcellvoltage
      break;

    case MSP_SET_BATTERY_CONFIG:
      m.readU8();  // vbatmincellvoltage
      m.readU8();  // vbatmaxcellvoltage
      _model.config.vbat.cellWarning = m.readU8() * 10;  // vbatwarningcellvoltage
      m.readU16(); // batteryCapacity
      _model.config.vbat.source = toVbatSource(m.readU8());  // voltageMeterSource
      _model.config.ibat.source = toIbatSource(m.readU8());  // currentMeterSource
      if(m.remain() >= 6)
      {
        m.readU16(); // vbatmincellvoltage
        m.readU16(); // vbatmaxcellvoltage
        _model.config.vbat.cellWarning = m.readU16();
      }
      break;

    case MSP_BATTERY_STATE:
      // battery characteristics
      r.writeU8(_model.state.battery.cells); // cell count, 0 indicates battery not detected.
      r.writeU16(0); // capacity in mAh

      // battery state
      r.writeU8(toVbatVoltageLegacy(_model.state.battery.voltage)); // in 0.1V steps
      r.writeU16(0); // milliamp hours drawn from battery
      r.writeU16(toIbatCurrent(_model.state.battery.current)); // send current in 0.01 A steps, range is -320A to 320A

      // battery alerts
      r.writeU8(0);
      r.writeU16(toVbatVoltage(_model.state.battery.voltage)); // in 0.01 steps
      break;

    case MSP_VOLTAGE_METERS:
      for(int i = 0; i < 1; i++)
      {
        r.writeU8(i + 10);  // meter id (10-19 vbat adc)
        r.writeU8(toVbatVoltageLegacy(_model.state.battery.voltage));  // meter value
      }
      break;

    case MSP_CURRENT_METERS:
      for(int i = 0; i < 1; i++)
      {
        r.writeU8(i + 10);  // meter id (10-19 ibat adc)
        r.writeU16(0); // mah drawn
        r.writeU16(constrain(toIbatCurrent(_model.state.battery.current) * 10, 0, 0xffff));  // meter value
      }
      break;

    case MSP_VOLTAGE_METER_CONFIG:
      r.writeU8(1); // num voltage sensors
      for(int i = 0; i < 1; i++)
      {
        r.writeU8(5); // frame size (5)
        r.writeU8(i + 10); // id (10-19 vbat adc)
        r.writeU8(0); // type resistor divider
        r.writeU8(_model.config.vbat.scale); // scale
        r.writeU8(_model.config.vbat.resDiv);  // resdivval
        r.writeU8(_model.config.vbat.resMult);  // resdivmultiplier
      }
      break;

    case MSP_SET_VOLTAGE_METER_CONFIG:
      {
        int id = m.readU8();
        if(id == 10 + 0) // id (10-19 vbat adc, allow only 10)
        {
          _model.config.vbat.scale = m.readU8();
          _model.config.vbat.resDiv = m.readU8();
          _model.config.vbat.resMult = m.readU8();
        }
      }
      break;

    case MSP_CURRENT_METER_CONFIG:
      r.writeU8(1); // num voltage sensors
      for(int i = 0; i < 1; i++)
      {
        r.writeU8(6); // frame size (6)
        r.writeU8(i + 10); // id (10-19 ibat adc)
        r.writeU8(1); // type adc
        r.writeU16(_model.config.ibat.scale); // scale
        r.writeU16(_model.config.ibat.offset);  // offset
      }
      break;

    case MSP_SET_CURRENT_METER_CONFIG:
      {
        int id = m.readU8();
        if(id == 10 + 0) // id (10-19 ibat adc, allow only 10)
        {
          _model.config.ibat.scale = m.readU16();
          _model.config.ibat.offset = m.readU16();
        }
      }
      break;

    case MSP_DATAFLASH_SUMMARY:
#ifdef USE_FLASHFS
      {
        uint8_t flags = flashfsIsSupported() ? 2 : 0;
        flags |= flashfsIsReady() ? 1 : 0;
        r.writeU8(flags);
        r.writeU32(flashfsGetSectors());
        r.writeU32(flashfsGetSize());
        r.writeU32(flashfsGetOffset());
      }
#else
      r.writeU8(0);
      r.writeU32(0);
      r.writeU32(0);
      r.writeU32(0);
#endif
      break;

    case MSP_DATAFLASH_ERASE:
#ifdef USE_FLASHFS
      blackboxEraseAll();
#endif
      break;

    case MSP_DATAFLASH_READ:
#ifdef USE_FLASHFS
      {
        const unsigned int dataSize = m.remain();
        const uint32_t readAddress = m.readU32();
        uint16_t readLength;
        bool allowCompression = false;
        bool useLegacyFormat;

        if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
            readLength = m.readU16();
            if (m.remain()) {
                allowCompression = m.readU8();
            }
            useLegacyFormat = false;
        } else {
            readLength = 128;
            useLegacyFormat = true;
        }
        serializeFlashData(r, readAddress, readLength, useLegacyFormat, allowCompression);
      }
#endif            
      break;

    case MSP_ACC_TRIM:
      r.writeU16(0); // pitch
      r.writeU16(0); // roll
      break;

    case MSP_MIXER_CONFIG:
      r.writeU8(_model.config.mixer.type); // mixerMode, QUAD_X
      r.writeU8(_model.config.mixer.yawReverse); // yaw_motors_reversed
      break;

    case MSP_SET_MIXER_CONFIG:
      _model.config.mixer.type = m.readU8(); // mixerMode, QUAD_X
      _model.config.mixer.yawReverse = m.readU8(); // yaw_motors_reversed
      break;

    case MSP_SENSOR_CONFIG:
      r.writeU8(_model.config.accel.dev); // 3 acc mpu6050
      r.writeU8(_model.config.baro.dev);  // 2 baro bmp085
      r.writeU8(_model.config.mag.dev);   // 3 mag hmc5883l
      break;

    case MSP_SET_SENSOR_CONFIG:
      _model.config.accel.dev = m.readU8(); // 3 acc mpu6050
      _model.config.baro.dev = m.readU8();  // 2 baro bmp085
      _model.config.mag.dev = m.readU8();   // 3 mag hmc5883l
      _model.reload();
      break;

    case MSP_SENSOR_ALIGNMENT:
      r.writeU8(_model.config.gyro.align); // gyro align
      r.writeU8(_model.config.gyro.align); // acc align, Starting with 4.0 gyro and acc alignment are the same
      r.writeU8(_model.config.mag.align);  // mag align
      //1.41+
      r.writeU8(_model.state.gyro.present ? 1 : 0); // gyro detection mask GYRO_1_MASK
      r.writeU8(0); // gyro_to_use
      r.writeU8(_model.config.gyro.align); // gyro 1
      r.writeU8(0); // gyro 2
      break;

    case MSP_SET_SENSOR_ALIGNMENT:
      {
        uint8_t gyroAlign = m.readU8(); // gyro align
        m.readU8(); // discard deprecated acc align
        _model.config.mag.align = m.readU8(); // mag align
        // API >= 1.41 - support the gyro_to_use and alignment for gyros 1 & 2
        if(m.remain() >= 3)
        {
          m.readU8(); // gyro_to_use
          gyroAlign = m.readU8(); // gyro 1 align
          m.readU8(); // gyro 2 align
        }
        _model.config.gyro.align = gyroAlign;
      }
      break;

    case MSP_CF_SERIAL_CONFIG:
      for(int i = 0; i < SERIAL_UART_COUNT; i++)
      {
        if(_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isFeatureActive(FEATURE_SOFTSERIAL)) continue;
        r.writeU8(_model.config.serial[i].id); // identifier
        r.writeU16(_model.config.serial[i].functionMask); // functionMask
        r.writeU8(toBaudIndex(_model.config.serial[i].baud)); // msp_baudrateIndex
        r.writeU8(0); // gps_baudrateIndex
        r.writeU8(0); // telemetry_baudrateIndex
        r.writeU8(toBaudIndex(_model.config.serial[i].blackboxBaud)); // blackbox_baudrateIndex
      }
      break;

    case MSP2_COMMON_SERIAL_CONFIG:
      {
        uint8_t count = 0;
        for (int i = 0; i < SERIAL_UART_COUNT; i++)
        {
          if(_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isFeatureActive(FEATURE_SOFTSERIAL)) continue;
          count++;
        }
        r.writeU8(count);
        for (int i = 0; i < SERIAL_UART_COUNT; i++)
        {
          if(_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isFeatureActive(FEATURE_SOFTSERIAL)) continue;
          r.writeU8(_model.config.serial[i].id); // identifier
          r.writeU32(_model.config.serial[i].functionMask); // functionMask
          r.writeU8(toBaudIndex(_model.config.serial[i].baud)); // msp_baudrateIndex
          r.writeU8(0); // gps_baudrateIndex
          r.writeU8(0); // telemetry_baudrateIndex
          r.writeU8(toBaudIndex(_model.config.serial[i].blackboxBaud)); // blackbox_baudrateIndex
        }
      }
      break;

    case MSP_SET_CF_SERIAL_CONFIG:
      {
        const int packetSize = 1 + 2 + 4;
        while(m.remain() >= packetSize)
        {
          int id = m.readU8();
          int k = _model.getSerialIndex((SerialPortId)id);
          {
            m.advance(packetSize - 1);
            continue;
          }
          _model.config.serial[k].id = id;
          _model.config.serial[k].functionMask = m.readU16();
          _model.config.serial[k].baud = fromBaudIndex((SerialSpeedIndex)m.readU8());
          m.readU8();
          m.readU8();
          _model.config.serial[k].blackboxBaud = fromBaudIndex((SerialSpeedIndex)m.readU8());
        }
      }
      _model.reload();
      break;

    case MSP2_COMMON_SET_SERIAL_CONFIG:
      {
        size_t count = m.readU8();
        (void)count; // ignore
        const int packetSize = 1 + 4 + 4;
        while(m.remain() >= packetSize)
        {
          int id = m.readU8();
          int k = _model.getSerialIndex((SerialPortId)id);
          if(k == -1)
          {
            m.advance(packetSize - 1);
            continue;
          }
          _model.config.serial[k].id = id;
          _model.config.serial[k].functionMask = m.readU32();
          _model.config.serial[k].baud = fromBaudIndex((SerialSpeedIndex)m.readU8());
          m.readU8();
          m.readU8();
          _model.config.serial[k].blackboxBaud = fromBaudIndex((SerialSpeedIndex)m.readU8());
        }
      }
      _model.reload();
      break;

    case MSP_BLACKBOX_CONFIG:
      r.writeU8(1); // Blackbox supported
      r.writeU8(_model.config.blackbox.dev); // device serial or none
      r.writeU8(1); // blackboxGetRateNum()); // unused
      r.writeU8(1); // blackboxGetRateDenom());
      r.writeU16(_model.config.blackbox.pDenom);//blackboxGetPRatio()); // p_denom
      //r.writeU8(_model.config.blackbox.pDenom); // sample_rate
      //r.writeU32(~_model.config.blackbox.fieldsMask);
      break;

    case MSP_SET_BLACKBOX_CONFIG:
      // Don't allow config to be updated while Blackbox is logging
      if (true) {
        _model.config.blackbox.dev = m.readU8();
        const int rateNum = m.readU8(); // was rate_num
        const int rateDenom = m.readU8(); // was rate_denom
        uint16_t pRatio = 0;
        if (m.remain() >= 2) {
            pRatio = m.readU16(); // p_denom specified, so use it directly
        } else {
            // p_denom not specified in MSP, so calculate it from old rateNum and rateDenom
            //pRatio = blackboxCalculatePDenom(rateNum, rateDenom);
            (void)(rateNum + rateDenom);
        }
        _model.config.blackbox.pDenom = pRatio;

        /*if (m.remain() >= 1) {
            _model.config.blackbox.pDenom = m.readU8();
        } else if(pRatio > 0) {
            _model.config.blackbox.pDenom = blackboxCalculateSampleRate(pRatio);
            //_model.config.blackbox.pDenom = pRatio;
        }
        if (m.remain() >= 4) {
          _model.config.blackbox.fieldsMask = ~m.readU32();
        }*/
      }
      break;

    case MSP_ATTITUDE:
      r.writeU16(lrintf(Utils::toDeg(_model.state.attitude.euler.x) * 10.f)); // roll  [decidegrees]
      r.writeU16(lrintf(Utils::toDeg(_model.state.attitude.euler.y) * 10.f)); // pitch [decidegrees]
      r.writeU16(lrintf(Utils::toDeg(-_model.state.attitude.euler.z)));       // yaw   [degrees]
      break;

    case MSP_ALTITUDE:
      r.writeU32(lrintf(_model.state.altitude.height * 100.f));  // alt [cm]
      r.writeU16(lrintf(_model.state.altitude.vario * 100.f));   // vario [cm/s]
      break;

    case MSP_BEEPER_CONFIG:
      r.writeU32(~_model.config.buzzer.beeperMask); // beeper mask
      r.writeU8(0);  // dshot beacon tone
      r.writeU32(0); // dshot beacon off flags
      break;

    case MSP_SET_BEEPER_CONFIG:
      _model.config.buzzer.beeperMask = ~m.readU32(); // beeper mask
      break;

    case MSP_BOARD_ALIGNMENT_CONFIG:
      r.writeU16(_model.config.boardAlignment[0]); // roll
      r.writeU16(_model.config.boardAlignment[1]); // pitch
      r.writeU16(_model.config.boardAlignment[2]); // yaw
      break;

    case MSP_SET_BOARD_ALIGNMENT_CONFIG:
      _model.config.boardAlignment[0] = m.readU16();
      _model.config.boardAlignment[1] = m.readU16();
      _model.config.boardAlignment[2] = m.readU16();
      break;

    case MSP_RX_MAP:
      for(size_t i = 0; i < INPUT_CHANNELS; i++)
      {
        r.writeU8(_model.config.input.channel[i].map);
      }
      break;

    case MSP_SET_RX_MAP:
      for(size_t i = 0; i < 8; i++)
      {
        _model.config.input.channel[i].map = m.readU8();
      }
      break;

    case MSP_RSSI_CONFIG:
      r.writeU8(_model.config.input.rssiChannel);
      break;

    case MSP_SET_RSSI_CONFIG:
      _model.config.input.rssiChannel = m.readU8();
      break;

    case MSP_MOTOR_CONFIG:
      r.writeU16(_model.config.output.minThrottle); // minthrottle
      r.writeU16(_model.config.output.maxThrottle); // maxthrottle
      r.writeU16(_model.config.output.minCommand);  // mincommand
      r.writeU8(_model.state.currentMixer.count);   // motor count
      // 1.42+
      r.writeU8(_model.config.output.motorPoles); // motor pole count
      r.writeU8(_model.config.output.dshotTelemetry); // dshot telemtery
      r.writeU8(0); // esc sensor
      break;

    case MSP_SET_MOTOR_CONFIG:
      _model.config.output.minThrottle = m.readU16(); // minthrottle
      _model.config.output.maxThrottle = m.readU16(); // maxthrottle
      _model.config.output.minCommand = m.readU16();  // mincommand
      if(m.remain() >= 2)
      {
#ifdef ESPFC_DSHOT_TELEMETRY
        _model.config.output.motorPoles = m.readU8();
        _model.config.output.dshotTelemetry = m.readU8();
#else
        m.readU8();
        m.readU8();
#endif
      }
      _model.reload();
      break;

    case MSP_MOTOR_3D_CONFIG:
      r.writeU16(1406); // deadband3d_low;
      r.writeU16(1514); // deadband3d_high;
      r.writeU16(1460); // neutral3d;
      break;

    case MSP_ARMING_CONFIG:
      r.writeU8(5); // auto_disarm delay
      r.writeU8(0);  // disarm kill switch
      r.writeU8(180); // small angle
      break;

    case MSP_RC_DEADBAND:
      r.writeU8(_model.config.input.deadband);
      r.writeU8(0); // yaw deadband
      r.writeU8(0); // alt hold deadband
      r.writeU16(0); // deadband 3d throttle
      break;

    case MSP_SET_RC_DEADBAND:
      _model.config.input.deadband = m.readU8();
      m.readU8(); // yaw deadband
      m.readU8(); // alt hod deadband
      m.readU16(); // deadband 3d throttle
      break;

    case MSP_RX_CONFIG:
      r.writeU8(_model.config.input.serialRxProvider); // serialrx_provider
      r.writeU16(_model.config.input.maxCheck); //maxcheck
      r.writeU16(_model.config.input.midRc); //midrc
      r.writeU16(_model.config.input.minCheck); //mincheck
      r.writeU8(0); // spectrum bind
      r.writeU16(_model.config.input.minRc); //min_us
      r.writeU16(_model.config.input.maxRc); //max_us
      r.writeU8(_model.config.input.interpolationMode); // rc interpolation
      r.writeU8(_model.config.input.interpolationInterval); // rc interpolation interval
      r.writeU16(1500); // airmode activate threshold
      r.writeU8(0); // rx spi prot
      r.writeU32(0); // rx spi id
      r.writeU8(0); // rx spi chan count
      r.writeU8(0); // fpv camera angle
      r.writeU8(2); // rc iterpolation channels: RPYT
      r.writeU8(_model.config.input.filterType); // rc_smoothing_type
      r.writeU8(_model.config.input.filter.freq); // rc_smoothing_input_cutoff
      r.writeU8(_model.config.input.filterDerivative.freq); // rc_smoothing_derivative_cutoff
      r.writeU8(0);//_model.config.input.filter.type); // rc_smoothing_input_type
      r.writeU8(fromFilterTypeDerivative(_model.config.input.filterDerivative.type)); // rc_smoothing_derivative_type
      r.writeU8(0); // usb type
      // 1.42+
      r.writeU8(_model.config.input.filterAutoFactor); // rc_smoothing_auto_factor
      break;

    case MSP_SET_RX_CONFIG:
      _model.config.input.serialRxProvider = m.readU8(); // serialrx_provider
      _model.config.input.maxCheck = m.readU16(); //maxcheck
      _model.config.input.midRc = m.readU16(); //midrc
      _model.config.input.minCheck = m.readU16(); //mincheck
      m.readU8(); // spectrum bind
      _model.config.input.minRc = m.readU16(); //min_us
      _model.config.input.maxRc = m.readU16(); //max_us
      if (m.remain() >= 4) {
        _model.config.input.interpolationMode = m.readU8(); // rc interpolation
        _model.config.input.interpolationInterval = m.readU8(); // rc interpolation interval
        m.readU16(); // airmode activate threshold
      }
      if (m.remain() >= 6) {
        m.readU8(); // rx spi prot
        m.readU32(); // rx spi id
        m.readU8(); // rx spi chan count
      }
      if (m.remain() >= 1) {
        m.readU8(); // fpv camera angle
      }
      // 1.40+
      if (m.remain() >= 6) {
        m.readU8(); // rc iterpolation channels
        _model.config.input.filterType = m.readU8(); // rc_smoothing_type
        _model.config.input.filter.freq = m.readU8(); // rc_smoothing_input_cutoff
        _model.config.input.filterDerivative.freq = m.readU8(); // rc_smoothing_derivative_cutoff
        //_model.config.input.filter.type = m.readU8() == 1 ? FILTER_BIQUAD : FILTER_PT1; // rc_smoothing_input_type
        m.readU8();
        _model.config.input.filterDerivative.type = toFilterTypeDerivative(m.readU8()); // rc_smoothing_derivative_type
      }
      if (m.remain() >= 1) {
        m.readU8(); // usb type
      }
      // 1.42+
      if (m.remain() >= 1) {
        _model.config.input.filterAutoFactor = m.readU8(); // rc_smoothing_auto_factor
      }

      _model.reload();
      break;

    case MSP_FAILSAFE_CONFIG:
      r.writeU8(_model.config.failsafe.delay); // failsafe_delay
      r.writeU8(0); // failsafe_off_delay
      r.writeU16(1000); //failsafe_throttle
      r.writeU8(_model.config.failsafe.killSwitch); // failsafe_kill_switch
      r.writeU16(0); // failsafe_throttle_low_delay
      r.writeU8(1); //failsafe_procedure; default drop
      break;

    case MSP_SET_FAILSAFE_CONFIG:
      _model.config.failsafe.delay = m.readU8(); //failsafe_delay
      m.readU8(); //failsafe_off_delay
      m.readU16(); //failsafe_throttle
      _model.config.failsafe.killSwitch = m.readU8(); //failsafe_kill_switch
      m.readU16(); //failsafe_throttle_low_delay
      m.readU8(); //failsafe_procedure
      break;

    case MSP_RXFAIL_CONFIG:
      for (size_t i = 0; i < _model.state.input.channelCount; i++)
      {
        r.writeU8(_model.config.input.channel[i].fsMode);
        r.writeU16(_model.config.input.channel[i].fsValue);
      }
      break;

    case MSP_SET_RXFAIL_CONFIG:
      {
        size_t i = m.readU8();
        if(i < INPUT_CHANNELS)
        {
          _model.config.input.channel[i].fsMode = m.readU8(); // mode
          _model.config.input.channel[i].fsValue = m.readU16(); // pulse
        }
        else
        {
          r.result = -1;
        }
      }
      break;

    case MSP_RC:
      for(size_t i = 0; i < _model.state.input.channelCount; i++)
      {
        r.writeU16(lrintf(_model.state.input.us[i]));
      }
      break;

    case MSP_RC_TUNING:
      r.writeU8(_model.config.input.rate[AXIS_ROLL]);
      r.writeU8(_model.config.input.expo[AXIS_ROLL]);
      for(size_t i = 0; i < AXIS_COUNT_RPY; i++)
      {
        r.writeU8(_model.config.input.superRate[i]);
      }
      r.writeU8(_model.config.controller.tpaScale); // dyn thr pid
      r.writeU8(50); // thrMid8
      r.writeU8(0);  // thr expo
      r.writeU16(_model.config.controller.tpaBreakpoint); // tpa breakpoint
      r.writeU8(_model.config.input.expo[AXIS_YAW]); // yaw expo
      r.writeU8(_model.config.input.rate[AXIS_YAW]); // yaw rate
      r.writeU8(_model.config.input.rate[AXIS_PITCH]); // pitch rate
      r.writeU8(_model.config.input.expo[AXIS_PITCH]); // pitch expo
      // 1.41+
      r.writeU8(_model.config.output.throttleLimitType); // throttle_limit_type (off)
      r.writeU8(_model.config.output.throttleLimitPercent); // throtle_limit_percent (100%)
      //1.42+
      r.writeU16(_model.config.input.rateLimit[0]); // rate limit roll
      r.writeU16(_model.config.input.rateLimit[1]); // rate limit pitch
      r.writeU16(_model.config.input.rateLimit[2]); // rate limit yaw
      // 1.43+
      r.writeU8(_model.config.input.rateType); // rates type

      break;

    case MSP_SET_RC_TUNING:
      if(m.remain() >= 10)
      {
        const uint8_t rate = m.readU8();
        if(_model.config.input.rate[AXIS_PITCH] == _model.config.input.rate[AXIS_ROLL])
        {
          _model.config.input.rate[AXIS_PITCH] = rate;
        }
        _model.config.input.rate[AXIS_ROLL] = rate;

        const uint8_t expo = m.readU8();
        if(_model.config.input.expo[AXIS_PITCH] == _model.config.input.expo[AXIS_ROLL])
        {
          _model.config.input.expo[AXIS_PITCH] = expo;
        }
        _model.config.input.expo[AXIS_ROLL] = expo;

        for(size_t i = 0; i < AXIS_COUNT_RPY; i++)
        {
          _model.config.input.superRate[i] = m.readU8();
        }
        _model.config.controller.tpaScale = Utils::clamp(m.readU8(), (uint8_t)0, (uint8_t)90); // dyn thr pid
        m.readU8(); // thrMid8
        m.readU8();  // thr expo
        _model.config.controller.tpaBreakpoint = Utils::clamp(m.readU16(), (uint16_t)1000, (uint16_t)2000); // tpa breakpoint
        if(m.remain() >= 1)
        {
          _model.config.input.expo[AXIS_YAW] = m.readU8(); // yaw expo
        }
        if(m.remain() >= 1)
        {
          _model.config.input.rate[AXIS_YAW]  = m.readU8(); // yaw rate
        }
        if(m.remain() >= 1)
        {
          _model.config.input.rate[AXIS_PITCH] = m.readU8(); // pitch rate
        }
        if(m.remain() >= 1)
        {
          _model.config.input.expo[AXIS_PITCH]  = m.readU8(); // pitch expo
        }
        // 1.41
        if(m.remain() >= 2)
        {
          _model.config.output.throttleLimitType = m.readU8(); // throttle_limit_type
          _model.config.output.throttleLimitPercent = m.readU8(); // throttle_limit_percent
        }
        // 1.42
        if(m.remain() >= 6)
        {
          _model.config.input.rateLimit[0] = m.readU16(); // roll
          _model.config.input.rateLimit[1] = m.readU16(); // pitch
          _model.config.input.rateLimit[2] = m.readU16(); // yaw
        }
        // 1.43
        if (m.remain() >= 1)
        {
          _model.config.input.rateType = m.readU8();
        }
      }
      else
      {
        r.result = -1;
        // error
      }
      break;

    case MSP_ADVANCED_CONFIG:
      r.writeU8(1); // gyroSync unused
      r.writeU8(_model.config.loopSync);
      r.writeU8(_model.config.output.async);
      r.writeU8(_model.config.output.protocol);
      r.writeU16(_model.config.output.rate);
      r.writeU16(_model.config.output.dshotIdle);
      r.writeU8(0);    // 32k gyro
      r.writeU8(0);    // PWM inversion
      r.writeU8(0);    // gyro_to_use: {1:0, 2:1. 2:both}
      r.writeU8(0);    // gyro high fsr (flase)
      r.writeU8(48);   // gyro cal threshold
      r.writeU16(125); // gyro cal duration (1.25s)
      r.writeU16(0);   // gyro offset yaw
      r.writeU8(0);    // check overflow
      r.writeU8(_model.config.debug.mode);
      r.writeU8(DEBUG_COUNT);
      break;

    case MSP_SET_ADVANCED_CONFIG:
      m.readU8(); // ignore gyroSync, removed in 1.43
      _model.config.loopSync = m.readU8();
      _model.config.output.async = m.readU8();
      _model.config.output.protocol = m.readU8();
      _model.config.output.rate = m.readU16();
      if(m.remain() >= 2) {
        _model.config.output.dshotIdle = m.readU16(); // dshot idle
      }
      if(m.remain()) {
        m.readU8();  // 32k gyro
      }
      if(m.remain()) {
        m.readU8();  // PWM inversion
      }
      if(m.remain() >= 8) {
        m.readU8();  // gyro_to_use
        m.readU8();  // gyro high fsr
        m.readU8();  // gyro cal threshold
        m.readU16(); // gyro cal duration
        m.readU16(); // gyro offset yaw
        m.readU8();  // check overflow
      }
      if(m.remain()) {
        _model.config.debug.mode = m.readU8();
      }
      _model.reload();
      break;

    //case MSP_COMPASS_CONFIG:
    //  r.writeU16(0); // mag_declination * 10
    //  break;

    case MSP_FILTER_CONFIG:
      r.writeU8(_model.config.gyro.filter.freq);           // gyro lpf
      r.writeU16(_model.config.dterm.filter.freq);         // dterm lpf
      r.writeU16(_model.config.yaw.filter.freq);           // yaw lpf
      r.writeU16(_model.config.gyro.notch1Filter.freq);    // gyro notch 1 hz
      r.writeU16(_model.config.gyro.notch1Filter.cutoff);  // gyro notch 1 cutoff
      r.writeU16(_model.config.dterm.notchFilter.freq);    // dterm notch hz
      r.writeU16(_model.config.dterm.notchFilter.cutoff);  // dterm notch cutoff
      r.writeU16(_model.config.gyro.notch2Filter.freq);    // gyro notch 2 hz
      r.writeU16(_model.config.gyro.notch2Filter.cutoff);  // gyro notch 2 cutoff
      r.writeU8(_model.config.dterm.filter.type);          // dterm type
      r.writeU8(fromGyroDlpf(_model.config.gyro.dlpf));
      r.writeU8(0);                                        // dlfp 32khz type
      r.writeU16(_model.config.gyro.filter.freq);          // lowpass1 freq
      r.writeU16(_model.config.gyro.filter2.freq);         // lowpass2 freq
      r.writeU8(_model.config.gyro.filter.type);           // lowpass1 type
      r.writeU8(_model.config.gyro.filter2.type);          // lowpass2 type
      r.writeU16(_model.config.dterm.filter2.freq);        // dterm lopwass2 freq
      // 1.41+
      r.writeU8(_model.config.dterm.filter2.type);         // dterm lopwass2 type
      r.writeU16(_model.config.gyro.dynLpfFilter.cutoff);  // dyn lpf gyro min
      r.writeU16(_model.config.gyro.dynLpfFilter.freq);    // dyn lpf gyro max
      r.writeU16(_model.config.dterm.dynLpfFilter.cutoff); // dyn lpf dterm min
      r.writeU16(_model.config.dterm.dynLpfFilter.freq);   // dyn lpf dterm max
      // gyro analyse
      r.writeU8(3);  // deprecated dyn notch range
      r.writeU8(_model.config.gyro.dynamicFilter.count);  // dyn_notch_width_percent
      r.writeU16(_model.config.gyro.dynamicFilter.q); // dyn_notch_q
      r.writeU16(_model.config.gyro.dynamicFilter.min_freq); // dyn_notch_min_hz
      // rpm filter
      r.writeU8(_model.config.gyro.rpmFilter.harmonics);  // gyro_rpm_notch_harmonics
      r.writeU8(_model.config.gyro.rpmFilter.minFreq);  // gyro_rpm_notch_min
      // 1.43+
      r.writeU16(_model.config.gyro.dynamicFilter.max_freq); // dyn_notch_max_hz
      break;

    case MSP_SET_FILTER_CONFIG:
      _model.config.gyro.filter.freq = m.readU8();
      _model.config.dterm.filter.freq = m.readU16();
      _model.config.yaw.filter.freq = m.readU16();
      if (m.remain() >= 8) {
          _model.config.gyro.notch1Filter.freq = m.readU16();
          _model.config.gyro.notch1Filter.cutoff = m.readU16();
          _model.config.dterm.notchFilter.freq = m.readU16();
          _model.config.dterm.notchFilter.cutoff = m.readU16();
      }
      if (m.remain() >= 4) {
          _model.config.gyro.notch2Filter.freq = m.readU16();
          _model.config.gyro.notch2Filter.cutoff = m.readU16();
      }
      if (m.remain() >= 1) {
          _model.config.dterm.filter.type = (FilterType)m.readU8();
      }
      if (m.remain() >= 10) {
        m.readU8(); // dlfp type
        m.readU8(); // 32k dlfp type
        _model.config.gyro.filter.freq = m.readU16();
        _model.config.gyro.filter2.freq = m.readU16();
        _model.config.gyro.filter.type = m.readU8();
        _model.config.gyro.filter2.type = m.readU8();
        _model.config.dterm.filter2.freq = m.readU16();
      }
      // 1.41+
      if (m.remain() >= 9) {
        _model.config.dterm.filter2.type = m.readU8();
        _model.config.gyro.dynLpfFilter.cutoff = m.readU16(); // dyn gyro lpf min
        _model.config.gyro.dynLpfFilter.freq = m.readU16();   // dyn gyro lpf max
        _model.config.dterm.dynLpfFilter.cutoff = m.readU16(); // dyn dterm lpf min
        _model.config.dterm.dynLpfFilter.freq = m.readU16();   // dyn dterm lpf min
      }
      if (m.remain() >= 8) {
        m.readU8();  // deprecated dyn_notch_range
        _model.config.gyro.dynamicFilter.count = m.readU8();  // dyn_notch_width_percent
        _model.config.gyro.dynamicFilter.q = m.readU16(); // dyn_notch_q
        _model.config.gyro.dynamicFilter.min_freq = m.readU16(); // dyn_notch_min_hz
        _model.config.gyro.rpmFilter.harmonics = m.readU8();  // gyro_rpm_notch_harmonics
        _model.config.gyro.rpmFilter.minFreq = m.readU8();  // gyro_rpm_notch_min
      }
      // 1.43+
      if (m.remain() >= 1) {
        _model.config.gyro.dynamicFilter.max_freq = m.readU16(); // dyn_notch_max_hz
      }
      _model.reload();
      break;

    case MSP_PID_CONTROLLER:
      r.writeU8(1); // betaflight controller id
      break;

    case MSP_PIDNAMES:
      r.writeString(F("ROLL;PITCH;YAW;ALT;Pos;PosR;NavR;LEVEL;MAG;VEL;"));
      break;

    case MSP_PID:
      for(size_t i = 0; i < PID_ITEM_COUNT; i++)
      {
        r.writeU8(_model.config.pid[i].P);
        r.writeU8(_model.config.pid[i].I);
        r.writeU8(_model.config.pid[i].D);
      }
      break;

    case MSP_SET_PID:
      for (int i = 0; i < PID_ITEM_COUNT; i++)
      {
        _model.config.pid[i].P = m.readU8();
        _model.config.pid[i].I = m.readU8();
        _model.config.pid[i].D = m.readU8();
      }
      _model.reload();
      break;

    case MSP_PID_ADVANCED: /// !!!FINISHED HERE!!!
      r.writeU16(0);
      r.writeU16(0);
      r.writeU16(0); // was pidProfile.yaw_p_limit
      r.writeU8(0); // reserved
      r.writeU8(0); // vbatPidCompensation;
      r.writeU8(0); // feedForwardTransition;
      r.writeU8((uint8_t)std::min(_model.config.dterm.setpointWeight, (int16_t)255)); // was low byte of dtermSetpointWeight
      r.writeU8(0); // reserved
      r.writeU8(0); // reserved
      r.writeU8(0); // reserved
      r.writeU16(0); // rateAccelLimit;
      r.writeU16(0); // yawRateAccelLimit;
      r.writeU8(_model.config.level.angleLimit); // levelAngleLimit;
      r.writeU8(0); // was pidProfile.levelSensitivity
      r.writeU16(0); // itermThrottleThreshold;
      r.writeU16(1000); // itermAcceleratorGain; anti_gravity_gain, 0 in 1.45+
      r.writeU16(_model.config.dterm.setpointWeight);
      r.writeU8(0); // iterm rotation
      r.writeU8(0); // smart feed forward
      r.writeU8(_model.config.iterm.relax); // iterm relax
      r.writeU8(1); // iterm relax type (setpoint only)
      r.writeU8(0); // abs control gain
      r.writeU8(0); // throttle boost
      r.writeU8(0); // acro trainer max angle
      r.writeU16(_model.config.pid[FC_PID_ROLL].F); //pid roll f
      r.writeU16(_model.config.pid[FC_PID_PITCH].F); //pid pitch f
      r.writeU16(_model.config.pid[FC_PID_YAW].F); //pid yaw f
      r.writeU8(0); // antigravity mode
      // 1.41+
      r.writeU8(0); // d min roll
      r.writeU8(0); // d min pitch
      r.writeU8(0); // d min yaw
      r.writeU8(0); // d min gain
      r.writeU8(0); // d min advance
      r.writeU8(0); // use_integrated_yaw
      r.writeU8(0); // integrated_yaw_relax
      // 1.42+
      r.writeU8(_model.config.iterm.relaxCutoff); // iterm_relax_cutoff
      // 1.43+
      r.writeU8(_model.config.output.motorLimit); // motor_output_limit
      r.writeU8(0); // auto_profile_cell_count
      r.writeU8(0); // idle_min_rpm
      break;

    case MSP_SET_PID_ADVANCED:
      m.readU16();
      m.readU16();
      m.readU16(); // was pidProfile.yaw_p_limit
      m.readU8(); // reserved
      m.readU8();
      m.readU8();
      _model.config.dterm.setpointWeight = m.readU8();
      m.readU8(); // reserved
      m.readU8(); // reserved
      m.readU8(); // reserved
      m.readU16();
      m.readU16();
      if (m.remain() >= 2) {
          _model.config.level.angleLimit = m.readU8();
          m.readU8(); // was pidProfile.levelSensitivity
      }
      if (m.remain() >= 4) {
          m.readU16(); // itermThrottleThreshold;
          m.readU16(); // itermAcceleratorGain; anti_gravity_gain
      }
      if (m.remain() >= 2) {
        _model.config.dterm.setpointWeight = m.readU16();
      }
      if (m.remain() >= 14) {
        m.readU8(); //iterm rotation
        m.readU8(); //smart feed forward
        _model.config.iterm.relax = m.readU8(); //iterm relax
        m.readU8(); //iterm relax type
        m.readU8(); //abs control gain
        m.readU8(); //throttle boost
        m.readU8(); //acro trainer max angle
        _model.config.pid[FC_PID_ROLL].F = m.readU16(); // pid roll f
        _model.config.pid[FC_PID_PITCH].F = m.readU16(); // pid pitch f
        _model.config.pid[FC_PID_YAW].F = m.readU16(); // pid yaw f
        m.readU8(); //antigravity mode
      }
      // 1.41+
      if (m.remain() >= 7) {
        m.readU8(); // d min roll
        m.readU8(); // d min pitch
        m.readU8(); // d min yaw
        m.readU8(); // d min gain
        m.readU8(); // d min advance
        m.readU8(); // use_integrated_yaw
        m.readU8(); // integrated_yaw_relax
      }
      // 1.42+
      if (m.remain() >= 1) {
        _model.config.iterm.relaxCutoff = m.readU8(); // iterm_relax_cutoff
      }
      // 1.43+
      if (m.remain() >= 3) {
        _model.config.output.motorLimit = m.readU8(); // motor_output_limit
        m.readU8(); // auto_profile_cell_count
        m.readU8(); // idle_min_rpm
      }
      _model.reload();
      break;

    case MSP_RAW_IMU:
      for (int i = 0; i < AXIS_COUNT_RPY; i++)
      {
        r.writeU16(lrintf(_model.state.accel.adc[i] * ACCEL_G_INV * 2048.f));
      }
      for (int i = 0; i < AXIS_COUNT_RPY; i++)
      {
        r.writeU16(lrintf(Utils::toDeg(_model.state.gyro.adc[i])));
      }
      for (int i = 0; i < AXIS_COUNT_RPY; i++)
      {
        r.writeU16(lrintf(_model.state.mag.adc[i] * 1090));
      }
      break;

    case MSP_MOTOR:
      for (size_t i = 0; i < 8; i++)
      {
        if (i >= OUTPUT_CHANNELS || _model.config.pin[i + PIN_OUTPUT_0] == -1)
        {
          r.writeU16(0);
          continue;
        }
        r.writeU16(_model.state.output.us[i]);
      }
      break;

    case MSP_MOTOR_TELEMETRY:
      r.writeU8(OUTPUT_CHANNELS);
      for (size_t i = 0; i < OUTPUT_CHANNELS; i++)
      {
        int rpm = 0;
        uint16_t invalidPct = 0;
        uint8_t escTemperature = 0;  // degrees celcius
        uint16_t escVoltage = 0;     // 0.01V per unit
        uint16_t escCurrent = 0;     // 0.01A per unit
        uint16_t escConsumption = 0; // mAh

        if (_model.config.pin[i + PIN_OUTPUT_0] != -1)
        {
          rpm = lrintf(_model.state.output.telemetry.rpm[i]);
          invalidPct = _model.state.output.telemetry.errors[i];
          escTemperature = _model.state.output.telemetry.temperature[i];
          escVoltage = _model.state.output.telemetry.voltage[i];
          escCurrent = _model.state.output.telemetry.current[i];
        }

        r.writeU32(rpm);
        r.writeU16(invalidPct);
        r.writeU8(escTemperature);
        r.writeU16(escVoltage);
        r.writeU16(escCurrent);
        r.writeU16(escConsumption);
      }
      break;

    case MSP_SET_MOTOR:
      if(_model.isFeatureActive(FEATURE_GPS) && _model.config.blackbox.mode > 0)
      {
        _model.setGpsHome(true);
      }
      for(size_t i = 0; i < OUTPUT_CHANNELS; i++)
      {
        _model.state.output.disarmed[i] = m.readU16();
      }
      break;

    case MSP_SERVO:
      for(size_t i = 0; i < OUTPUT_CHANNELS; i++)
      {
        if (i >= OUTPUT_CHANNELS || _model.config.pin[i + PIN_OUTPUT_0] == -1)
        {
          r.writeU16(1500);
          continue;
        }
        r.writeU16(_model.state.output.us[i]);
      }
      break;

    case MSP_SERVO_CONFIGURATIONS:
      for(size_t i = 0; i < 8; i++)
      {
        if(i < OUTPUT_CHANNELS)
        {
          r.writeU16(_model.config.output.channel[i].min);
          r.writeU16(_model.config.output.channel[i].max);
          r.writeU16(_model.config.output.channel[i].neutral);
        }
        else
        {
          r.writeU16(1000);
          r.writeU16(2000);
          r.writeU16(1500);
        }
        r.writeU8(100);
        r.writeU8(-1);
        r.writeU32(0);
      }
      break;

    case MSP_SET_SERVO_CONFIGURATION:
      {
        uint8_t i = m.readU8();
        if(i < OUTPUT_CHANNELS)
        {
          _model.config.output.channel[i].min = m.readU16();
          _model.config.output.channel[i].max = m.readU16();
          _model.config.output.channel[i].neutral = m.readU16();
          m.readU8();
          m.readU8();
          m.readU32();
        }
        else
        {
          r.result = -1;
        }
      }
      break;

    case MSP_ACC_CALIBRATION:
      if(!_model.isModeActive(MODE_ARMED)) _model.calibrateGyro();
      break;

    case MSP_MAG_CALIBRATION:
      if(!_model.isModeActive(MODE_ARMED)) _model.calibrateMag();
      break;

    case MSP_VTX_CONFIG:
      if (!_model.state.vtx.active) {
        r.writeU8(0); // vtx type
        r.writeU8(0); // band
        r.writeU8(0); // channel
        r.writeU8(0); // power
        r.writeU8(0); // status
        r.writeU16(0); // freq
        r.writeU8(0); // ready
        r.writeU8(0); // low power disarm
      } else {
        r.writeU8(3 /* SMARTAUDIO */); // vtx type unknown
        r.writeU8(_model.config.vtx.band);    // band
        r.writeU8(_model.config.vtx.channel); // channel
        r.writeU8(_model.config.vtx.power);   // power
        r.writeU8(0);    // status (looks like 1 means pit mode :shrug:)
        r.writeU16(0);   // freq
        r.writeU8(1);    // ready
        r.writeU8(_model.config.vtx.lowPowerDisarm);    // low power disarm
      }
      // 1.42
      r.writeU16(0);   // pit mode freq
      r.writeU8(0);    // vtx table available (no)
      r.writeU8(0);    // vtx table bands
      r.writeU8(0);    // vtx table channels
      r.writeU8(0);    // vtx power levels
      break;
    
    case MSP_SET_VTX_CONFIG:
      {
        uint16_t freq = m.readU16();
        if (freq <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) {  // Value is band and channel
          //const uint8_t newBand = (freq / 8) + 1;
          //const uint8_t newChannel = (freq % 8) + 1;
        }

        if (m.remain() >= 2) {
          _model.config.vtx.power =  m.readU8();
          /*const uint8_t newPitmode = */m.readU8();
        }

        if (m.remain()) {
          _model.config.vtx.lowPowerDisarm = m.readU8();
        }

        // API version 1.42 - this parameter kept separate since clients may already be supplying
        if (m.remain() >= 2) {
          /*const uint16_t pitModeFreq = */m.readU16();
        }

        // API version 1.42 - extensions for non-encoded versions of the band, channel or frequency
        if (m.remain() >= 4) {
          // Added standalone values for band, channel and frequency to move
          // away from the flawed encoded combined method originally implemented.
          _model.config.vtx.band = m.readU8(); 
          _model.config.vtx.channel = m.readU8();
          /*uint16_t newFreq = */m.readU16();
        }
      }
      break;


    case MSP_SET_ARMING_DISABLED:
      {
        const uint8_t cmd = m.readU8();
        uint8_t disableRunawayTakeoff = 0;
        if(m.remain()) {
          disableRunawayTakeoff = m.readU8();
        }
        (void)disableRunawayTakeoff;
#if defined(ESPFC_DEV_PRESET_UNSAFE_ARMING)
        (void)cmd;
#warning "Danger macro used ESPFC_DEV_PRESET_UNSAFE_ARMING"
#else
      _model.setArmingDisabled(ARMING_DISABLED_MSP, cmd);
#endif
        if (_model.isModeActive(MODE_ARMED)) _model.disarm(DISARM_REASON_ARMING_DISABLED);
      }
      break;

    case MSP_SET_PASSTHROUGH:
      {
        uint8_t ptMode = MSP_PASSTHROUGH_ESC_4WAY;
        uint8_t ptArg = 0;
        if(m.remain() >= 2) {
          ptMode = m.readU8();
          ptArg = m.readU8();
        }
        switch (ptMode)
        {
          case MSP_PASSTHROUGH_ESC_4WAY:
            r.writeU8(esc4wayInit());
            serialDeviceInit(&s, 0);
            _postCommand = std::bind(&MspProcessor::processEsc4way, this);
            break;
          default:
            r.writeU8(0);
            break;
        }
        (void)ptArg;
      }
      break;

    case MSP_DEBUG:
      for (int i = 0; i < 8; i++) {
        r.writeU16(_model.state.debug[i]);
      }
      break;

    case MSP_SET_GPS_CONFIG:
      m.readU8(); // provider
      m.readU8(); // sbas mode
      m.readU8(); // auto config
      m.readU8(); // auto baud
      if (m.remain() >= 2) {
          // Added in API version 1.43
          _model.config.gps.setHomeOnce = m.readU8(); // gps_set_home_point_once
          m.readU8(); // gps_ublox_use_galileo
      }
      break;

    case MSP_GPS_CONFIG:
      r.writeU8(1); // provider
      r.writeU8(0); // sbasMode, 0: auto
      r.writeU8(1); // autoConfig, 0: off, 1: on
      r.writeU8(1); // autoBaud, 0: off, 1: on
      // Added in API version 1.43
      r.writeU8(_model.config.gps.setHomeOnce); // gps_set_home_point_once
      r.writeU8(1); // gps_ublox_use_galileo
      break;

  case MSP_RAW_GPS:
      r.writeU8(_model.state.gps.fixType > 2); // STATE(GPS_FIX));
      r.writeU8(_model.state.gps.numSats); // numSat
      r.writeU32(_model.state.gps.location.raw.lat); // lat
      r.writeU32(_model.state.gps.location.raw.lon); // lon
      r.writeU16(std::clamp((int)_model.state.gps.location.raw.height / 1000, 0, (int)std::numeric_limits<uint16_t>::max())); // height [m]
      r.writeU16(_model.state.gps.velocity.raw.groundSpeed / 10); // cm/s
      r.writeU16(_model.state.gps.velocity.raw.heading / 10000); // deg * 10
      // Added in API version 1.44
      r.writeU16(_model.state.gps.accuracy.pDop); // pDOP
      break;

  case MSP_COMP_GPS:
      r.writeU16(0); // GPS_distanceToHome
      r.writeU16(0); // GPS_directionToHome / 10 // resolution increased in Betaflight 4.4 by factor of 10, this maintains backwards compatibility for DJI OSD
      r.writeU8(0);  // GPS_update & 1 // direct or msp
      break;

  case MSP_GPSSVINFO:
      r.writeU8(_model.state.gps.numCh); // GPS_numCh
      for (size_t i = 0; i < _model.state.gps.numCh; i++) {
        r.writeU8(_model.state.gps.svinfo[i].gnssId); // GPS_svinfo_chn[i]
        r.writeU8(_model.state.gps.svinfo[i].id); // GPS_svinfo_svid[i]
        r.writeU8(static_cast<uint8_t>(_model.state.gps.svinfo[i].quality.value & 0xff)); // GPS_svinfo_quality[i]
        r.writeU8(_model.state.gps.svinfo[i].cno); // GPS_svinfo_cno[i]
      }
      break;

    case MSP_EEPROM_WRITE:
      _model.save();
      break;

    case MSP_RESET_CONF:
      if(!_model.isModeActive(MODE_ARMED))
      {
        _model.reset();
        _model.save();
      }
      break;

    case MSP_REBOOT:
      r.writeU8(0); // reboot to firmware
      _postCommand = std::bind(&MspProcessor::processRestart, this);
      break;

    default:
      r.result = 0;
      break;
  }
}

void MspProcessor::processEsc4way()
{
#if defined(ESPFC_MULTI_CORE) && defined(ESPFC_FREE_RTOS)
  timer_pause(TIMER_GROUP_0, TIMER_0);
#endif
  esc4wayProcess(getSerialPort());
  processRestart();
}

void MspProcessor::processRestart()
{
  Hardware::restart(_model);
}

void MspProcessor::serializeFlashData(MspResponse& r, uint32_t address, const uint16_t size, bool useLegacyFormat, bool allowCompression)
{
#ifdef USE_FLASHFS
  (void)allowCompression; // not supported

  const uint32_t allowedToRead = r.remain() - 16;
  const uint32_t flashfsSize = flashfsGetSize();

  uint16_t readLen = std::min(std::min((uint32_t)size, allowedToRead), flashfsSize - address);

  r.writeU32(address);

  uint16_t *readLenPtr = (uint16_t*)&r.data[r.len];
  if (!useLegacyFormat)
  {
    // new format supports variable read lengths
    r.writeU16(readLen);
    r.writeU8(0); // NO_COMPRESSION
  }

  const size_t bytesRead = flashfsReadAbs(address, &r.data[r.len], readLen);
  r.advance(bytesRead);

  if (!useLegacyFormat)
  {
    // update the 'read length' with the actual amount read from flash.
    *readLenPtr = bytesRead;
  }
  else
  {
    // pad the buffer with zeros
    //for (int i = bytesRead; i < allowedToRead; i++) r.writeU8(0);
  }
#endif
}

void MspProcessor::sendResponse(MspResponse& r, Device::SerialDevice& s)
{
  debugResponse(r);
  uint8_t buff[256];
  size_t len = r.serialize(buff, 256);
  s.write(buff, len);
}

void MspProcessor::postCommand()
{
  if(!_postCommand) return;
  std::function<void(void)> cb = _postCommand;
  _postCommand = {};
  cb();
}

bool MspProcessor::debugSkip(uint8_t cmd)
{
  //return true;
  //return false;
  if(cmd == MSP_STATUS) return true;
  if(cmd == MSP_STATUS_EX) return true;
  if(cmd == MSP_BOXNAMES) return true;
  if(cmd == MSP_ANALOG) return true;
  if(cmd == MSP_ATTITUDE) return true;
  if(cmd == MSP_ALTITUDE) return true;
  if(cmd == MSP_RC) return true;
  if(cmd == MSP_RAW_IMU) return true;
  if(cmd == MSP_MOTOR) return true;
  if(cmd == MSP_SERVO) return true;
  if(cmd == MSP_BATTERY_STATE) return true;
  if(cmd == MSP_VOLTAGE_METERS) return true;
  if(cmd == MSP_CURRENT_METERS) return true;
  return false;
}

void MspProcessor::debugMessage(const MspMessage& m)
{
  if(debugSkip(m.cmd)) return;
  Device::SerialDevice * s = _model.getSerialStream(SERIAL_FUNCTION_TELEMETRY_HOTT);
  if(!s) return;

  s->print(m.dir == MSP_TYPE_REPLY ? '>' : '<');
  s->print(m.cmd); s->print('.');
  s->print(m.expected); s->print(' ');
  for(size_t i = 0; i < m.expected; i++)
  {
    s->print(m.buffer[i], HEX); s->print(' ');
  }
  s->println();
}

void MspProcessor::debugResponse(const MspResponse& r)
{
  if(debugSkip(r.cmd)) return;
  Device::SerialDevice * s = _model.getSerialStream(SERIAL_FUNCTION_TELEMETRY_HOTT);
  if(!s) return;

  s->print(r.result == 1 ? '>' : (r.result == -1 ? '!' : '@'));
  s->print(r.cmd); s->print('.');
  s->print(r.len); s->print(' ');
  for(size_t i = 0; i < r.len; i++)
  {
    s->print(r.data[i], HEX); s->print(' ');
  }
  s->println();
}

}