diff --git a/lib/Espfc/src/Connect/Cli.cpp b/lib/Espfc/src/Connect/Cli.cpp
index 97dd3f12..989558ac 100644
--- a/lib/Espfc/src/Connect/Cli.cpp
+++ b/lib/Espfc/src/Connect/Cli.cpp
@@ -5,6 +5,7 @@
 #include "Device/GyroDevice.h"
 #include "Hal/Pgm.h"
 #include "msp/msp_protocol.h"
+#include "Connect/EspProtocol.hpp"
 
 #ifdef USE_FLASHFS
 #include "Device/FlashDevice.h"
@@ -474,6 +475,9 @@ const Cli::Param * Cli::initialize(ModelConfig& c)
     Param(PSTR("input_mid"), &c.input.midRc),
     Param(PSTR("input_max"), &c.input.maxRc),
 
+    Param(PSTR("input_min_check"), &c.input.minCheck),
+    Param(PSTR("input_max_check"), &c.input.maxCheck),
+
     Param(PSTR("input_interpolation"), &c.input.interpolationMode, interpolChoices),
     Param(PSTR("input_interpolation_interval"), &c.input.interpolationInterval),
 
@@ -670,11 +674,11 @@ const Cli::Param * Cli::initialize(ModelConfig& c)
     Param(PSTR("pin_i2c_scl"), &c.pin[PIN_I2C_0_SCL]),
     Param(PSTR("pin_i2c_sda"), &c.pin[PIN_I2C_0_SDA]),
 #endif
-#ifdef ESPFC_ADC_0
-    Param(PSTR("pin_input_adc_0"), &c.pin[PIN_INPUT_ADC_0]),
+#ifdef ESPFC_ADC_VBAT
+    Param(PSTR("pin_input_adc_vbat"), &c.pin[PIN_INPUT_ADC_VBAT]),
 #endif
-#ifdef ESPFC_ADC_1
-    Param(PSTR("pin_input_adc_1"), &c.pin[PIN_INPUT_ADC_1]),
+#ifdef ESPFC_ADC_IBAT
+    Param(PSTR("pin_input_adc_ibat"), &c.pin[PIN_INPUT_ADC_IBAT]),
 #endif
 #ifdef ESPFC_SPI_0
     Param(PSTR("pin_spi_0_sck"), &c.pin[PIN_SPI_0_SCK]),
@@ -990,12 +994,14 @@ void Cli::execute(CliCmd& cmd, Stream& s)
   }
   else if(strcmp_P(cmd.args[0], PSTR("dump")) == 0)
   {
+    s.println(F("#dump begin"));
     s.println(F("defaults"));
     for(size_t i = 0; _params[i].name; ++i)
     {
       print(_params[i], s);
     }
     s.println(F("save"));
+    s.println(F("#dump end"));
   }
   else if(strcmp_P(cmd.args[0], PSTR("cal")) == 0)
   {
@@ -1579,9 +1585,9 @@ void Cli::printVersion(Stream& s) const
   s.print(' ');
   s.print(buildTime);
   s.print(" api=");
-  s.print(API_VERSION_MAJOR);
+  s.print(ESP_API_VERSION_MAJOR);
   s.print('.');
-  s.print(API_VERSION_MINOR);
+  s.print(ESP_API_VERSION_MINOR);
   s.print(" gcc=");
   s.print(__VERSION__);
   s.print(" std=");
diff --git a/lib/Espfc/src/Connect/EspProtocol.hpp b/lib/Espfc/src/Connect/EspProtocol.hpp
new file mode 100644
index 00000000..da3855d2
--- /dev/null
+++ b/lib/Espfc/src/Connect/EspProtocol.hpp
@@ -0,0 +1,610 @@
+#pragma once
+
+#include <cstdint>
+
+namespace Espfc::Connect
+{
+
+enum EspCommand : uint8_t
+{
+  ESP_CMD_VERSION = 0x01,
+  ESP_CMD_STATUS = 0x02,
+  ESP_CMD_STATISTICS = 0x03,
+  ESP_CMD_ATTITUDE = 0x04,
+  ESP_CMD_SENSORS = 0x05,
+  ESP_CMD_INPUT = 0x06,
+  ESP_CMD_OUTPUT = 0x07,
+  ESP_CMD_VOLTAGE = 0x08,
+  ESP_CMD_CURRENT = 0x09,
+  ESP_CMD_GPS = 0x0a,
+  ESP_CMD_GPS_INFO = 0x0b,
+  ESP_CMD_RPM_TLM = 0x0c,
+  ESP_CMD_PID_INNER = 0x0d,
+  ESP_CMD_PID_OUTER = 0x0e,
+  ESP_CMD_DEBUG = 0x0f,
+
+  ESP_CMD_MODE_NAMES = 0x10,
+  ESP_CMD_FEATURE_NAMES = 0x11,
+  ESP_CMD_DEBUG_NAMES = 0x12,
+  ESP_CMD_SERIAL_NAMES = 0x13,
+  ESP_CMD_PID_NAMES = 0x14,
+  ESP_CMD_MIXER_NAMES = 0x15,
+  ESP_CMD_BLACKBOX_NAMES = 0x16,
+
+  ESP_CMD_INPUT_CONFIG = 0x20,
+  ESP_CMD_INPUT_CHANNEL_CONFIG = 0x21,
+  ESP_CMD_OUTPUT_CONFIG = 0x22,
+  ESP_CMD_OUTPUT_CHANNEL_CONFIG = 0x23,
+  ESP_CMD_GYRO_CONFIG = 0x24,
+  ESP_CMD_ACCEL_CONFIG = 0x25,
+  ESP_CMD_SERIAL_CONFIG = 0x26,
+  ESP_CMD_VOLTAGE_CONFIG = 0x27,
+  ESP_CMD_CURRENT_CONFIG = 0x28,
+  ESP_CMD_PID_CONFIG = 0x29,
+  ESP_CMD_PID_COMMON_CONFIG = 0x2a,
+  ESP_CMD_MODES_CONFIG = 0x2b,
+  ESP_CMD_FAILSAFE_CONFIG = 0x2c,
+  ESP_CMD_BLACKBOX_CONFIG = 0x2d,
+  ESP_CMD_MIXER_CONFIG = 0x2e,
+  ESP_CMD_RPM_FILTER_CONFIG = 0x2f,
+  ESP_CMD_DYN_NOTCH_CONFIG = 0x30,
+  ESP_CMD_VTX_CONFIG = 0x31,
+  ESP_CMD_GPS_CONFIG = 0x32,
+  ESP_CMD_BARO_CONFIG = 0x33,
+  ESP_CMD_MAG_CONFIG = 0x34,
+  ESP_CMD_FEATURE_CONFIG = 0x35,
+  ESP_CMD_MODEL_CONFIG = 0x36,
+  ESP_CMD_CALIBRATE = 0x37,
+  ESP_CMD_ESC_PASSTHROUGH = 0x38,
+  ESP_CMD_ALIGNMENT_CONFIG = 0x39,
+  ESP_CMD_PIN_CONFIG = 0x3a,
+  ESP_CMD_SENSOR_CONFIG = 0x3b,
+  ESP_CMD_PID_TUNING = 0x3c,
+
+  ESP_CMD_FLASH_LOGS = 0x40,
+  ESP_CMD_FLASH_READ = 0x41,
+  ESP_CMD_FLASH_ERASE = 0x42,
+
+  ESP_CMD_OUTPUT_OVERRIDE = 0x4a,
+
+  ESP_CMD_DISABLE_ARM = 0xf0,
+  ESP_CMD_DEFAULTS = 0xf1,
+  ESP_CMD_SAVE = 0xf2,
+  ESP_CMD_REBOOT = 0xf3,
+};
+
+static const uint8_t ESP_API_VERSION_MAJOR = 0x01;
+static const uint8_t ESP_API_VERSION_MINOR = 0x00;
+
+enum EspCmdHardwareType : uint8_t
+{
+  ESP_HW_TYPE_UNKNOWN = 0x00,
+  ESP_HW_TYPE_ESP32 = 0x01,
+  ESP_HW_TYPE_ESP32S2 = 0x02,
+  ESP_HW_TYPE_ESP32S3 = 0x03,
+  ESP_HW_TYPE_ESP32C3 = 0x04,
+  ESP_HW_TYPE_ESP8266 = 0x0f,
+  ESP_HW_TYPE_RP2040 = 0x10,
+  ESP_HW_TYPE_RP2350 = 0x11,
+};
+
+enum EspCmdPinIndex : uint8_t
+{
+  ESP_PIN_0,
+  ESP_PIN_1,
+  ESP_PIN_2,
+  ESP_PIN_3,
+  ESP_PIN_4,
+  ESP_PIN_5,
+  ESP_PIN_6,
+  ESP_PIN_7,
+  ESP_PIN_SCL = ESP_PIN_0,
+  ESP_PIN_SDA = ESP_PIN_1,
+  ESP_PIN_SCK = ESP_PIN_0,
+  ESP_PIN_MOSI = ESP_PIN_1,
+  ESP_PIN_MISO = ESP_PIN_2,
+  ESP_PIN_CS0 = ESP_PIN_3,
+  ESP_PIN_CS1 = ESP_PIN_4,
+  ESP_PIN_CS2 = ESP_PIN_5,
+  ESP_PIN_IDX_MASK = 0x0f,
+};
+
+enum EspCmdPinFunction : uint8_t
+{
+  ESP_PIN_SERIAL = 0x00,
+  ESP_PIN_OUTPUT = 0x10,
+  ESP_PIN_INPUT = 0x20,
+  ESP_PIN_I2C = 0x30,
+  ESP_PIN_SPI = 0x40,
+  ESP_PIN_ADC = 0x50,
+  ESP_PIN_BUTTON = 0x60,
+  ESP_PIN_BUZZER = 0x70,
+  ESP_PIN_LED = 0x80,
+  ESP_PIN_FN_MASK = 0xf0,
+};
+
+struct EspCmdVersion
+{
+  uint8_t apiMajor;
+  uint8_t apiMinor;
+  uint8_t hwType;
+  uint32_t capabilities;
+  uint8_t fwVersion[16];
+  uint8_t fwRevision[16];
+} __attribute__((packed));
+
+struct EspCmdStatus
+{
+  uint16_t sensors;
+  uint16_t gyroTimeUs;
+  uint16_t loopTimeUs;
+  uint32_t modeSwitchMask;
+  uint32_t modeActiveMask;
+  uint32_t armingDisableFlags;
+} __attribute__((packed));
+
+struct EspCmdStatistics
+{
+  uint32_t uptimeMs;
+  uint16_t loopTimeUs;
+  uint8_t cpuLoad;
+  uint8_t cpu0Load;
+  uint8_t cpu1Load;
+  uint32_t heapTotal;
+  uint32_t heapFree;
+  uint32_t flashTotal;
+  uint32_t flashUsed;
+} __attribute__((packed));
+
+struct EspCmdAttitude
+{
+  int16_t x;
+  int16_t y;
+  int16_t z;
+  int16_t w;
+} __attribute__((packed));
+
+struct EspCmdSensors
+{
+  int16_t gyro[3];
+  int16_t accel[3];
+  int16_t mag[3];
+  int16_t baroAlt;
+} __attribute__((packed));
+
+struct EspCmdPidTerm {
+  int16_t p;
+  int16_t i;
+  int16_t d;
+  int16_t f;
+} __attribute__((packed));
+
+struct EspCmdPidInner
+{
+  EspCmdPidTerm roll;
+  EspCmdPidTerm pitch;
+  EspCmdPidTerm yaw;
+  EspCmdPidTerm alt;
+} __attribute__((packed));
+
+struct EspCmdPidOuter
+{
+  EspCmdPidTerm roll;
+  EspCmdPidTerm pitch;
+} __attribute__((packed));
+
+struct EspCmdInput
+{
+  uint8_t channelCount; // number of input channels
+  uint16_t channels[16];  // up to 16 channels
+} __attribute__((packed));
+
+struct EspCmdOutput
+{
+  uint8_t channelCount; // number of output channels
+  uint16_t channels[OUTPUT_CHANNELS];  // up to 8 channels
+} __attribute__((packed));
+
+struct EspCmdVoltage
+{
+  uint8_t count;
+  struct __attribute__((packed)){
+    int8_t source; // voltage source
+    uint16_t voltage; // in 0.01V units
+    int8_t cellCount; // number of cells in the battery
+  } items[1];
+} __attribute__((packed));
+
+struct EspCmdCurrent
+{
+  uint8_t count;
+  struct __attribute__((packed)) {
+    int8_t source; // voltage source
+    uint16_t current; // in 0.01A units
+    uint32_t consumption; // in mAh
+  } items[1];
+} __attribute__((packed));
+
+struct EspCmdGps
+{
+  uint32_t time; // ms since epoch
+  uint8_t fixType; // 0: no fix, 1: 2D fix, 2: 3D fix, 3: DGPS fix, 4: RTK fix
+  uint8_t sats; // number of satellites used in the fix
+  int32_t latitude; // in degrees * 1e7
+  int32_t longitude; // in degrees * 1e7
+  int32_t altitude; // in meters * 1000
+  int32_t speed; // in cm/s
+  int32_t course; // in degrees * 100
+  uint16_t pDop; // * 100
+  uint16_t hAccu; // dm
+  uint16_t vAccu; // dm
+} __attribute__((packed));
+
+struct EspCmdGpsInfo
+{
+  uint8_t numSats;
+  struct __attribute__((packed)) SvInfo {
+    uint8_t gnssId;
+    uint8_t id;
+    uint8_t quality;
+    uint8_t cno;
+  } svs[32]; // up to 32 SVs
+} __attribute__((packed));
+
+struct EspCmdRpmTlm
+{
+  uint8_t channelCount;
+  struct __attribute__((packed)) Channel {
+    uint32_t rpm; // in RPM
+    uint8_t errors; // percentage of invalid samples
+    int8_t temperature; // in degrees Celsius
+    int8_t voltage; // in 0.1V units
+    int8_t current; // in 0.1A units
+  } channels[4]; // up to 4 channels
+} __attribute__((packed));
+
+struct EspCmdDebug
+{
+  int16_t debug[DEBUG_VALUE_COUNT];
+} __attribute__((packed));
+
+struct EspCmdNames
+{
+  char names[1];
+} __attribute__((packed));
+
+struct EspCmdInputChannelConfig
+{
+  uint8_t map; // channel mapping
+  uint16_t min; // minimum range
+  uint16_t max; // maximum range
+  uint8_t fsMode; // failsafe mode
+  uint16_t fsValue; // failsafe value
+} __attribute__((packed));
+
+struct EspCmdInputChannelConfigRequest
+{
+  uint8_t channel;
+  EspCmdInputChannelConfig config;
+} __attribute__((packed));
+
+struct EspCmdInputChannelConfigResponse
+{
+  uint8_t count;
+  EspCmdInputChannelConfig configs[INPUT_CHANNELS]; // up to 16 channels
+} __attribute__((packed));
+
+struct EspCmdOutputConfig
+{
+  uint8_t protocol;
+  uint8_t async;
+  uint16_t rate;
+  uint16_t servoRate;
+  uint16_t minCommand;
+  uint16_t minThrottle;
+  uint16_t maxThrottle;
+  uint16_t digitalIdle;
+  uint8_t digitalTlm;
+  uint8_t motorPoles;
+  uint8_t motorLimit;
+  uint8_t throttleLimitType;
+  uint8_t throttleLimitPercent;
+} __attribute__((packed));
+
+struct EspCmdOutputChannelConfig
+{
+  uint16_t min; // minimum value
+  uint16_t neutral; // neutral value
+  uint16_t max; // maximum value
+  uint8_t servo; // servo or motor
+  uint8_t reverse; // normal or reversed
+} __attribute__((packed));
+
+struct EspCmdOutputChannelConfigRequest
+{
+  uint8_t channel;
+  EspCmdOutputChannelConfig config;
+} __attribute__((packed));
+
+struct EspCmdOutputChannelConfigResponse
+{
+  uint8_t count;
+  EspCmdOutputChannelConfig configs[OUTPUT_CHANNELS]; // up to 8 channels
+} __attribute__((packed));
+
+struct EspCmdOutputOverride
+{
+  uint8_t count;
+  uint16_t overrides[OUTPUT_CHANNELS];
+} __attribute__((packed));
+
+struct EspCmdLpfConfig {
+  int8_t type;
+  int16_t freq;
+} __attribute__((packed));
+
+struct EspCmdDynLpfConfig {
+  uint8_t type;
+  uint16_t minFreq;
+  uint16_t maxFreq;
+} __attribute__((packed));
+
+struct EspCmdNotchConfig {
+  uint8_t type;
+  uint16_t freq;
+  uint16_t bandwidth;
+} __attribute__((packed));
+
+struct EspCmdDynNotchConfig {
+  uint8_t count;
+  int8_t q;
+  int16_t minFreq;
+  int16_t maxFreq;
+} __attribute__((packed));
+
+struct EspCmdRpmNotchConfig {
+  uint8_t harmonics;
+  int8_t q;
+  uint16_t minFreq;
+} __attribute__((packed));
+
+struct EspCmdGyroConfig
+{
+  uint8_t align;
+  EspCmdLpfConfig lpf[3];
+  EspCmdDynNotchConfig dynNotch;
+  EspCmdRpmNotchConfig rpmNotch;
+  //EspCmdDynLpfConfig dynLpf;
+  //EspCmdNotchConfig notch[2];
+} __attribute__((packed));
+
+struct EspCmdAccelConfig
+{
+  EspCmdLpfConfig lpf;
+} __attribute__((packed));
+
+struct EspCmdSerialConfig
+{
+  uint32_t baud;
+  uint32_t func;
+} __attribute__((packed));
+
+struct EspCmdSerialConfigResponse
+{
+  uint8_t serialCount;
+  EspCmdSerialConfig serial[SERIAL_UART_COUNT];
+} __attribute__((packed));
+
+struct EspCmdVoltageConfig
+{
+  uint8_t count;
+  struct __attribute__((packed)) {
+    int8_t source;
+    uint16_t scale; // scale factor
+    int16_t cellWarning; // cell warning threshold
+  } items[1];
+} __attribute__((packed));
+
+struct EspCmdCurrentConfig
+{
+  uint8_t count;
+  struct __attribute__((packed)) {
+    int8_t source;
+    int16_t scale; // scale factor
+    int16_t offset; // offset value
+  } items[1];
+} __attribute__((packed));
+
+struct EspCmdPidConfig
+{
+  uint8_t index; // Pid index
+  uint8_t p; // P value
+  uint8_t i; // I value
+  uint8_t d; // D value
+  int16_t f; // F value
+} __attribute__((packed));
+
+struct EspCmdPidConfigResponse
+{
+  uint8_t pidCount;
+  EspCmdPidConfig config[FC_PID_ITEM_COUNT - FC_PID_ALT];
+} __attribute__((packed));
+
+struct EspCmdPidCommonConfig
+{
+  uint8_t loopSync; // loop sync mode
+  uint8_t tpaScale; // TPA scale
+  uint16_t tpaBreakpoint; // TPA breakpoint
+  uint8_t levelAngleLimit; // level angle limit
+  uint16_t levelRateLimit; // level rate limit
+  EspCmdLpfConfig levelPtermFilter; // level P-term filter
+  EspCmdLpfConfig yawFilter; // yaw filter
+  EspCmdLpfConfig dtermFilter[2]; // D-term filter
+  EspCmdNotchConfig dtermNotchFilter; // D-term notch filter
+  uint8_t itermRelax; // I-term relax mode
+  uint8_t itermRelaxType; // I-term relax type
+  uint8_t itermRelaxCutoff; // I-term relax cutoff
+  uint16_t itermLimit; // I-term limit
+} __attribute__((packed));
+
+struct EspCmdModesConfig
+{
+  uint8_t modeCount;
+  struct __attribute__((packed)) Mode {
+    uint8_t id; // mode ID
+    uint8_t ch; // channel
+    uint16_t min; // rate limit
+    uint16_t max; // angle limit
+  } modes[ACTUATOR_CONDITIONS]; // up to 8 modes
+} __attribute__((packed));
+
+struct EspCmdFailsafeConfig
+{
+  uint8_t delay; // failsafe delay
+} __attribute__((packed));
+
+struct EspCmdBlackboxConfig
+{
+  int8_t device; // blackbox device
+  uint8_t denom; // blackbox rate denominator
+  int8_t mode; // blackbox mode
+  int32_t fieldMask; // blackbox field mask
+  int8_t debugMode; // debug mode
+  uint8_t debugAxis; // debug axis
+} __attribute__((packed));
+
+struct EspCmdMixerConfig
+{
+  uint8_t type; // mixer type
+  uint8_t yawReverse; // yaw reverse
+  uint8_t sync; // mixer sync
+} __attribute__((packed));
+
+struct EspCmdScalerConfig
+{
+  uint8_t dimension; // scaler dimension
+  uint8_t channel; // scaler channel
+  int16_t minScale; // minimum scale
+  int16_t maxScale; // maximum scale
+} __attribute__((packed));
+
+struct EspCmdVtxConfig
+{
+  uint8_t power; // VTX power
+  uint8_t band; // VTX band
+  uint8_t channel; // VTX channel
+  uint8_t pitMode; // pit mode
+} __attribute__((packed));
+
+struct EspCmdGpsConfig
+{
+  uint8_t minSats; // minimum satellites
+  uint8_t setHomeOnce; // set home position once
+} __attribute__((packed));
+
+struct EspCmdBaroConfig
+{
+  EspCmdLpfConfig lpf; // barometer low-pass filter
+} __attribute__((packed));
+
+struct EspCmdMagConfig
+{
+  int8_t align; // magnetometer alignment
+  EspCmdLpfConfig lpf; // magnetometer low-pass filter
+} __attribute__((packed));
+
+struct EspCmdFeatureConfig
+{
+  uint32_t features; // feature flags
+} __attribute__((packed));
+
+struct EspCmdModelConfig
+{
+  uint8_t name[32]; // model name
+} __attribute__((packed));
+
+struct EspCmdAlignmentConfig
+{
+  uint8_t align[3]; // gyro alignment
+} __attribute__((packed));
+
+struct EspCmdFlashLogs
+{
+  uint32_t totalSize; // total flash size
+  uint32_t usedSize; // used flash size
+  struct __attribute__((packed)) {
+    uint32_t address;
+    uint32_t size;
+  } logs[16];
+} __attribute__((packed));
+
+struct EspCmdFlashReadRequest
+{
+  uint32_t address; // flash address to read from
+  uint16_t size; // size of data to read
+} __attribute__((packed));
+
+struct EspCmdFlashReadResponse
+{
+  uint32_t address; // flash address read from
+  uint16_t size; // size of data read
+  uint16_t flags; // format flags
+  //uint8_t data[1]; // data read from flash (variable length)
+} __attribute__((packed));
+
+enum EspCmdInputType: uint8_t
+{
+  RX_NONE = 0,
+  RX_SERIAL_IBUS = 1,
+  RX_SERIAL_SBUS,
+  RX_SERIAL_CRSF,
+  RX_ESPNOW = 0x10,
+  RX_PPM = 0x11,
+};
+
+struct EspCmdInputConfig
+{
+  uint8_t type;
+  uint8_t deadband;
+  uint8_t smoothing;
+  int16_t mid;
+  int16_t min;
+  int16_t max;
+} __attribute__((packed));
+
+struct EspCmdSensorConfig
+{
+  uint8_t loopSync;
+  uint8_t accelDev;
+  uint8_t baroDev;
+  uint8_t magDev;
+  int16_t alignment[3];
+};
+
+enum EspCmdPidTuningMode: uint8_t {
+  ESP_CMD_PID_TUNING_MODE_CALC = 0x01,
+  ESP_CMD_PID_TUNING_MODE_SAVE = 0x80,
+};
+
+struct EspCmdPidTuningItem
+{
+  uint8_t p; // P value
+  uint8_t i; // I value
+  uint8_t d; // D value
+  int16_t f; // F value
+} __attribute__((packed));
+
+
+struct EspCmdPidTuning
+{
+  uint8_t mode; // bits: 0=calc, 7=save
+  uint8_t rpGain; // roll/pitch gain
+  uint8_t rpStability; // roll/pitch stability
+  uint8_t rpAgility; // roll/pitch agility
+  uint8_t rpBalance; // roll/pitch balance
+  uint8_t yawGain; // yaw gain
+  uint8_t yawStability; // stability
+  EspCmdPidTuningItem pids[3];
+} __attribute__((packed));
+
+}
diff --git a/lib/Espfc/src/Connect/Msp.cpp b/lib/Espfc/src/Connect/Msp.cpp
index ea903751..d21c73b7 100644
--- a/lib/Espfc/src/Connect/Msp.cpp
+++ b/lib/Espfc/src/Connect/Msp.cpp
@@ -126,7 +126,7 @@ size_t MspResponse::serializeV1(uint8_t * buff, size_t len_max) const
   if(len + 6ul > len_max) return 0;
 
   buff[0] = '$';
-  buff[1] = 'M';
+  buff[1] = variant == MSP_ESP ? 'E' : 'M';
   buff[2] = result == -1 ? '!' : '>';
   buff[3] = len;
   buff[4] = cmd;
diff --git a/lib/Espfc/src/Connect/Msp.hpp b/lib/Espfc/src/Connect/Msp.hpp
index 39ac2633..3260f825 100644
--- a/lib/Espfc/src/Connect/Msp.hpp
+++ b/lib/Espfc/src/Connect/Msp.hpp
@@ -2,6 +2,8 @@
 
 #include <cstdint>
 #include <cstddef>
+#include <cstring>
+#include <type_traits>
 #include "Hal/Pgm.h"
 
 namespace Espfc {
@@ -30,12 +32,17 @@ enum MspState {
 
 enum MspType {
   MSP_TYPE_CMD,
-  MSP_TYPE_REPLY
+  MSP_TYPE_REPLY,
 };
 
 enum MspVersion {
   MSP_V1,
-  MSP_V2
+  MSP_V2,
+};
+
+enum MspVariant {
+  MSP_BF,
+  MSP_ESP,
 };
 
 struct MspHeaderV1 {
@@ -62,9 +69,18 @@ class MspMessage
   uint16_t readU16();
   uint32_t readU32();
 
+  template<typename T>
+  void readTo(T& value)
+  {
+    static_assert(std::is_trivially_copyable<T>::value, "Type must be trivially copyable");
+    std::memcpy(&value, &buffer[read], sizeof(T));
+    read += sizeof(T);
+  }
+
   MspState state;
   MspType dir;
   MspVersion version;
+  MspVariant variant;
   uint8_t flags;
   uint16_t cmd;
   uint16_t expected;
@@ -81,12 +97,19 @@ class MspResponse
   MspResponse();
 
   MspVersion version;
+  MspVariant variant;
   uint16_t cmd;
   int8_t  result;
   uint8_t direction;
   uint16_t len;
   uint8_t data[MSP_BUF_OUT_SIZE];
 
+  template<typename T>
+  void write(const T& v)
+  {
+    writeData(reinterpret_cast<const char*>(&v), sizeof(T));
+  }
+
   int remain() const;
   void advance(size_t size);
   void writeData(const char * v, int size);
diff --git a/lib/Espfc/src/Connect/MspParser.cpp b/lib/Espfc/src/Connect/MspParser.cpp
index 03ccdb8b..1339bb52 100644
--- a/lib/Espfc/src/Connect/MspParser.cpp
+++ b/lib/Espfc/src/Connect/MspParser.cpp
@@ -20,17 +20,26 @@ void MspParser::parse(char c, MspMessage& msg)
       msg.received = 0;
       msg.checksum = 0;
       msg.checksum2 = 0;
-      if(c == 'M')
-      {
-        msg.version = MSP_V1;
-        msg.state = MSP_STATE_HEADER_M;
-      }
-      else if(c == 'X')
+      switch(c)
       {
-        msg.version = MSP_V2;
-        msg.state = MSP_STATE_HEADER_X;
+        case 'M':
+          msg.version = MSP_V1;
+          msg.variant = MSP_BF;
+          msg.state = MSP_STATE_HEADER_M;
+          break;
+        case 'E':
+          msg.version = MSP_V1;
+          msg.variant = MSP_ESP;
+          msg.state = MSP_STATE_HEADER_M;
+          break;
+        case 'X':
+          msg.version = MSP_V2;
+          msg.variant = MSP_BF;
+          msg.state = MSP_STATE_HEADER_X;
+          break;
+        default:
+          msg.state = MSP_STATE_IDLE;
       }
-      else msg.state = MSP_STATE_IDLE;
       break;
 
     case MSP_STATE_HEADER_M:               // type '<','>','!'
diff --git a/lib/Espfc/src/Connect/MspProcessor.cpp b/lib/Espfc/src/Connect/MspProcessor.cpp
index 35787bfa..eb476770 100644
--- a/lib/Espfc/src/Connect/MspProcessor.cpp
+++ b/lib/Espfc/src/Connect/MspProcessor.cpp
@@ -1,5 +1,6 @@
 #include "Connect/MspProcessor.hpp"
 #include "Hardware.h"
+#include "EspProtocol.hpp"
 #include <platform.h>
 #include <algorithm>
 #include <limits>
@@ -155,9 +156,90 @@ constexpr uint8_t MSP_PASSTHROUGH_ESC_4WAY = 0xff;
 
 }
 
-namespace Espfc {
+namespace Espfc::Connect {
 
-namespace 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) {}
 
@@ -174,13 +256,1248 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD
 {
   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:
@@ -456,7 +1773,7 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD
       break;
 
     case MSP_DATAFLASH_SUMMARY:
-#ifdef USE_FLASHFS   
+#ifdef USE_FLASHFS
       {
         uint8_t flags = flashfsIsSupported() ? 2 : 0;
         flags |= flashfsIsReady() ? 1 : 0;
@@ -1694,5 +3011,3 @@ void MspProcessor::debugResponse(const MspResponse& r)
 }
 
 }
-
-}
diff --git a/lib/Espfc/src/Connect/MspProcessor.hpp b/lib/Espfc/src/Connect/MspProcessor.hpp
index d63eb118..b386d01b 100644
--- a/lib/Espfc/src/Connect/MspProcessor.hpp
+++ b/lib/Espfc/src/Connect/MspProcessor.hpp
@@ -27,6 +27,8 @@ class MspProcessor
   void debugResponse(const MspResponse& r);
 
 private:
+  void processCommandBF(MspMessage& m, MspResponse& r, Device::SerialDevice& s);
+  void processCommandESP(MspMessage& m, MspResponse& r, Device::SerialDevice& s);
   Model& _model;
   MspParser _parser;
   std::function<void(void)> _postCommand;
diff --git a/lib/Espfc/src/Control/Actuator.cpp b/lib/Espfc/src/Control/Actuator.cpp
index 3f346fba..9218ef25 100644
--- a/lib/Espfc/src/Control/Actuator.cpp
+++ b/lib/Espfc/src/Control/Actuator.cpp
@@ -114,6 +114,7 @@ void Actuator::updateModeMask()
     int16_t max = c->max; // * 25 + 900;
     size_t ch = c->ch;    // + AXIS_AUX_1;
     if(ch < AXIS_AUX_1 || ch >= AXIS_COUNT) continue; // invalid channel
+    if(c->id >= MODE_COUNT) continue; // invalid id
 
     int16_t val = _model.state.input.us[ch];
     if(val > min && val < max)
diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h
index 198af605..d76381e8 100644
--- a/lib/Espfc/src/ModelConfig.h
+++ b/lib/Espfc/src/ModelConfig.h
@@ -277,18 +277,16 @@ enum PinFunction {
   PIN_I2C_0_SCL,
   PIN_I2C_0_SDA,
 #endif
-#ifdef ESPFC_ADC_0
-  PIN_INPUT_ADC_0,
+#ifdef ESPFC_ADC_VBAT
+  PIN_INPUT_ADC_VBAT,
 #endif
-#ifdef ESPFC_ADC_1
-  PIN_INPUT_ADC_1,
+#ifdef ESPFC_ADC_IBAT
+  PIN_INPUT_ADC_IBAT,
 #endif
 #ifdef ESPFC_SPI_0
   PIN_SPI_0_SCK,
   PIN_SPI_0_MOSI,
   PIN_SPI_0_MISO,
-#endif
-#ifdef ESPFC_SPI_0
   PIN_SPI_CS0,
   PIN_SPI_CS1,
   PIN_SPI_CS2,
@@ -532,7 +530,7 @@ struct FailsafeConfig
 struct BlackboxConfig
 {
   int8_t dev = 0;
-  int16_t pDenom = 32; // 1k
+  int16_t pDenom = 2; // 1:2 of pid rate (32 for static 1k)
   int32_t fieldsMask = 0xffff;
   int8_t mode = 0;
 };
@@ -678,6 +676,17 @@ struct LedConfig
   int8_t type = 0;
 };
 
+struct PidTuningConfig
+{
+  uint8_t mode = 0; // 0: slider, 1: manual
+  uint8_t rpGain = 100; // roll/pitch gain
+  uint8_t rpStability = 100; // roll/pitch stability
+  uint8_t rpAgility = 100; // roll/pitch agility
+  uint8_t rpBalance = 100; // roll/pitch balance
+  uint8_t yawGain = 100; // yaw gain
+  uint8_t yawStability = 100; // stability
+} __attribute__((packed));
+
 // persistent data
 class ModelConfig
 {
@@ -757,11 +766,11 @@ class ModelConfig
       [PIN_I2C_0_SCL] = ESPFC_I2C_0_SCL,
       [PIN_I2C_0_SDA] = ESPFC_I2C_0_SDA,
 #endif
-#ifdef ESPFC_ADC_0
-      [PIN_INPUT_ADC_0] = ESPFC_ADC_0_PIN,
+#ifdef ESPFC_ADC_VBAT
+      [PIN_INPUT_ADC_VBAT] = ESPFC_ADC_VBAT_PIN,
 #endif
-#ifdef ESPFC_ADC_1
-      [PIN_INPUT_ADC_1] = ESPFC_ADC_1_PIN,
+#ifdef ESPFC_ADC_IBAT
+      [PIN_INPUT_ADC_IBAT] = ESPFC_ADC_IBAT_PIN,
 #endif
 #ifdef ESPFC_SPI_0
       [PIN_SPI_0_SCK] = ESPFC_SPI_0_SCK,
@@ -813,6 +822,8 @@ class ModelConfig
     int16_t boardAlignment[3] = {0, 0, 0};
     char modelName[MODEL_NAME_LEN + 1];
 
+    PidTuningConfig pidTuning;
+
     ModelConfig()
     {
       for(size_t i = 0; i < INPUT_CHANNELS; i++)
diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h
index db3bc254..ec936789 100644
--- a/lib/Espfc/src/ModelState.h
+++ b/lib/Espfc/src/ModelState.h
@@ -104,6 +104,7 @@ class BatteryState
     int8_t cells;
     int8_t samples;
     Utils::Timer timer;
+    float consumption; // in mAh
 };
 
 enum CalibrationState {
@@ -165,6 +166,7 @@ struct OutputState
   int16_t disarmed[OUTPUT_CHANNELS];
   bool saturated;
   OutputTelemetryState telemetry;
+  uint32_t overrideTimeout;
 };
 
 struct InputState
@@ -395,7 +397,7 @@ struct GpsAccuracy
   uint32_t vertical = 0; // mm (1e3)
   uint32_t speed = 0; // mm/s (1e3)
   uint32_t heading = 0; // deg * 1e5
-  uint32_t pDop = 0; // (1e2)
+  uint16_t pDop = 0; // (1e2)
 };
 
 struct GpsSatelite
@@ -438,6 +440,33 @@ struct GpsDateTime
   uint8_t minute; // 0-59
   uint8_t second; // 0-59
   uint16_t msec; // 0-999
+
+  inline bool isLeap(uint32_t y) const
+  {
+    return (y % 4 == 0 && (y % 100 != 0 || y % 400 == 0));
+  }
+
+  uint32_t toUnixTimestamp() const
+  {
+    static const uint16_t daysBeforeMonth[12] = { 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334 };
+    
+    const uint32_t y = year;
+    const uint32_t yearsSince1970 = y - 1970;
+    const uint32_t leaps = ((y - 1) / 4 - (y - 1) / 100 + (y - 1) / 400) 
+                          - ((1969) / 4 - (1969)  / 100 +  (1969) / 400);
+
+    uint32_t days = yearsSince1970 * 365 + leaps;
+    days += daysBeforeMonth[month - 1] + day - 1;
+    if (month > 2 && isLeap(y)) days += 1;
+
+    uint32_t seconds =
+      days * 86400UL +
+      hour * 3600UL +
+      minute * 60UL +
+      second;
+
+    return seconds;
+  }
 };
 
 static constexpr size_t SAT_MAX = 32u;
diff --git a/lib/Espfc/src/Output/Mixer.cpp b/lib/Espfc/src/Output/Mixer.cpp
index e0e8eb1d..d348d7b4 100644
--- a/lib/Espfc/src/Output/Mixer.cpp
+++ b/lib/Espfc/src/Output/Mixer.cpp
@@ -86,6 +86,7 @@ int FAST_CODE_ATTR Mixer::update()
   float outputs[OUTPUT_CHANNELS];
   const MixerConfig& mixer = _model.state.currentMixer;
 
+  overrideTimeout();
   readTelemetry();
   updateMixer(mixer, outputs);
   writeOutput(mixer, outputs);
@@ -280,6 +281,7 @@ void FAST_CODE_ATTR Mixer::writeOutput(const MixerConfig& mixer, float * out)
 
 void FAST_CODE_ATTR Mixer::readTelemetry()
 {
+  // TODO: https://github.com/betaflight/betaflight/pull/14490
   Utils::Stats::Measure mixerMeasure(_model.state.stats, COUNTER_MIXER_READ);
   if(!_model.config.output.dshotTelemetry || !_motor) return;
 
@@ -379,6 +381,19 @@ bool Mixer::_stop(void)
   return false;
 }
 
+void Mixer::overrideTimeout()
+{
+  if(_model.state.output.overrideTimeout == 0 || _model.state.output.overrideTimeout > millis()) return;
+
+  for (size_t i = 0; i < OUTPUT_CHANNELS; i++)
+  {
+    const OutputChannelConfig& och = _model.config.output.channel[i];
+    _model.state.output.disarmed[i] = och.servo ? och.neutral : _model.config.output.minCommand;
+  }
+  
+  _model.state.output.overrideTimeout = 0;
+}
+
 }
 
 }
diff --git a/lib/Espfc/src/Output/Mixer.h b/lib/Espfc/src/Output/Mixer.h
index 97099ff2..7a995c95 100644
--- a/lib/Espfc/src/Output/Mixer.h
+++ b/lib/Espfc/src/Output/Mixer.h
@@ -24,6 +24,8 @@ class Mixer
     bool inline _stop(void);
 
   private:
+    void overrideTimeout();
+
     Model& _model;
     EscDriver * _motor;
     EscDriver * _servo;
diff --git a/lib/Espfc/src/Sensor/VoltageSensor.cpp b/lib/Espfc/src/Sensor/VoltageSensor.cpp
index 4e5eb948..72383903 100644
--- a/lib/Espfc/src/Sensor/VoltageSensor.cpp
+++ b/lib/Espfc/src/Sensor/VoltageSensor.cpp
@@ -45,13 +45,13 @@ int VoltageSensor::update()
 
 int VoltageSensor::readVbat()
 {
-#ifdef ESPFC_ADC_0
-  if (_model.config.vbat.source != 1 || _model.config.pin[PIN_INPUT_ADC_0] == -1) return 0;
+#ifdef ESPFC_ADC_VBAT
+  if (_model.config.vbat.source != 1 || _model.config.pin[PIN_INPUT_ADC_VBAT] == -1) return 0;
   // wemos d1 mini has divider 3.2:1 (220k:100k)
   // additionaly I've used divider 5.7:1 (4k7:1k)
   // total should equals ~18.24:1, 73:4 resDiv:resMult should be ideal,
   // but ~52:1 is real, did I miss something?
-  _model.state.battery.rawVoltage = analogRead(_model.config.pin[PIN_INPUT_ADC_0]);
+  _model.state.battery.rawVoltage = analogRead(_model.config.pin[PIN_INPUT_ADC_VBAT]);
   float volts = _vFilterFast.update(_model.state.battery.rawVoltage * ESPFC_ADC_SCALE);
 
   volts *= _model.config.vbat.scale * 0.1f;
@@ -84,10 +84,10 @@ int VoltageSensor::readVbat()
 
 int VoltageSensor::readIbat()
 {
-#ifdef ESPFC_ADC_1
-  if (_model.config.ibat.source != 1 && _model.config.pin[PIN_INPUT_ADC_1] == -1) return 0;
+#ifdef ESPFC_ADC_IBAT
+  if (_model.config.ibat.source != 1 && _model.config.pin[PIN_INPUT_ADC_IBAT] == -1) return 0;
 
-  _model.state.battery.rawCurrent = analogRead(_model.config.pin[PIN_INPUT_ADC_1]);
+  _model.state.battery.rawCurrent = analogRead(_model.config.pin[PIN_INPUT_ADC_IBAT]);
   float volts = _iFilterFast.update(_model.state.battery.rawCurrent * ESPFC_ADC_SCALE);
   float milivolts = volts * 1000.0f;
 
diff --git a/lib/Espfc/src/SerialManager.cpp b/lib/Espfc/src/SerialManager.cpp
index f4805d64..fb1310f1 100644
--- a/lib/Espfc/src/SerialManager.cpp
+++ b/lib/Espfc/src/SerialManager.cpp
@@ -111,7 +111,7 @@ int SerialManager::begin()
       continue;
     }
 
-    port->begin(sdc);   
+    port->begin(sdc);
     _model.state.serial[i].stream = port;
 
     if(i == ESPFC_SERIAL_DEBUG_PORT)
diff --git a/lib/Espfc/src/Target/TargetESP32.h b/lib/Espfc/src/Target/TargetESP32.h
index 531b7c5d..ce78a4ba 100644
--- a/lib/Espfc/src/Target/TargetESP32.h
+++ b/lib/Espfc/src/Target/TargetESP32.h
@@ -66,11 +66,11 @@
 #define ESPFC_BUTTON_PIN 0
 #define ESPFC_LED_PIN 2
 
-#define ESPFC_ADC_0
-#define ESPFC_ADC_0_PIN 36
+#define ESPFC_ADC_VBAT
+#define ESPFC_ADC_VBAT_PIN 36
 
-#define ESPFC_ADC_1
-#define ESPFC_ADC_1_PIN 39
+#define ESPFC_ADC_IBAT
+#define ESPFC_ADC_IBAT_PIN 39
 
 #define ESPFC_ADC_SCALE (3.3f / 4096)
 
diff --git a/lib/Espfc/src/Target/TargetESP32c3.h b/lib/Espfc/src/Target/TargetESP32c3.h
index cd3ad6a1..555f52fe 100644
--- a/lib/Espfc/src/Target/TargetESP32c3.h
+++ b/lib/Espfc/src/Target/TargetESP32c3.h
@@ -63,11 +63,11 @@
 #define ESPFC_BUTTON_PIN -1
 #define ESPFC_LED_PIN -1
 
-#define ESPFC_ADC_0
-#define ESPFC_ADC_0_PIN 0
+#define ESPFC_ADC_VBAT
+#define ESPFC_ADC_VBAT_PIN 0
 
-#define ESPFC_ADC_1
-#define ESPFC_ADC_1_PIN 1
+#define ESPFC_ADC_IBAT
+#define ESPFC_ADC_IBAT_PIN 1
 
 #define ESPFC_ADC_SCALE (3.3f / 4096)
 
diff --git a/lib/Espfc/src/Target/TargetESP32s2.h b/lib/Espfc/src/Target/TargetESP32s2.h
index d780c824..34fb6d04 100644
--- a/lib/Espfc/src/Target/TargetESP32s2.h
+++ b/lib/Espfc/src/Target/TargetESP32s2.h
@@ -60,11 +60,11 @@
 #define ESPFC_BUTTON_PIN -1
 #define ESPFC_LED_PIN 15
 
-#define ESPFC_ADC_0
-#define ESPFC_ADC_0_PIN 1
+#define ESPFC_ADC_VBAT
+#define ESPFC_ADC_VBAT_PIN 0
 
-#define ESPFC_ADC_1
-#define ESPFC_ADC_1_PIN 4
+#define ESPFC_ADC_IBAT
+#define ESPFC_ADC_IBAT_PIN 4
 
 #define ESPFC_ADC_SCALE (3.3f / 4096)
 
diff --git a/lib/Espfc/src/Target/TargetESP32s3.h b/lib/Espfc/src/Target/TargetESP32s3.h
index 4abb4e82..3400ae58 100644
--- a/lib/Espfc/src/Target/TargetESP32s3.h
+++ b/lib/Espfc/src/Target/TargetESP32s3.h
@@ -75,11 +75,11 @@
 #define ESPFC_BUTTON_PIN -1
 #define ESPFC_LED_PIN -1
 
-#define ESPFC_ADC_0
-#define ESPFC_ADC_0_PIN 1
+#define ESPFC_ADC_VBAT
+#define ESPFC_ADC_VBAT_PIN 0
 
-#define ESPFC_ADC_1
-#define ESPFC_ADC_1_PIN 4
+#define ESPFC_ADC_IBAT
+#define ESPFC_ADC_IBAT_PIN 4
 
 #define ESPFC_ADC_SCALE (3.3f / 4096)
 
diff --git a/lib/Espfc/src/Target/TargetESP8266.h b/lib/Espfc/src/Target/TargetESP8266.h
index 7f50d25e..e90a3929 100644
--- a/lib/Espfc/src/Target/TargetESP8266.h
+++ b/lib/Espfc/src/Target/TargetESP8266.h
@@ -45,8 +45,8 @@
 #define ESPFC_BUTTON_PIN -1
 #define ESPFC_LED_PIN -1
 
-#define ESPFC_ADC_0
-#define ESPFC_ADC_0_PIN 17   // A0
+#define ESPFC_ADC_VBAT
+#define ESPFC_ADC_VBAT_PIN 17   // A0
 
 #define ESPFC_ADC_SCALE (1.0f / 1024)
 
@@ -147,6 +147,11 @@ inline uint32_t targetFreeHeap()
   return ESP.getFreeHeap();
 }
 
+inline uint32_t targetTotalHeap()
+{
+  return ESP.getMaxFreeBlockSize();
+}
+
 /*
 //#include "user_interface.h"
 const rst_info * resetInfo = system_get_rst_info();
diff --git a/lib/Espfc/src/Target/TargetEsp32Common.h b/lib/Espfc/src/Target/TargetEsp32Common.h
index bf93de41..769a4420 100644
--- a/lib/Espfc/src/Target/TargetEsp32Common.h
+++ b/lib/Espfc/src/Target/TargetEsp32Common.h
@@ -103,4 +103,9 @@ inline uint32_t targetFreeHeap()
   return ESP.getFreeHeap();
 }
 
+inline uint32_t targetTotalHeap()
+{
+  return ESP.getHeapSize();
+}
+
 };
diff --git a/lib/Espfc/src/Target/TargetRP2040.h b/lib/Espfc/src/Target/TargetRP2040.h
index a5bba5ae..cd16b321 100644
--- a/lib/Espfc/src/Target/TargetRP2040.h
+++ b/lib/Espfc/src/Target/TargetRP2040.h
@@ -59,11 +59,11 @@
 #define ESPFC_LED_PIN -1
 
 
-#define ESPFC_ADC_0
-#define ESPFC_ADC_0_PIN 26
+#define ESPFC_ADC_VBAT
+#define ESPFC_ADC_VBAT_PIN 26
 
-#define ESPFC_ADC_1
-#define ESPFC_ADC_1_PIN 27
+#define ESPFC_ADC_IBAT
+#define ESPFC_ADC_IBAT_PIN 27
 
 #define ESPFC_ADC_SCALE (3.3f / 4096)
 
@@ -184,4 +184,9 @@ inline uint32_t targetFreeHeap()
   return rp2040.getFreeHeap();
 }
 
+inline uint32_t targetTotalHeap()
+{
+  return rp2040.getTotalHeap();
+}
+
 };
\ No newline at end of file
diff --git a/lib/Espfc/src/Target/TargetUnitTest.h b/lib/Espfc/src/Target/TargetUnitTest.h
index 4cb6254c..f664435e 100644
--- a/lib/Espfc/src/Target/TargetUnitTest.h
+++ b/lib/Espfc/src/Target/TargetUnitTest.h
@@ -49,6 +49,11 @@ inline uint32_t targetCpuFreq()
 }
 
 inline uint32_t targetFreeHeap()
+{
+  return 1;
+}
+
+inline uint32_t targetTotalHeap()
 {
   return 1;
 }
\ No newline at end of file
diff --git a/lib/Espfc/src/Utils/Math.hpp b/lib/Espfc/src/Utils/Math.hpp
index bd6fb1f6..b0d602da 100644
--- a/lib/Espfc/src/Utils/Math.hpp
+++ b/lib/Espfc/src/Utils/Math.hpp
@@ -4,6 +4,7 @@
 #include <cstddef>
 #include <cmath>
 #include <algorithm>
+#include <limits>
 
 namespace Espfc {
 
@@ -53,6 +54,34 @@ class Peak
     return value;
   }
 
+  // saturating cast
+  template <typename T, typename S>
+  constexpr T scast(S value)
+  {
+    static_assert(std::is_integral_v<T>, "Target must be an integral type");
+    static_assert(std::is_integral_v<S>, "Source must be an integral type");
+
+    if constexpr (sizeof(T) >= sizeof(S))
+    {
+      return static_cast<T>(value);
+    }
+
+    constexpr auto min_val = std::numeric_limits<T>::min();
+    constexpr auto max_val = std::numeric_limits<T>::max();
+
+    if (value < static_cast<S>(min_val)) return min_val;
+    if (value > static_cast<S>(max_val)) return max_val;
+
+    return static_cast<T>(value);
+  }
+
+  // float saturating cast
+  template<typename T>
+  T fcast(const float value)
+  {
+    return scast<T>(lrintf(value));
+  }
+
   inline int alignToClock(uint32_t clock, uint32_t maxFreq)
   {
     uint32_t result = clock;
diff --git a/lib/betaflight/src/platform.c b/lib/betaflight/src/platform.c
index 553d619d..3ae8cdc9 100644
--- a/lib/betaflight/src/platform.c
+++ b/lib/betaflight/src/platform.c
@@ -6,6 +6,7 @@ const char * const shortGitRevision = STR(ESPFC_REVISION);
 const char * const buildTime = __TIME__;
 const char * const buildDate = __DATE__;
 const char * flightControllerIdentifier = "BTFL";
+//const char * flightControllerIdentifier = "ESPF";
 const char * boardIdentifier = "ESPF";
 
 PG_RESET_TEMPLATE_DEF(serialConfig_t, serialConfig);
diff --git a/lib/betaflight/src/platform.h b/lib/betaflight/src/platform.h
index 92c91f47..74b34e07 100644
--- a/lib/betaflight/src/platform.h
+++ b/lib/betaflight/src/platform.h
@@ -47,7 +47,7 @@
 #endif
 
 #ifndef ESPFC_VERSION
-#define ESPFC_VERSION v0.0.0
+#define ESPFC_VERSION v0.3.0-dev
 #endif
 
 #define MAX_SUPPORTED_MOTORS 8
diff --git a/test/test_math/test_math.cpp b/test/test_math/test_math.cpp
index 712352a6..6a161615 100644
--- a/test/test_math/test_math.cpp
+++ b/test/test_math/test_math.cpp
@@ -41,6 +41,94 @@ void test_math_map3()
   TEST_ASSERT_FLOAT_WITHIN(1.f, -500.f, Utils::map3(-50.0f, -100.0f, 0.0f, 100.0f, -1000.0f, 0.0f, 1000.0f));
 }
 
+void test_math_scast()
+{
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::scast<int8_t>(0));
+  TEST_ASSERT_EQUAL_INT32(   127, Utils::scast<int8_t>(127));
+  TEST_ASSERT_EQUAL_INT32(   127, Utils::scast<int8_t>(128));
+  TEST_ASSERT_EQUAL_INT32(   127, Utils::scast<int8_t>(1000));
+  TEST_ASSERT_EQUAL_INT32(    -1, Utils::scast<int8_t>(-1));
+  TEST_ASSERT_EQUAL_INT32(  -127, Utils::scast<int8_t>(-127));
+  TEST_ASSERT_EQUAL_INT32(  -128, Utils::scast<int8_t>(-128));
+  TEST_ASSERT_EQUAL_INT32(  -128, Utils::scast<int8_t>(-129));
+  TEST_ASSERT_EQUAL_INT32(  -128, Utils::scast<int8_t>(-1000));
+
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::scast<uint8_t>(0));
+  TEST_ASSERT_EQUAL_INT32(   127, Utils::scast<uint8_t>(127));
+  TEST_ASSERT_EQUAL_INT32(   128, Utils::scast<uint8_t>(128));
+  TEST_ASSERT_EQUAL_INT32(   255, Utils::scast<uint8_t>(1000));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::scast<uint8_t>(-1));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::scast<uint8_t>(-127));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::scast<uint8_t>(-128));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::scast<uint8_t>(-129));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::scast<uint8_t>(-1000));
+
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::scast<int16_t>(0));
+  TEST_ASSERT_EQUAL_INT32( 32000, Utils::scast<int16_t>(32000));
+  TEST_ASSERT_EQUAL_INT32( 32767, Utils::scast<int16_t>(32767));
+  TEST_ASSERT_EQUAL_INT32( 32767, Utils::scast<int16_t>(32768));
+  TEST_ASSERT_EQUAL_INT32( 32767, Utils::scast<int16_t>(32769));
+  TEST_ASSERT_EQUAL_INT32( 32767, Utils::scast<int16_t>(40000));
+  TEST_ASSERT_EQUAL_INT32(    -1, Utils::scast<int16_t>(-1));
+  TEST_ASSERT_EQUAL_INT32(-32767, Utils::scast<int16_t>(-32767));
+  TEST_ASSERT_EQUAL_INT32(-32768, Utils::scast<int16_t>(-32768));
+  TEST_ASSERT_EQUAL_INT32(-32768, Utils::scast<int16_t>(-32769));
+  TEST_ASSERT_EQUAL_INT32(-32768, Utils::scast<int16_t>(-50000));
+
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::scast<uint16_t>(0));
+  TEST_ASSERT_EQUAL_INT32(   127, Utils::scast<uint16_t>(127));
+  TEST_ASSERT_EQUAL_INT32( 32767, Utils::scast<uint16_t>(32767));
+  TEST_ASSERT_EQUAL_INT32( 32768, Utils::scast<uint16_t>(32768));
+  TEST_ASSERT_EQUAL_INT32( 65535, Utils::scast<uint16_t>(65535));
+  TEST_ASSERT_EQUAL_INT32( 65535, Utils::scast<uint16_t>(65536));
+  TEST_ASSERT_EQUAL_INT32( 65535, Utils::scast<uint16_t>(100000));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::scast<uint16_t>(-1));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::scast<uint16_t>(-127));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::scast<uint16_t>(-128));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::scast<uint16_t>(-129));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::scast<uint16_t>(-1000));
+}
+
+void test_math_fcast()
+{
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::fcast<int8_t>(0.0f));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::fcast<int8_t>(0.1f));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::fcast<int8_t>(0.5f));
+  TEST_ASSERT_EQUAL_INT32(     1, Utils::fcast<int8_t>(0.501f));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::fcast<int8_t>(-0.5f));
+  TEST_ASSERT_EQUAL_INT32(    -1, Utils::fcast<int8_t>(-0.51f));
+  TEST_ASSERT_EQUAL_INT32(   127, Utils::fcast<int8_t>(127.0f));
+  TEST_ASSERT_EQUAL_INT32(  -128, Utils::fcast<int8_t>(-128.0f));
+  TEST_ASSERT_EQUAL_INT32(   127, Utils::fcast<int8_t>(32767.0f));
+  TEST_ASSERT_EQUAL_INT32(  -128, Utils::fcast<int8_t>(-32768.0f));
+  TEST_ASSERT_EQUAL_INT32(   127, Utils::fcast<int8_t>(50000.0f));
+  TEST_ASSERT_EQUAL_INT32(  -128, Utils::fcast<int8_t>(-50000.0f));
+
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::fcast<uint8_t>(0.0f));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::fcast<uint8_t>(0.5f));
+  TEST_ASSERT_EQUAL_INT32(     1, Utils::fcast<uint8_t>(0.501f));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::fcast<uint8_t>(-0.5f));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::fcast<uint8_t>(-0.51f));
+  TEST_ASSERT_EQUAL_INT32(   127, Utils::fcast<uint8_t>(127.0f));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::fcast<uint8_t>(-128.0f));
+  TEST_ASSERT_EQUAL_INT32(   255, Utils::fcast<uint8_t>(32767.0f));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::fcast<uint8_t>(-32768.0f));
+  TEST_ASSERT_EQUAL_INT32(   255, Utils::fcast<uint8_t>(50000.0f));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::fcast<uint8_t>(-50000.0f));
+
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::fcast<int16_t>(0.0f));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::fcast<int16_t>(0.5f));
+  TEST_ASSERT_EQUAL_INT32(     1, Utils::fcast<int16_t>(0.501f));
+  TEST_ASSERT_EQUAL_INT32(     0, Utils::fcast<int16_t>(-0.5f));
+  TEST_ASSERT_EQUAL_INT32(    -1, Utils::fcast<int16_t>(-0.6f));
+  TEST_ASSERT_EQUAL_INT32(   127, Utils::fcast<int16_t>(127.0f));
+  TEST_ASSERT_EQUAL_INT32(  -128, Utils::fcast<int16_t>(-128.0f));
+  TEST_ASSERT_EQUAL_INT32( 32767, Utils::fcast<int16_t>(32767.0f));
+  TEST_ASSERT_EQUAL_INT32(-32768, Utils::fcast<int16_t>(-32768.0f));
+  TEST_ASSERT_EQUAL_INT32( 32767, Utils::fcast<int16_t>(50000.0f));
+  TEST_ASSERT_EQUAL_INT32(-32768, Utils::fcast<int16_t>(-50000.0f));
+}
+
 void test_math_baro_altitude()
 {
   TEST_ASSERT_FLOAT_WITHIN(0.1f,   0.0f, Utils::toAltitude(101325.f)); // sea level
@@ -1307,6 +1395,8 @@ int main(int argc, char **argv)
   RUN_TEST(test_math_map);
   RUN_TEST(test_math_map3);
   RUN_TEST(test_math_deadband);
+  RUN_TEST(test_math_scast);
+  RUN_TEST(test_math_fcast);
 
   RUN_TEST(test_math_bit);
   RUN_TEST(test_math_bitmask);