#include "MspProcessor.h"
#include <algorithm>
#include <limits>

#include "../../../madflight_modules.h"

/*
//betaflight msp flow

setup:
//OK:   MSP_API_VERSION: //1 -> pc config: 1.46 works, 1.47 does not work,  web app: 1.44 does not work, 1.46, 1.47 works
//OK:   MSP_FC_VARIANT                  2    // out message: Get flight controller variant
//OK:   MSP_FC_VERSION                  3    // out message: Get flight controller version
//OK:   MSP_BUILD_INFO                  5    // out message: Get build information
//OK:   MSP_BOARD_INFO                  4    // out message: Get board information
//OK:   MSP_STATUS                      101  // out message: Cycletime & errors_count & sensor present & box activation & 
//OK:   MSP_UID                         160  // out message: Unique device ID
//TODO: MSP2_GET_TEXT                       0x3006, MSP2TEXT_BUILDKEY                        5
//TODO: MSP2_GET_TEXT                       0x3006, MSP2TEXT_CRAFT_NAME                      2
//TODO: MSP2_GET_TEXT                       0x3006, MSP2TEXT_PILOT_NAME                      1
//OK:   MSP_SET_ARMING_DISABLED         99   // in message:  Enable/disable arming
//TODO: MSP_SET_RTC                     246  // in message:  Set the RTC clock
//TODO: MSP_CURRENT_METER_CONFIG        40   // out message: Get current meter configuration
//TODO: MSP_BATTERY_CONFIG              32   // out message: Get battery configuration
//TODO: MSP_BLACKBOX_CONFIG             80   // out message: Get blackbox settings
//TODO: MSP_DATAFLASH_SUMMARY           70   // out message: Get description of dataflash chip
//TODO: MSP_SDCARD_SUMMARY              79   // out message: Get SD card state
//OK:   MSP_ACC_TRIM                    240  // out message: Get acc angle trim values
//OK:   MSP_STATUS_EX                   150  // out message: Cycletime, errors_count, CPU load, sensor present etc
//TODO: MSP2_MCU_INFO                       0x300C
//OK:   MSP_MIXER_CONFIG                42   // out message: Get mixer configuration
//OK:   MSP_SENSOR_ALIGNMENT            126  // out message: Orientation of acc,gyro,mag
//TODO: MSP2_SENSOR_CONFIG_ACTIVE           0x300A

loop:
//OK:   MSP_ATTITUDE                    108  // out message: 2 angles 1 heading
//OK:   MSP_ANALOG                      110  // out message: Vbat, powermetersum, rssi if available on RX
//OK:   MSP_BATTERY_STATE               130  // out message: Connected/Disconnected, Voltage, Current Used
//OK:   MSP_STATUS_EX                   150  // out message: Cycletime, errors_count, CPU load, sensor present etc
*/

//shown as "manufacturerName/boardName(targetName)" in status bar and connection log
//if boardName is empty then boardIdentifier is shown as boardName
//if manufacturerName and targetName are empty then boardIdentifier is shown as targetName
const char * const manufacturerName = "";
const char * const boardName = ""; 
const char * const boardIdentifier = "MADF"; //4 UPPER CASE alpha numeric characters
const char * const targetName = MADFLIGHT_VERSION;

const char * const targetVersion = "0000000";

#define GIT_SHORT_REVISION_LENGTH   7 // lower case hexadecimal digits.
const char * const shortGitRevision = "v0.0.0";

//date and time shown in configurator log
#define BUILD_TIME_LENGTH 8
const char * const buildTime = __TIME__;  // "HH:MM:SS"

#define BUILD_DATE_LENGTH 11
const char * const buildDate = __DATE__; // "MMM DD YYYY" MMM = Jan/Feb/...

const char * flightControllerIdentifier = "BTFL"; //needs to be BTFL for app.betaflight.com

//can be anything, but some functions (servo) appear to check version
#define FC_VERSION_MAJOR            4  // increment when a major release is made (big new feature, etc)
#define FC_VERSION_MINOR            4  // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL      4  // increment when a bug is fixed

#define getBoardId0 0xDEADBEEF
#define getBoardId1 0xDEADBEEF
#define getBoardId2 0xDEADBEEF

enum MixerType {
  FC_MIXER_TRI = 1,
  FC_MIXER_QUADP = 2,  //shown as X
  FC_MIXER_QUADX = 3,  //nice
  FC_MIXER_BICOPTER = 4,
  FC_MIXER_GIMBAL = 5,
  FC_MIXER_Y6 = 6,   //nice
  FC_MIXER_HEX6 = 7,   //nice
  FC_MIXER_FLYING_WING = 8, //ugly wing
  FC_MIXER_Y4 = 9,
  FC_MIXER_HEX6X = 10,
  FC_MIXER_OCTOX8 = 11,
  FC_MIXER_OCTOFLATP = 12,
  FC_MIXER_OCTOFLATX = 13,
  FC_MIXER_AIRPLANE = 14,        //shown as biplane in configurator
  FC_MIXER_HELI_120_CCPM = 15,  //ugly wing
  FC_MIXER_HELI_90_DEG = 16,
  FC_MIXER_VTAIL4 = 17,
  FC_MIXER_HEX6H = 18,  //ugly wing
  FC_MIXER_PPM_TO_SERVO = 19,    // PPM -> servo relay
  FC_MIXER_DUALCOPTER = 20,
  FC_MIXER_SINGLECOPTER = 21, //ugly wing
  FC_MIXER_ATAIL4 = 22,
  FC_MIXER_CUSTOM = 23,
  FC_MIXER_CUSTOM_AIRPLANE = 24,  //ugly wing
  FC_MIXER_CUSTOM_TRI = 25,
  FC_MIXER_QUADX_1234 = 26,
};

#define FC_MIXERRTYPE FC_MIXER_QUADX

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

extern "C" {
  #include "betaflight/msp_protocol.h"
  #include "betaflight/msp_protocol_v2_common.h"
  #include "betaflight/msp_protocol_v2_betaflight.h"
  //#include "betaflight/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)
{
  if(speed >= 2470000) return SERIAL_SPEED_INDEX_2470000;
  if(speed >= 2000000) return SERIAL_SPEED_INDEX_2000000;
  if(speed >= 1500000) return SERIAL_SPEED_INDEX_1500000;
  if(speed >= 1000000) return SERIAL_SPEED_INDEX_1000000;
  if(speed >= 921600)  return SERIAL_SPEED_INDEX_921600;
  if(speed >= 500000)  return SERIAL_SPEED_INDEX_500000;
  if(speed >= 460800)  return SERIAL_SPEED_INDEX_460800;
  if(speed >= 400000)  return SERIAL_SPEED_INDEX_400000;
  if(speed >= 250000)  return SERIAL_SPEED_INDEX_250000;
  if(speed >= 230400)  return SERIAL_SPEED_INDEX_230400;
  if(speed >= 115200)  return SERIAL_SPEED_INDEX_115200;
  if(speed >= 57600)   return SERIAL_SPEED_INDEX_57600;
  if(speed >= 38400)   return SERIAL_SPEED_INDEX_38400;
  if(speed >= 19200)   return SERIAL_SPEED_INDEX_19200;
  if(speed >= 9600)    return SERIAL_SPEED_INDEX_9600;
  return SERIAL_SPEED_INDEX_AUTO;
}

static int32_t fromBaudIndex(SerialSpeedIndex index)
{
  switch(index)
  {
    case SERIAL_SPEED_INDEX_9600:    return 9600;
    case SERIAL_SPEED_INDEX_19200:   return 19200;
    case SERIAL_SPEED_INDEX_38400:   return 38400;
    case SERIAL_SPEED_INDEX_57600:   return 57600;
    case SERIAL_SPEED_INDEX_115200:  return 115200;
    case SERIAL_SPEED_INDEX_230400:  return 230400;
    case SERIAL_SPEED_INDEX_250000:  return 250000;
    case SERIAL_SPEED_INDEX_400000:  return 400000;
    case SERIAL_SPEED_INDEX_460800:  return 460800;
    case SERIAL_SPEED_INDEX_500000:  return 500000;
    case SERIAL_SPEED_INDEX_921600:  return 921600;
    case SERIAL_SPEED_INDEX_1000000: return 1000000;
    case SERIAL_SPEED_INDEX_1500000: return 1500000;
    case SERIAL_SPEED_INDEX_2000000: return 2000000;
    case SERIAL_SPEED_INDEX_2470000: return 2470000;
    case SERIAL_SPEED_INDEX_AUTO:
    default:
      return 0;
  }
}

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

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

static uint8_t fromGyroDlpf(uint8_t t)
{
  switch(t) {
    case GYRO_DLPF_256: return 0;
    case 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;

}

*/


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)
{
  r.cmd = m.cmd;
  r.version = m.version;
  r.result = 1;
  switch(m.cmd)
  {
    case MSP_API_VERSION: //1 0x01
      r.writeU8(MSP_PROTOCOL_VERSION);
      r.writeU8(API_VERSION_MAJOR);
      r.writeU8(API_VERSION_MINOR);
      break;

    case MSP_FC_VARIANT: //2 0x02
      r.writeData(flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
      break;

    case MSP_FC_VERSION: //3 0x03
      r.writeU8(FC_VERSION_MAJOR);
      r.writeU8(FC_VERSION_MINOR);
      r.writeU8(FC_VERSION_PATCH_LEVEL);
      break;

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

    case MSP_BOARD_INFO: //4 0x04
      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(strlen(boardName));  // board name
      r.writeData(boardName, strlen(boardName));
      r.writeU8(strlen(manufacturerName));   // manufacturer name
      r.writeData( manufacturerName, strlen( manufacturerName));
      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(1000); // sample rate //_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;
                                     //loop- err-- activ modemask--- pi load- ----- fm -- -----------  
                                     // 0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 
    case MSP_STATUS_EX:  //150 0x96 -> E7 10 00 00 23 00 00 00 00 00 00 0C 00 04 00 00 1A 04 10 11 00 00 29 00
    case MSP_STATUS: {    //101 0x65 -> E7 10 00 00 23 00 00 00 00 00 00 0C 00 00 00 00 1A 04 10 10 00 00 29 00
      r.writeU16(0x01e7); //0,1 _model.state.stats.loopTime());
      r.writeU16(0);  //2,3 _model.state.i2cErrorCount); // i2c error count
      uint16_t sensors = (imu.installed()?0x01:0) | (bar.installed()?0x02:0) | (mag.installed()?0x04:0) | (gps.installed()?0x08:0) | (rdr.installed()?0x10:0) | (imu.installed()?0x20:0);
      r.writeU16(sensors); //4,5 accel << 1 | baro << 1 | mag << 2 | gps << 3 | sonar << 4 | gyro << 5);
      r.writeU32(0); //6,7,8,9 _model.state.mode.mask); // flight mode flags
      r.writeU8(0); //10 pid profile
      r.writeU16(0); //11,12 cpu load in % - lrintf(_model.state.stats.getCpuLoad()));
      if (m.cmd == MSP_STATUS_EX) {
        r.writeU8(1); //13 max profile count
        r.writeU8(0); //14 current rate profile index
      } else {  // MSP_STATUS
        //r.writeU16(_model.state.gyro.timer.interval); // gyro cycle time
        r.writeU16(0); //13,14
      }

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

      // Write arming disable flags
      r.writeU8(0); //16 ARMING_DISABLED_FLAGS_COUNT);  // 1 byte, flag count
      r.writeU32(0); //17,18,19,20 _model.state.mode.armingDisabledFlags);  // 4 bytes, flags
      r.writeU8(0); //21 reboot required

      r.writeU8(0); //21 - cpu temperature in deg C
      r.writeU8(0x00); //22 - ???
      break;
    }

    case MSP_UID: //99 0x63
      r.writeU32(getBoardId0);
      r.writeU32(getBoardId1);
      r.writeU32(getBoardId2);
      break;

    case MSP_SET_ARMING_DISABLED: {
        const uint8_t cmd = m.readU8(); //0x00 = disarm, 0x01 = arm
        (void)cmd;
        uint8_t disableRunawayTakeoff = 0;
        if(m.remain()) { //BF config sends 0x00 when cmd=arm, 0x01 when cmd=disarm in motors tab
          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_ACC_TRIM: //240 0xF0
      r.writeU16(0); // pitch
      r.writeU16(0); // roll
      break;

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

    case MSP_ATTITUDE: {//108 0x6C
      static float roll = 0;
      roll+= 0.1;
      while(roll>=180) roll -= 360;
      static float yaw = 0;
      yaw+= 0.1;
      while(yaw>=360) yaw -= 360;
      r.writeU16(lrintf(ahr.roll * 10));// roll  [decidegrees]
      r.writeU16(lrintf(-ahr.pitch * 10));// // pitch [decidegrees]
      r.writeU16(lrintf(ahr.yaw)); // yaw   [degrees]
      break;
    }

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

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

      // battery state
      r.writeU8(lrintf(bat.v * 10)); // voltage in 0.1V
      r.writeU16(lrintf(bat.mah)); // milliamp hours drawn from battery
      r.writeU16(lrintf(bat.i * 100));//toIbatCurrent(_model.state.battery.current)); // current in 0.01A
      r.writeU8(0); // battery alerts ???
      r.writeU16(lrintf(bat.v * 100)); // vvoltage in 0.01V, this is the voltage shown in the configurator
      break;

    case MSP_ANALOG: //110 0x6E
      r.writeU8(lrintf(bat.v * 10)); // voltage in 0.1V
      r.writeU16(lrintf(bat.mah)); // mah drawn
      r.writeU16(0);//_model.getRssi()); // rssi
      r.writeU16(lrintf(bat.i * 100)); // current in 0.01A
      r.writeU16(lrintf(bat.v * 100)); // voltage in 0.01V
      break;

    case MSP_BOXNAMES: //116 0x74 out message: The aux switch names
      //r.writeString("ARM;AIRMODE;ANGLE;ALTHOLD;BEEPER;FAILSAFE;BLACKBOX;BLACKBOXERASE;");
      r.writeString("ARM;FLIGHTMODE;");
      break;

// === BF Receiver Tab

    case MSP_RC: //105 0x69
      //max 12 channels for betaflight: AERT + AUX1-8
      r.writeU16(rcl.pwm[ rcl._getCh(cfg.rcl_rol_ch) ]);
      r.writeU16(rcl.pwm[ rcl._getCh(cfg.rcl_pit_ch) ]);
      r.writeU16(rcl.pwm[ rcl._getCh(cfg.rcl_yaw_ch) ]);
      r.writeU16(rcl.pwm[ rcl._getCh(cfg.rcl_thr_ch) ]);
      r.writeU16(rcl.pwm[ rcl._getCh(cfg.rcl_arm_ch) ]);
      r.writeU16(rcl.pwm[ rcl._getCh(cfg.rcl_flt_ch) ]);
      for(size_t i = 6; i < 12; i++) {
        r.writeU16(rcl.pwm[i]);
      }
      break;

// === BF Sensor Tab

    case MSP_ALTITUDE: //109 0x6D
      r.writeU32(lrintf((bar.alt - bar.ground_level) * 100.f));  // alt [cm]
      r.writeU16(lrintf(0 * 100.f));   //TODO vario [cm/s]
      break;

    case MSP_RAW_IMU: { //102 0x66
      r.writeU16(lrintf(imu.ax * 2048)); //g / 2048
      r.writeU16(lrintf(imu.ay * 2048));
      r.writeU16(lrintf(imu.az * 2048));
      r.writeU16(lrintf(imu.gx * 4));  //deg/s / 4
      r.writeU16(lrintf(imu.gy * 4));
      r.writeU16(lrintf(imu.gz * 4));
      r.writeU16(lrintf(mag.mx * 10)); //dimensionless in BF with scale options 100 to 10000
      r.writeU16(lrintf(mag.my * 10));
      r.writeU16(lrintf(mag.mz * 10));
      break;
    }

    case MSP_DEBUG:
      for (int i = 0; i < 8; i++) {
        r.writeU16(i*100);
      }
      break;

    case MSP_SONAR_ALTITUDE: //  58 0x3A out message: Get sonar altitude [cm]
      r.writeU32(rdr.dist * 100);
      break;


// === BF Motor Tab, test motors

    case MSP_SET_MOTOR: //214 0xD6
      out.testmotor_enable(true);
      for(size_t i = 0; i < 4; i++) {
        out.testmotor_set_output(i, (m.readU16() - 1000) / 1024.f); //BF sends values 1000-2024
      }
      break;

    case MSP_MOTOR:
      for (size_t i = 0; i < 4; i++) {
        r.writeU16(out.get_output(i) * 1024 + 1000);
      }
      for (size_t i = 0; i < 4; i++) {
        r.writeU16(0);
      }      
      break;

    case MSP_MOTOR_CONFIG:
      r.writeU16(1000); // minthrottle
      r.writeU16(2024); // maxthrottle
      r.writeU16(1000);  // mincommand
      r.writeU8(4);   // motor count
      // 1.42+
      r.writeU8(14); // motor pole count
      r.writeU8(0); // dshot telemtery
      r.writeU8(0); // esc sensor
      break;

    case MSP_MOTOR_TELEMETRY:
      r.writeU8(4); //number of motors
      for (size_t i = 0; i < 4; i++) {
        r.writeU32(out.rpm(i)); //rpm
        r.writeU16(0); //invalidPct telemetry.errors
        r.writeU8(0); //escTemperature [degrees celcius]
        r.writeU16(0);  //escVoltage [0.01V per unit]
        r.writeU16(0);  //escCurent [0.01A per unit]
        r.writeU16(0); //escConsumption [mAh]
      }
      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;
*/

// ======= TODO ============
#if 0
    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_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_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_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_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_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_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_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_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_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_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_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_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_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;

#endif

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

#if 0
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, MF_Serial* 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();
}

#endif //#if 0