src/main/msp/msp.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24 #include <math.h>
  25 #include <stdlib.h>
  26 #include <limits.h>
  27 #include <ctype.h>
  28
  29 #include "platform.h"
  30
  31 #include "blackbox/blackbox.h"
  32 #include "blackbox/blackbox_io.h"
  33
  34 #include "build/build_config.h"
  35 #include "build/debug.h"
  36 #include "build/version.h"
  37
  38 #include "cli/cli.h"
  39
  40 #include "common/axis.h"
  41 #include "common/bitarray.h"
  42 #include "common/color.h"
  43 #include "common/huffman.h"
  44 #include "common/maths.h"
  45 #include "common/streambuf.h"
  46 #include "common/utils.h"
  47
  48 #include "config/config.h"
  49 #include "config/config_eeprom.h"
  50 #include "config/feature.h"
  51 #include "config/simplified_tuning.h"
  52
  53 #include "drivers/accgyro/accgyro.h"
  54 #include "drivers/bus_i2c.h"
  55 #include "drivers/bus_spi.h"
  56 #include "drivers/camera_control.h"
  57 #include "drivers/compass/compass.h"
  58 #include "drivers/display.h"
  59 #include "drivers/dshot.h"
  60 #include "drivers/dshot_command.h"
  61 #include "drivers/flash.h"
  62 #include "drivers/io.h"
  63 #include "drivers/motor.h"
  64 #include "drivers/osd.h"
  65 #include "drivers/pwm_output.h"
  66 #include "drivers/sdcard.h"
  67 #include "drivers/serial.h"
  68 #include "drivers/serial_escserial.h"
  69 #include "drivers/system.h"
  70 #include "drivers/transponder_ir.h"
  71 #include "drivers/usb_msc.h"
  72 #include "drivers/vtx_common.h"
  73 #include "drivers/vtx_table.h"
  74
  75 #include "fc/board_info.h"
  76 #include "fc/controlrate_profile.h"
  77 #include "fc/core.h"
  78 #include "fc/dispatch.h"
  79 #include "fc/rc.h"
  80 #include "fc/rc_adjustments.h"
  81 #include "fc/rc_controls.h"
  82 #include "fc/rc_modes.h"
  83 #include "fc/runtime_config.h"
  84
  85 #include "flight/failsafe.h"
  86 #include "flight/gps_rescue.h"
  87 #include "flight/imu.h"
  88 #include "flight/mixer.h"
  89 #include "flight/pid.h"
  90 #include "flight/pid_init.h"
  91 #include "flight/position.h"
  92 #include "flight/rpm_filter.h"
  93 #include "flight/servos.h"
  94
  95 #include "io/asyncfatfs/asyncfatfs.h"
  96 #include "io/beeper.h"
  97 #include "io/flashfs.h"
  98 #include "io/gimbal.h"
  99 #include "io/gps.h"
 100 #include "io/ledstrip.h"
 101 #include "io/serial.h"
 102 #include "io/serial_4way.h"
 103 #include "io/servos.h"
 104 #include "io/transponder_ir.h"
 105 #include "io/usb_msc.h"
 106 #include "io/vtx_control.h"
 107 #include "io/vtx.h"
 108 #include "io/vtx_msp.h"
 109
 110 #include "msp/msp_box.h"
 111 #include "msp/msp_protocol.h"
 112 #include "msp/msp_protocol_v2_betaflight.h"
 113 #include "msp/msp_protocol_v2_common.h"
 114 #include "msp/msp_serial.h"
 115
 116 #include "osd/osd.h"
 117 #include "osd/osd_elements.h"
 118 #include "osd/osd_warnings.h"
 119
 120 #include "pg/beeper.h"
 121 #include "pg/board.h"
 122 #include "pg/dyn_notch.h"
 123 #include "pg/gyrodev.h"
 124 #include "pg/motor.h"
 125 #include "pg/rx.h"
 126 #include "pg/rx_spi.h"
 127 #ifdef USE_RX_EXPRESSLRS
 128 #include "pg/rx_spi_expresslrs.h"
 129 #endif
 130 #include "pg/usb.h"
 131 #include "pg/vcd.h"
 132 #include "pg/vtx_table.h"
 133
 134 #include "rx/rx.h"
 135 #include "rx/rx_bind.h"
 136 #include "rx/msp.h"
 137
 138 #include "scheduler/scheduler.h"
 139
 140 #include "sensors/acceleration.h"
 141 #include "sensors/barometer.h"
 142 #include "sensors/battery.h"
 143 #include "sensors/boardalignment.h"
 144 #include "sensors/compass.h"
 145 #include "sensors/gyro.h"
 146 #include "sensors/gyro_init.h"
 147 #include "sensors/rangefinder.h"
 148
 149 #include "telemetry/msp_shared.h"
 150 #include "telemetry/telemetry.h"
 151
 152 #ifdef USE_HARDWARE_REVISION_DETECTION
 153 #include "hardware_revision.h"
 154 #endif
 155
 156 #include "msp.h"
 157
 158
 159 static const char * const flightControllerIdentifier = FC_FIRMWARE_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
 160
 161 enum {
 162     MSP_REBOOT_FIRMWARE = 0,
 163     MSP_REBOOT_BOOTLOADER_ROM,
 164     MSP_REBOOT_MSC,
 165     MSP_REBOOT_MSC_UTC,
 166     MSP_REBOOT_BOOTLOADER_FLASH,
 167     MSP_REBOOT_COUNT,
 168 };
 169
 170 static uint8_t rebootMode;
 171
 172 typedef enum {
 173     MSP_SDCARD_STATE_NOT_PRESENT = 0,
 174     MSP_SDCARD_STATE_FATAL       = 1,
 175     MSP_SDCARD_STATE_CARD_INIT   = 2,
 176     MSP_SDCARD_STATE_FS_INIT     = 3,
 177     MSP_SDCARD_STATE_READY       = 4
 178 } mspSDCardState_e;
 179
 180 typedef enum {
 181     MSP_SDCARD_FLAG_SUPPORTED   = 1
 182 } mspSDCardFlags_e;
 183
 184 typedef enum {
 185     MSP_FLASHFS_FLAG_READY       = 1,
 186     MSP_FLASHFS_FLAG_SUPPORTED  = 2
 187 } mspFlashFsFlags_e;
 188
 189 typedef enum {
 190     MSP_PASSTHROUGH_ESC_SIMONK = PROTOCOL_SIMONK,
 191     MSP_PASSTHROUGH_ESC_BLHELI = PROTOCOL_BLHELI,
 192     MSP_PASSTHROUGH_ESC_KISS = PROTOCOL_KISS,
 193     MSP_PASSTHROUGH_ESC_KISSALL = PROTOCOL_KISSALL,
 194     MSP_PASSTHROUGH_ESC_CASTLE = PROTOCOL_CASTLE,
 195
 196     MSP_PASSTHROUGH_SERIAL_ID = 0xFD,
 197     MSP_PASSTHROUGH_SERIAL_FUNCTION_ID = 0xFE,
 198
 199     MSP_PASSTHROUGH_ESC_4WAY = 0xFF,
 200 } mspPassthroughType_e;
 201
 202 #define RATEPROFILE_MASK (1 << 7)
 203
 204 #define RTC_NOT_SUPPORTED 0xff
 205
 206 typedef enum {
 207     DEFAULTS_TYPE_BASE = 0,
 208     DEFAULTS_TYPE_CUSTOM,
 209 } defaultsType_e;
 210
 211 #ifdef USE_VTX_TABLE
 212 static bool vtxTableNeedsInit = false;
 213 #endif
 214
 215 static int mspDescriptor = 0;
 216
 217 mspDescriptor_t mspDescriptorAlloc(void)
 218 {
 219     return (mspDescriptor_t)mspDescriptor++;
 220 }
 221
 222 static uint32_t mspArmingDisableFlags = 0;
 223
 224 static void mspArmingDisableByDescriptor(mspDescriptor_t desc)
 225 {
 226     mspArmingDisableFlags |= (1 << desc);
 227 }
 228
 229 static void mspArmingEnableByDescriptor(mspDescriptor_t desc)
 230 {
 231     mspArmingDisableFlags &= ~(1 << desc);
 232 }
 233
 234 static bool mspIsMspArmingEnabled(void)
 235 {
 236     return mspArmingDisableFlags == 0;
 237 }
 238
 239 #define MSP_PASSTHROUGH_ESC_4WAY 0xff
 240
 241 static uint8_t mspPassthroughMode;
 242 static uint8_t mspPassthroughArgument;
 243
 244 #if defined(USE_ESCSERIAL) && defined(USE_SERIAL_4WAY_BLHELI_INTERFACE)
 245 static void mspEscPassthroughFn(serialPort_t *serialPort)
 246 {
 247     escEnablePassthrough(serialPort, &motorConfig()->dev, mspPassthroughArgument, mspPassthroughMode);
 248 }
 249 #endif
 250
 251 static serialPort_t *mspFindPassthroughSerialPort(void)
 252 {
 253     serialPortUsage_t *portUsage = NULL;
 254
 255     switch (mspPassthroughMode) {
 256     case MSP_PASSTHROUGH_SERIAL_ID:
 257     {
 258         portUsage = findSerialPortUsageByIdentifier(mspPassthroughArgument);
 259         break;
 260     }
 261     case MSP_PASSTHROUGH_SERIAL_FUNCTION_ID:
 262     {
 263         const serialPortConfig_t *portConfig = findSerialPortConfig(1 << mspPassthroughArgument);
 264         if (portConfig) {
 265             portUsage = findSerialPortUsageByIdentifier(portConfig->identifier);
 266         }
 267         break;
 268     }
 269     }
 270     return portUsage ? portUsage->serialPort : NULL;
 271 }
 272
 273 static void mspSerialPassthroughFn(serialPort_t *serialPort)
 274 {
 275     serialPort_t *passthroughPort = mspFindPassthroughSerialPort();
 276     if (passthroughPort && serialPort) {
 277         serialPassthrough(passthroughPort, serialPort, NULL, NULL);
 278     }
 279 }
 280
 281 static void mspFcSetPassthroughCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
 282 {
 283     const unsigned int dataSize = sbufBytesRemaining(src);
 284     if (dataSize == 0) {
 285         // Legacy format
 286         mspPassthroughMode = MSP_PASSTHROUGH_ESC_4WAY;
 287     } else {
 288         mspPassthroughMode = sbufReadU8(src);
 289         mspPassthroughArgument = sbufReadU8(src);
 290     }
 291
 292     switch (mspPassthroughMode) {
 293     case MSP_PASSTHROUGH_SERIAL_ID:
 294     case MSP_PASSTHROUGH_SERIAL_FUNCTION_ID:
 295         if (mspFindPassthroughSerialPort()) {
 296             if (mspPostProcessFn) {
 297                 *mspPostProcessFn = mspSerialPassthroughFn;
 298             }
 299             sbufWriteU8(dst, 1);
 300         } else {
 301             sbufWriteU8(dst, 0);
 302         }
 303         break;
 304 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
 305     case MSP_PASSTHROUGH_ESC_4WAY:
 306         // get channel number
 307         // switch all motor lines HI
 308         // reply with the count of ESC found
 309         sbufWriteU8(dst, esc4wayInit());
 310
 311         if (mspPostProcessFn) {
 312             *mspPostProcessFn = esc4wayProcess;
 313         }
 314         break;
 315
 316 #ifdef USE_ESCSERIAL
 317     case MSP_PASSTHROUGH_ESC_SIMONK:
 318     case MSP_PASSTHROUGH_ESC_BLHELI:
 319     case MSP_PASSTHROUGH_ESC_KISS:
 320     case MSP_PASSTHROUGH_ESC_KISSALL:
 321     case MSP_PASSTHROUGH_ESC_CASTLE:
 322         if (mspPassthroughArgument < getMotorCount() || (mspPassthroughMode == MSP_PASSTHROUGH_ESC_KISS && mspPassthroughArgument == ALL_MOTORS)) {
 323             sbufWriteU8(dst, 1);
 324
 325             if (mspPostProcessFn) {
 326                 *mspPostProcessFn = mspEscPassthroughFn;
 327             }
 328
 329             break;
 330         }
 331         FALLTHROUGH;
 332 #endif // USE_ESCSERIAL
 333 #endif // USE_SERIAL_4WAY_BLHELI_INTERFACE
 334     default:
 335         sbufWriteU8(dst, 0);
 336     }
 337 }
 338
 339 // TODO: Remove the pragma once this is called from unconditional code
 340 #pragma GCC diagnostic ignored "-Wunused-function"
 341 static void configRebootUpdateCheckU8(uint8_t *parm, uint8_t value)
 342 {
 343     if (*parm != value) {
 344         setRebootRequired();
 345     }
 346     *parm = value;
 347 }
 348 #pragma GCC diagnostic pop
 349
 350 static void mspRebootFn(serialPort_t *serialPort)
 351 {
 352     UNUSED(serialPort);
 353
 354     motorShutdown();
 355
 356     switch (rebootMode) {
 357     case MSP_REBOOT_FIRMWARE:
 358         systemReset();
 359
 360         break;
 361     case MSP_REBOOT_BOOTLOADER_ROM:
 362         systemResetToBootloader(BOOTLOADER_REQUEST_ROM);
 363
 364         break;
 365 #if defined(USE_USB_MSC)
 366     case MSP_REBOOT_MSC:
 367     case MSP_REBOOT_MSC_UTC: {
 368 #ifdef USE_RTC_TIME
 369         const int16_t timezoneOffsetMinutes = (rebootMode == MSP_REBOOT_MSC) ? timeConfig()->tz_offsetMinutes : 0;
 370         systemResetToMsc(timezoneOffsetMinutes);
 371 #else
 372         systemResetToMsc(0);
 373 #endif
 374         }
 375         break;
 376 #endif
 377 #if defined(USE_FLASH_BOOT_LOADER)
 378     case MSP_REBOOT_BOOTLOADER_FLASH:
 379         systemResetToBootloader(BOOTLOADER_REQUEST_FLASH);
 380
 381         break;
 382 #endif
 383     default:
 384
 385         return;
 386     }
 387
 388     // control should never return here.
 389     while (true) ;
 390 }
 391
 392 #define MSP_DISPATCH_DELAY_US 1000000
 393
 394 void mspReboot(dispatchEntry_t* self)
 395 {
 396     UNUSED(self);
 397
 398     if (ARMING_FLAG(ARMED)) {
 399         return;
 400     }
 401
 402     mspRebootFn(NULL);
 403 }
 404
 405 dispatchEntry_t mspRebootEntry =
 406 {
 407     mspReboot, 0, NULL, false
 408 };
 409
 410 void writeReadEeprom(dispatchEntry_t* self)
 411 {
 412     UNUSED(self);
 413
 414     if (ARMING_FLAG(ARMED)) {
 415         return;
 416     }
 417
 418     writeEEPROM();
 419     readEEPROM();
 420
 421 #ifdef USE_VTX_TABLE
 422     if (vtxTableNeedsInit) {
 423         vtxTableNeedsInit = false;
 424         vtxTableInit();  // Reinitialize and refresh the in-memory copies
 425     }
 426 #endif
 427 }
 428
 429 dispatchEntry_t writeReadEepromEntry =
 430 {
 431     writeReadEeprom, 0, NULL, false
 432 };
 433
 434 static void serializeSDCardSummaryReply(sbuf_t *dst)
 435 {
 436     uint8_t flags = 0;
 437     uint8_t state = 0;
 438     uint8_t lastError = 0;
 439     uint32_t freeSpace = 0;
 440     uint32_t totalSpace = 0;
 441
 442 #if defined(USE_SDCARD)
 443     if (sdcardConfig()->mode != SDCARD_MODE_NONE) {
 444         flags = MSP_SDCARD_FLAG_SUPPORTED;
 445
 446         // Merge the card and filesystem states together
 447         if (!sdcard_isInserted()) {
 448             state = MSP_SDCARD_STATE_NOT_PRESENT;
 449         } else if (!sdcard_isFunctional()) {
 450             state = MSP_SDCARD_STATE_FATAL;
 451         } else {
 452             switch (afatfs_getFilesystemState()) {
 453             case AFATFS_FILESYSTEM_STATE_READY:
 454                 state = MSP_SDCARD_STATE_READY;
 455                 break;
 456
 457             case AFATFS_FILESYSTEM_STATE_INITIALIZATION:
 458              if (sdcard_isInitialized()) {
 459                  state = MSP_SDCARD_STATE_FS_INIT;
 460              } else {
 461                  state = MSP_SDCARD_STATE_CARD_INIT;
 462              }
 463              break;
 464
 465             case AFATFS_FILESYSTEM_STATE_FATAL:
 466             case AFATFS_FILESYSTEM_STATE_UNKNOWN:
 467             default:
 468                 state = MSP_SDCARD_STATE_FATAL;
 469                 break;
 470             }
 471         }
 472
 473         lastError = afatfs_getLastError();
 474         // Write free space and total space in kilobytes
 475         if (state == MSP_SDCARD_STATE_READY) {
 476             freeSpace = afatfs_getContiguousFreeSpace() / 1024;
 477             totalSpace = sdcard_getMetadata()->numBlocks / 2;
 478         }
 479     }
 480 #endif
 481
 482     sbufWriteU8(dst, flags);
 483     sbufWriteU8(dst, state);
 484     sbufWriteU8(dst, lastError);
 485     sbufWriteU32(dst, freeSpace);
 486     sbufWriteU32(dst, totalSpace);
 487 }
 488
 489 static void serializeDataflashSummaryReply(sbuf_t *dst)
 490 {
 491 #ifdef USE_FLASHFS
 492     if (flashfsIsSupported()) {
 493         uint8_t flags = MSP_FLASHFS_FLAG_SUPPORTED;
 494         flags |= (flashfsIsReady() ? MSP_FLASHFS_FLAG_READY : 0);
 495
 496         const flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_FLASHFS);
 497
 498         sbufWriteU8(dst, flags);
 499         sbufWriteU32(dst, FLASH_PARTITION_SECTOR_COUNT(flashPartition));
 500         sbufWriteU32(dst, flashfsGetSize());
 501         sbufWriteU32(dst, flashfsGetOffset()); // Effectively the current number of bytes stored on the volume
 502     } else
 503 #endif
 504
 505     // FlashFS is not configured or valid device is not detected
 506     {
 507         sbufWriteU8(dst, 0);
 508         sbufWriteU32(dst, 0);
 509         sbufWriteU32(dst, 0);
 510         sbufWriteU32(dst, 0);
 511     }
 512 }
 513
 514 #ifdef USE_FLASHFS
 515 enum compressionType_e {
 516     NO_COMPRESSION,
 517     HUFFMAN
 518 };
 519
 520 static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uint16_t size, bool useLegacyFormat, bool allowCompression)
 521 {
 522     STATIC_ASSERT(MSP_PORT_DATAFLASH_INFO_SIZE >= 16, MSP_PORT_DATAFLASH_INFO_SIZE_invalid);
 523
 524     uint16_t readLen = size;
 525     const int bytesRemainingInBuf = sbufBytesRemaining(dst) - MSP_PORT_DATAFLASH_INFO_SIZE;
 526     if (readLen > bytesRemainingInBuf) {
 527         readLen = bytesRemainingInBuf;
 528     }
 529     // size will be lower than that requested if we reach end of volume
 530     const uint32_t flashfsSize = flashfsGetSize();
 531     if (readLen > flashfsSize - address) {
 532         // truncate the request
 533         readLen = flashfsSize - address;
 534     }
 535     sbufWriteU32(dst, address);
 536
 537     // legacy format does not support compression
 538 #ifdef USE_HUFFMAN
 539     const uint8_t compressionMethod = (!allowCompression || useLegacyFormat) ? NO_COMPRESSION : HUFFMAN;
 540 #else
 541     const uint8_t compressionMethod = NO_COMPRESSION;
 542     UNUSED(allowCompression);
 543 #endif
 544
 545     if (compressionMethod == NO_COMPRESSION) {
 546
 547         uint16_t *readLenPtr = (uint16_t *)sbufPtr(dst);
 548         if (!useLegacyFormat) {
 549             // new format supports variable read lengths
 550             sbufWriteU16(dst, readLen);
 551             sbufWriteU8(dst, 0); // placeholder for compression format
 552         }
 553
 554         const int bytesRead = flashfsReadAbs(address, sbufPtr(dst), readLen);
 555
 556         if (!useLegacyFormat) {
 557             // update the 'read length' with the actual amount read from flash.
 558             *readLenPtr = bytesRead;
 559         }
 560
 561         sbufAdvance(dst, bytesRead);
 562
 563         if (useLegacyFormat) {
 564             // pad the buffer with zeros
 565             for (int i = bytesRead; i < size; i++) {
 566                 sbufWriteU8(dst, 0);
 567             }
 568         }
 569     } else {
 570 #ifdef USE_HUFFMAN
 571         // compress in 256-byte chunks
 572         const uint16_t READ_BUFFER_SIZE = 256;
 573         // This may be DMAable, so make it cache aligned
 574         __attribute__ ((aligned(32))) uint8_t readBuffer[READ_BUFFER_SIZE];
 575
 576         huffmanState_t state = {
 577             .bytesWritten = 0,
 578             .outByte = sbufPtr(dst) + sizeof(uint16_t) + sizeof(uint8_t) + HUFFMAN_INFO_SIZE,
 579             .outBufLen = readLen,
 580             .outBit = 0x80,
 581         };
 582         *state.outByte = 0;
 583
 584         uint16_t bytesReadTotal = 0;
 585         // read until output buffer overflows or flash is exhausted
 586         while (state.bytesWritten < state.outBufLen && address + bytesReadTotal < flashfsSize) {
 587             const int bytesRead = flashfsReadAbs(address + bytesReadTotal, readBuffer,
 588                 MIN(sizeof(readBuffer), flashfsSize - address - bytesReadTotal));
 589
 590             const int status = huffmanEncodeBufStreaming(&state, readBuffer, bytesRead, huffmanTable);
 591             if (status == -1) {
 592                 // overflow
 593                 break;
 594             }
 595
 596             bytesReadTotal += bytesRead;
 597         }
 598
 599         if (state.outBit != 0x80) {
 600             ++state.bytesWritten;
 601         }
 602
 603         // header
 604         sbufWriteU16(dst, HUFFMAN_INFO_SIZE + state.bytesWritten);
 605         sbufWriteU8(dst, compressionMethod);
 606         // payload
 607         sbufWriteU16(dst, bytesReadTotal);
 608         sbufAdvance(dst, state.bytesWritten);
 609 #endif
 610     }
 611 }
 612 #endif // USE_FLASHFS
 613
 614 /*
 615  * Returns true if the command was processd, false otherwise.
 616  * May set mspPostProcessFunc to a function to be called once the command has been processed
 617  */
 618 static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
 619 {
 620     UNUSED(mspPostProcessFn);
 621
 622     switch (cmdMSP) {
 623     case MSP_API_VERSION:
 624         sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
 625         sbufWriteU8(dst, API_VERSION_MAJOR);
 626         sbufWriteU8(dst, API_VERSION_MINOR);
 627         break;
 628
 629     case MSP_FC_VARIANT:
 630         sbufWriteData(dst, flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
 631         break;
 632
 633     case MSP_FC_VERSION:
 634         sbufWriteU8(dst, FC_VERSION_MAJOR);
 635         sbufWriteU8(dst, FC_VERSION_MINOR);
 636         sbufWriteU8(dst, FC_VERSION_PATCH_LEVEL);
 637         break;
 638
 639     case MSP_BOARD_INFO:
 640     {
 641         sbufWriteData(dst, systemConfig()->boardIdentifier, BOARD_IDENTIFIER_LENGTH);
 642 #ifdef USE_HARDWARE_REVISION_DETECTION
 643         sbufWriteU16(dst, hardwareRevision);
 644 #else
 645         sbufWriteU16(dst, 0); // No other build targets currently have hardware revision detection.
 646 #endif
 647 #if defined(USE_MAX7456)
 648         sbufWriteU8(dst, 2);  // 2 == FC with MAX7456
 649 #else
 650         sbufWriteU8(dst, 0);  // 0 == FC
 651 #endif
 652
 653         // Target capabilities (uint8)
 654 #define TARGET_HAS_VCP 0
 655 #define TARGET_HAS_SOFTSERIAL 1
 656 #define TARGET_IS_UNIFIED 2
 657 #define TARGET_HAS_FLASH_BOOTLOADER 3
 658 #define TARGET_SUPPORTS_CUSTOM_DEFAULTS 4
 659 #define TARGET_HAS_CUSTOM_DEFAULTS 5
 660 #define TARGET_SUPPORTS_RX_BIND 6
 661
 662         uint8_t targetCapabilities = 0;
 663 #ifdef USE_VCP
 664         targetCapabilities |= BIT(TARGET_HAS_VCP);
 665 #endif
 666 #if defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)
 667         targetCapabilities |= BIT(TARGET_HAS_SOFTSERIAL);
 668 #endif
 669         targetCapabilities |= BIT(TARGET_IS_UNIFIED);
 670 #if defined(USE_FLASH_BOOT_LOADER)
 671         targetCapabilities |= BIT(TARGET_HAS_FLASH_BOOTLOADER);
 672 #endif
 673 #if defined(USE_CUSTOM_DEFAULTS)
 674         targetCapabilities |= BIT(TARGET_SUPPORTS_CUSTOM_DEFAULTS);
 675         if (hasCustomDefaults()) {
 676             targetCapabilities |= BIT(TARGET_HAS_CUSTOM_DEFAULTS);
 677         }
 678 #endif
 679 #if defined(USE_RX_BIND)
 680         if (getRxBindSupported()) {
 681             targetCapabilities |= BIT(TARGET_SUPPORTS_RX_BIND);
 682         }
 683 #endif
 684
 685         sbufWriteU8(dst, targetCapabilities);
 686
 687         // Target name with explicit length
 688         sbufWriteU8(dst, strlen(targetName));
 689         sbufWriteData(dst, targetName, strlen(targetName));
 690
 691 #if defined(USE_BOARD_INFO)
 692         // Board name with explicit length
 693         char *value = getBoardName();
 694         sbufWriteU8(dst, strlen(value));
 695         sbufWriteString(dst, value);
 696
 697         // Manufacturer id with explicit length
 698         value = getManufacturerId();
 699         sbufWriteU8(dst, strlen(value));
 700         sbufWriteString(dst, value);
 701 #else
 702         sbufWriteU8(dst, 0);
 703         sbufWriteU8(dst, 0);
 704 #endif
 705
 706 #if defined(USE_SIGNATURE)
 707         // Signature
 708         sbufWriteData(dst, getSignature(), SIGNATURE_LENGTH);
 709 #else
 710         uint8_t emptySignature[SIGNATURE_LENGTH];
 711         memset(emptySignature, 0, sizeof(emptySignature));
 712         sbufWriteData(dst, &emptySignature, sizeof(emptySignature));
 713 #endif
 714
 715         sbufWriteU8(dst, getMcuTypeId());
 716
 717         // Added in API version 1.42
 718         sbufWriteU8(dst, systemConfig()->configurationState);
 719
 720         // Added in API version 1.43
 721         sbufWriteU16(dst, gyro.sampleRateHz); // informational so the configurator can display the correct gyro/pid frequencies in the drop-down
 722
 723         // Configuration warnings / problems (uint32_t)
 724 #define PROBLEM_ACC_NEEDS_CALIBRATION 0
 725 #define PROBLEM_MOTOR_PROTOCOL_DISABLED 1
 726
 727         uint32_t configurationProblems = 0;
 728
 729 #if defined(USE_ACC)
 730         if (!accHasBeenCalibrated()) {
 731             configurationProblems |= BIT(PROBLEM_ACC_NEEDS_CALIBRATION);
 732         }
 733 #endif
 734
 735         if (!checkMotorProtocolEnabled(&motorConfig()->dev, NULL)) {
 736             configurationProblems |= BIT(PROBLEM_MOTOR_PROTOCOL_DISABLED);
 737         }
 738
 739         sbufWriteU32(dst, configurationProblems);
 740
 741         // Added in MSP API 1.44
 742 #if defined(USE_SPI)
 743         sbufWriteU8(dst, spiGetRegisteredDeviceCount());
 744 #else
 745         sbufWriteU8(dst, 0);
 746 #endif
 747 #if defined(USE_I2C)
 748         sbufWriteU8(dst, i2cGetRegisteredDeviceCount());
 749 #else
 750         sbufWriteU8(dst, 0);
 751 #endif
 752
 753         break;
 754     }
 755
 756     case MSP_BUILD_INFO:
 757         sbufWriteData(dst, buildDate, BUILD_DATE_LENGTH);
 758         sbufWriteData(dst, buildTime, BUILD_TIME_LENGTH);
 759         sbufWriteData(dst, shortGitRevision, GIT_SHORT_REVISION_LENGTH);
 760         break;
 761
 762     case MSP_ANALOG:
 763         sbufWriteU8(dst, (uint8_t)constrain(getLegacyBatteryVoltage(), 0, 255));
 764         sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
 765         sbufWriteU16(dst, getRssi());
 766         sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send current in 0.01 A steps, range is -320A to 320A
 767         sbufWriteU16(dst, getBatteryVoltage());
 768         break;
 769
 770     case MSP_DEBUG:
 771         for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) {
 772             sbufWriteU16(dst, debug[i]);      // 4 variables are here for general monitoring purpose
 773         }
 774         break;
 775
 776     case MSP_UID:
 777         sbufWriteU32(dst, U_ID_0);
 778         sbufWriteU32(dst, U_ID_1);
 779         sbufWriteU32(dst, U_ID_2);
 780         break;
 781
 782     case MSP_FEATURE_CONFIG:
 783         sbufWriteU32(dst, featureConfig()->enabledFeatures);
 784         break;
 785
 786 #ifdef USE_BEEPER
 787     case MSP_BEEPER_CONFIG:
 788         sbufWriteU32(dst, beeperConfig()->beeper_off_flags);
 789         sbufWriteU8(dst, beeperConfig()->dshotBeaconTone);
 790         sbufWriteU32(dst, beeperConfig()->dshotBeaconOffFlags);
 791         break;
 792 #endif
 793
 794     case MSP_BATTERY_STATE: {
 795         // battery characteristics
 796         sbufWriteU8(dst, (uint8_t)constrain(getBatteryCellCount(), 0, 255)); // 0 indicates battery not detected.
 797         sbufWriteU16(dst, batteryConfig()->batteryCapacity); // in mAh
 798
 799         // battery state
 800         sbufWriteU8(dst, (uint8_t)constrain(getLegacyBatteryVoltage(), 0, 255)); // in 0.1V steps
 801         sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
 802         sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send current in 0.01 A steps, range is -320A to 320A
 803
 804         // battery alerts
 805         sbufWriteU8(dst, (uint8_t)getBatteryState());
 806
 807         sbufWriteU16(dst, getBatteryVoltage()); // in 0.01V steps
 808         break;
 809     }
 810
 811     case MSP_VOLTAGE_METERS: {
 812         // write out id and voltage meter values, once for each meter we support
 813         uint8_t count = supportedVoltageMeterCount;
 814 #ifdef USE_ESC_SENSOR
 815         count -= VOLTAGE_METER_ID_ESC_COUNT - getMotorCount();
 816 #endif
 817
 818         for (int i = 0; i < count; i++) {
 819
 820             voltageMeter_t meter;
 821             uint8_t id = (uint8_t)voltageMeterIds[i];
 822             voltageMeterRead(id, &meter);
 823
 824             sbufWriteU8(dst, id);
 825             sbufWriteU8(dst, (uint8_t)constrain((meter.displayFiltered + 5) / 10, 0, 255));
 826         }
 827         break;
 828     }
 829
 830     case MSP_CURRENT_METERS: {
 831         // write out id and current meter values, once for each meter we support
 832         uint8_t count = supportedCurrentMeterCount;
 833 #ifdef USE_ESC_SENSOR
 834         count -= VOLTAGE_METER_ID_ESC_COUNT - getMotorCount();
 835 #endif
 836         for (int i = 0; i < count; i++) {
 837
 838             currentMeter_t meter;
 839             uint8_t id = (uint8_t)currentMeterIds[i];
 840             currentMeterRead(id, &meter);
 841
 842             sbufWriteU8(dst, id);
 843             sbufWriteU16(dst, (uint16_t)constrain(meter.mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
 844             sbufWriteU16(dst, (uint16_t)constrain(meter.amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps (mA). Negative range is truncated to zero
 845         }
 846         break;
 847     }
 848
 849     case MSP_VOLTAGE_METER_CONFIG:
 850         {
 851             // by using a sensor type and a sub-frame length it's possible to configure any type of voltage meter,
 852             // e.g. an i2c/spi/can sensor or any sensor not built directly into the FC such as ESC/RX/SPort/SBus that has
 853             // different configuration requirements.
 854             STATIC_ASSERT(VOLTAGE_SENSOR_ADC_VBAT == 0, VOLTAGE_SENSOR_ADC_VBAT_incorrect); // VOLTAGE_SENSOR_ADC_VBAT should be the first index
 855             sbufWriteU8(dst, MAX_VOLTAGE_SENSOR_ADC); // voltage meters in payload
 856             for (int i = VOLTAGE_SENSOR_ADC_VBAT; i < MAX_VOLTAGE_SENSOR_ADC; i++) {
 857                 const uint8_t adcSensorSubframeLength = 1 + 1 + 1 + 1 + 1; // length of id, type, vbatscale, vbatresdivval, vbatresdivmultipler, in bytes
 858                 sbufWriteU8(dst, adcSensorSubframeLength); // ADC sensor sub-frame length
 859
 860                 sbufWriteU8(dst, voltageMeterADCtoIDMap[i]); // id of the sensor
 861                 sbufWriteU8(dst, VOLTAGE_SENSOR_TYPE_ADC_RESISTOR_DIVIDER); // indicate the type of sensor that the next part of the payload is for
 862
 863                 sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatscale);
 864                 sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivval);
 865                 sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivmultiplier);
 866             }
 867             // if we had any other voltage sensors, this is where we would output any needed configuration
 868         }
 869
 870         break;
 871     case MSP_CURRENT_METER_CONFIG: {
 872         // the ADC and VIRTUAL sensors have the same configuration requirements, however this API reflects
 873         // that this situation may change and allows us to support configuration of any current sensor with
 874         // specialist configuration requirements.
 875
 876         int currentMeterCount = 1;
 877
 878 #ifdef USE_VIRTUAL_CURRENT_METER
 879         currentMeterCount++;
 880 #endif
 881         sbufWriteU8(dst, currentMeterCount);
 882
 883         const uint8_t adcSensorSubframeLength = 1 + 1 + 2 + 2; // length of id, type, scale, offset, in bytes
 884         sbufWriteU8(dst, adcSensorSubframeLength);
 885         sbufWriteU8(dst, CURRENT_METER_ID_BATTERY_1); // the id of the meter
 886         sbufWriteU8(dst, CURRENT_SENSOR_ADC); // indicate the type of sensor that the next part of the payload is for
 887         sbufWriteU16(dst, currentSensorADCConfig()->scale);
 888         sbufWriteU16(dst, currentSensorADCConfig()->offset);
 889
 890 #ifdef USE_VIRTUAL_CURRENT_METER
 891         const int8_t virtualSensorSubframeLength = 1 + 1 + 2 + 2; // length of id, type, scale, offset, in bytes
 892         sbufWriteU8(dst, virtualSensorSubframeLength);
 893         sbufWriteU8(dst, CURRENT_METER_ID_VIRTUAL_1); // the id of the meter
 894         sbufWriteU8(dst, CURRENT_SENSOR_VIRTUAL); // indicate the type of sensor that the next part of the payload is for
 895         sbufWriteU16(dst, currentSensorVirtualConfig()->scale);
 896         sbufWriteU16(dst, currentSensorVirtualConfig()->offset);
 897 #endif
 898
 899         // if we had any other current sensors, this is where we would output any needed configuration
 900         break;
 901     }
 902
 903     case MSP_BATTERY_CONFIG:
 904         sbufWriteU8(dst, (batteryConfig()->vbatmincellvoltage + 5) / 10);
 905         sbufWriteU8(dst, (batteryConfig()->vbatmaxcellvoltage + 5) / 10);
 906         sbufWriteU8(dst, (batteryConfig()->vbatwarningcellvoltage + 5) / 10);
 907         sbufWriteU16(dst, batteryConfig()->batteryCapacity);
 908         sbufWriteU8(dst, batteryConfig()->voltageMeterSource);
 909         sbufWriteU8(dst, batteryConfig()->currentMeterSource);
 910         sbufWriteU16(dst, batteryConfig()->vbatmincellvoltage);
 911         sbufWriteU16(dst, batteryConfig()->vbatmaxcellvoltage);
 912         sbufWriteU16(dst, batteryConfig()->vbatwarningcellvoltage);
 913         break;
 914
 915     case MSP_TRANSPONDER_CONFIG: {
 916 #ifdef USE_TRANSPONDER
 917         // Backward compatibility to BFC 3.1.1 is lost for this message type
 918         sbufWriteU8(dst, TRANSPONDER_PROVIDER_COUNT);
 919         for (unsigned int i = 0; i < TRANSPONDER_PROVIDER_COUNT; i++) {
 920             sbufWriteU8(dst, transponderRequirements[i].provider);
 921             sbufWriteU8(dst, transponderRequirements[i].dataLength);
 922         }
 923
 924         uint8_t provider = transponderConfig()->provider;
 925         sbufWriteU8(dst, provider);
 926
 927         if (provider) {
 928             uint8_t requirementIndex = provider - 1;
 929             uint8_t providerDataLength = transponderRequirements[requirementIndex].dataLength;
 930
 931             for (unsigned int i = 0; i < providerDataLength; i++) {
 932                 sbufWriteU8(dst, transponderConfig()->data[i]);
 933             }
 934         }
 935 #else
 936         sbufWriteU8(dst, 0); // no providers
 937 #endif
 938         break;
 939     }
 940
 941 #if defined(USE_OSD)
 942     case MSP_OSD_CONFIG: {
 943 #define OSD_FLAGS_OSD_FEATURE           (1 << 0)
 944 //#define OSD_FLAGS_OSD_SLAVE             (1 << 1)
 945 #define OSD_FLAGS_RESERVED_1            (1 << 2)
 946 #define OSD_FLAGS_OSD_HARDWARE_FRSKYOSD (1 << 3)
 947 #define OSD_FLAGS_OSD_HARDWARE_MAX_7456 (1 << 4)
 948 #define OSD_FLAGS_OSD_DEVICE_DETECTED   (1 << 5)
 949 #define OSD_FLAGS_OSD_MSP_DEVICE        (1 << 6)
 950
 951         uint8_t osdFlags = 0;
 952
 953         osdFlags |= OSD_FLAGS_OSD_FEATURE;
 954
 955         osdDisplayPortDevice_e deviceType;
 956         displayPort_t *osdDisplayPort = osdGetDisplayPort(&deviceType);
 957         bool displayIsReady = osdDisplayPort && displayCheckReady(osdDisplayPort, true);
 958         switch (deviceType) {
 959         case OSD_DISPLAYPORT_DEVICE_MAX7456:
 960             osdFlags |= OSD_FLAGS_OSD_HARDWARE_MAX_7456;
 961             if (displayIsReady) {
 962                 osdFlags |= OSD_FLAGS_OSD_DEVICE_DETECTED;
 963             }
 964
 965             break;
 966         case OSD_DISPLAYPORT_DEVICE_FRSKYOSD:
 967             osdFlags |= OSD_FLAGS_OSD_HARDWARE_FRSKYOSD;
 968             if (displayIsReady) {
 969                 osdFlags |= OSD_FLAGS_OSD_DEVICE_DETECTED;
 970             }
 971
 972             break;
 973         case OSD_DISPLAYPORT_DEVICE_MSP:
 974             osdFlags |= OSD_FLAGS_OSD_MSP_DEVICE;
 975             if (displayIsReady) {
 976                 osdFlags |= OSD_FLAGS_OSD_DEVICE_DETECTED;
 977             }
 978
 979             break;
 980         default:
 981             break;
 982         }
 983
 984         sbufWriteU8(dst, osdFlags);
 985
 986 #ifdef USE_OSD_SD
 987         // send video system (AUTO/PAL/NTSC/HD)
 988         sbufWriteU8(dst, vcdProfile()->video_system);
 989 #else
 990         sbufWriteU8(dst, VIDEO_SYSTEM_HD);
 991 #endif // USE_OSD_SD
 992
 993         // OSD specific, not applicable to OSD slaves.
 994
 995         // Configuration
 996         sbufWriteU8(dst, osdConfig()->units);
 997
 998         // Alarms
 999         sbufWriteU8(dst, osdConfig()->rssi_alarm);
1000         sbufWriteU16(dst, osdConfig()->cap_alarm);
1001
1002         // Reuse old timer alarm (U16) as OSD_ITEM_COUNT
1003         sbufWriteU8(dst, 0);
1004         sbufWriteU8(dst, OSD_ITEM_COUNT);
1005
1006         sbufWriteU16(dst, osdConfig()->alt_alarm);
1007
1008         // Element position and visibility
1009         for (int i = 0; i < OSD_ITEM_COUNT; i++) {
1010             sbufWriteU16(dst, osdElementConfig()->item_pos[i]);
1011         }
1012
1013         // Post flight statistics
1014         sbufWriteU8(dst, OSD_STAT_COUNT);
1015         for (int i = 0; i < OSD_STAT_COUNT; i++ ) {
1016             sbufWriteU8(dst, osdStatGetState(i));
1017         }
1018
1019         // Timers
1020         sbufWriteU8(dst, OSD_TIMER_COUNT);
1021         for (int i = 0; i < OSD_TIMER_COUNT; i++) {
1022             sbufWriteU16(dst, osdConfig()->timers[i]);
1023         }
1024
1025         // Enabled warnings
1026         // Send low word first for backwards compatibility (API < 1.41)
1027         sbufWriteU16(dst, (uint16_t)(osdConfig()->enabledWarnings & 0xFFFF));
1028         // API >= 1.41
1029         // Send the warnings count and 32bit enabled warnings flags.
1030         // Add currently active OSD profile (0 indicates OSD profiles not available).
1031         // Add OSD stick overlay mode (0 indicates OSD stick overlay not available).
1032         sbufWriteU8(dst, OSD_WARNING_COUNT);
1033         sbufWriteU32(dst, osdConfig()->enabledWarnings);
1034
1035 #ifdef USE_OSD_PROFILES
1036         sbufWriteU8(dst, OSD_PROFILE_COUNT);            // available profiles
1037         sbufWriteU8(dst, osdConfig()->osdProfileIndex); // selected profile
1038 #else
1039         // If the feature is not available there is only 1 profile and it's always selected
1040         sbufWriteU8(dst, 1);
1041         sbufWriteU8(dst, 1);
1042 #endif // USE_OSD_PROFILES
1043
1044 #ifdef USE_OSD_STICK_OVERLAY
1045         sbufWriteU8(dst, osdConfig()->overlay_radio_mode);
1046 #else
1047         sbufWriteU8(dst, 0);
1048 #endif // USE_OSD_STICK_OVERLAY
1049
1050         // API >= 1.43
1051         // Add the camera frame element width/height
1052         sbufWriteU8(dst, osdConfig()->camera_frame_width);
1053         sbufWriteU8(dst, osdConfig()->camera_frame_height);
1054
1055         break;
1056     }
1057 #endif // USE_OSD
1058
1059     case MSP_OSD_CANVAS: {
1060 #ifdef USE_OSD
1061         sbufWriteU8(dst, osdConfig()->canvas_cols);
1062         sbufWriteU8(dst, osdConfig()->canvas_rows);
1063 #endif
1064         break;
1065     }
1066
1067     default:
1068         return false;
1069     }
1070     return true;
1071 }
1072
1073 static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *dst)
1074 {
1075     bool unsupportedCommand = false;
1076
1077 #if !defined(USE_VTX_COMMON) || !defined(USE_VTX_MSP)
1078     UNUSED(srcDesc);
1079 #endif
1080
1081     switch (cmdMSP) {
1082     case MSP_STATUS_EX:
1083     case MSP_STATUS:
1084         {
1085             boxBitmask_t flightModeFlags;
1086             const int flagBits = packFlightModeFlags(&flightModeFlags);
1087
1088             sbufWriteU16(dst, getTaskDeltaTimeUs(TASK_PID));
1089 #ifdef USE_I2C
1090             sbufWriteU16(dst, i2cGetErrorCounter());
1091 #else
1092             sbufWriteU16(dst, 0);
1093 #endif
1094             sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_RANGEFINDER) << 4 | sensors(SENSOR_GYRO) << 5);
1095             sbufWriteData(dst, &flightModeFlags, 4);        // unconditional part of flags, first 32 bits
1096             sbufWriteU8(dst, getCurrentPidProfileIndex());
1097             sbufWriteU16(dst, constrain(getAverageSystemLoadPercent(), 0, LOAD_PERCENTAGE_ONE));
1098             if (cmdMSP == MSP_STATUS_EX) {
1099                 sbufWriteU8(dst, PID_PROFILE_COUNT);
1100                 sbufWriteU8(dst, getCurrentControlRateProfileIndex());
1101             } else {  // MSP_STATUS
1102                 sbufWriteU16(dst, 0); // gyro cycle time
1103             }
1104
1105             // write flightModeFlags header. Lowest 4 bits contain number of bytes that follow
1106             // header is emited even when all bits fit into 32 bits to allow future extension
1107             int byteCount = (flagBits - 32 + 7) / 8;        // 32 already stored, round up
1108             byteCount = constrain(byteCount, 0, 15);        // limit to 16 bytes (128 bits)
1109             sbufWriteU8(dst, byteCount);
1110             sbufWriteData(dst, ((uint8_t*)&flightModeFlags) + 4, byteCount);
1111
1112             // Write arming disable flags
1113             // 1 byte, flag count
1114             sbufWriteU8(dst, ARMING_DISABLE_FLAGS_COUNT);
1115             // 4 bytes, flags
1116             const uint32_t armingDisableFlags = getArmingDisableFlags();
1117             sbufWriteU32(dst, armingDisableFlags);
1118
1119             // config state flags - bits to indicate the state of the configuration, reboot required, etc.
1120             // other flags can be added as needed
1121             sbufWriteU8(dst, (getRebootRequired() << 0));
1122         }
1123         break;
1124
1125     case MSP_RAW_IMU:
1126         {
1127 #if defined(USE_ACC)
1128             // Hack scale due to choice of units for sensor data in multiwii
1129
1130             uint8_t scale;
1131             if (acc.dev.acc_1G > 512 * 4) {
1132                 scale = 8;
1133             } else if (acc.dev.acc_1G > 512 * 2) {
1134                 scale = 4;
1135             } else if (acc.dev.acc_1G >= 512) {
1136                 scale = 2;
1137             } else {
1138                 scale = 1;
1139             }
1140 #endif
1141
1142             for (int i = 0; i < 3; i++) {
1143 #if defined(USE_ACC)
1144                 sbufWriteU16(dst, lrintf(acc.accADC[i] / scale));
1145 #else
1146                 sbufWriteU16(dst, 0);
1147 #endif
1148             }
1149             for (int i = 0; i < 3; i++) {
1150                 sbufWriteU16(dst, gyroRateDps(i));
1151             }
1152             for (int i = 0; i < 3; i++) {
1153 #if defined(USE_MAG)
1154                 sbufWriteU16(dst, lrintf(mag.magADC[i]));
1155 #else
1156                 sbufWriteU16(dst, 0);
1157 #endif
1158             }
1159         }
1160         break;
1161
1162 case MSP_NAME:
1163         {
1164             const int nameLen = strlen(pilotConfig()->craftName);
1165             for (int i = 0; i < nameLen; i++) {
1166                 sbufWriteU8(dst, pilotConfig()->craftName[i]);
1167             }
1168         }
1169         break;
1170
1171 #ifdef USE_SERVOS
1172     case MSP_SERVO:
1173         sbufWriteData(dst, &servo, MAX_SUPPORTED_SERVOS * 2);
1174         break;
1175     case MSP_SERVO_CONFIGURATIONS:
1176         for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
1177             sbufWriteU16(dst, servoParams(i)->min);
1178             sbufWriteU16(dst, servoParams(i)->max);
1179             sbufWriteU16(dst, servoParams(i)->middle);
1180             sbufWriteU8(dst, servoParams(i)->rate);
1181             sbufWriteU8(dst, servoParams(i)->forwardFromChannel);
1182             sbufWriteU32(dst, servoParams(i)->reversedSources);
1183         }
1184         break;
1185
1186     case MSP_SERVO_MIX_RULES:
1187         for (int i = 0; i < MAX_SERVO_RULES; i++) {
1188             sbufWriteU8(dst, customServoMixers(i)->targetChannel);
1189             sbufWriteU8(dst, customServoMixers(i)->inputSource);
1190             sbufWriteU8(dst, customServoMixers(i)->rate);
1191             sbufWriteU8(dst, customServoMixers(i)->speed);
1192             sbufWriteU8(dst, customServoMixers(i)->min);
1193             sbufWriteU8(dst, customServoMixers(i)->max);
1194             sbufWriteU8(dst, customServoMixers(i)->box);
1195         }
1196         break;
1197 #endif
1198
1199     case MSP_MOTOR:
1200         for (unsigned i = 0; i < 8; i++) {
1201 #ifdef USE_MOTOR
1202             if (!motorIsEnabled() || i >= MAX_SUPPORTED_MOTORS || !motorIsMotorEnabled(i)) {
1203                 sbufWriteU16(dst, 0);
1204                 continue;
1205             }
1206
1207             sbufWriteU16(dst, motorConvertToExternal(motor[i]));
1208 #else
1209             sbufWriteU16(dst, 0);
1210 #endif
1211         }
1212
1213         break;
1214
1215     // Added in API version 1.42
1216     case MSP_MOTOR_TELEMETRY:
1217         sbufWriteU8(dst, getMotorCount());
1218         for (unsigned i = 0; i < getMotorCount(); i++) {
1219             int rpm = 0;
1220             uint16_t invalidPct = 0;
1221             uint8_t escTemperature = 0;  // degrees celcius
1222             uint16_t escVoltage = 0;     // 0.01V per unit
1223             uint16_t escCurrent = 0;     // 0.01A per unit
1224             uint16_t escConsumption = 0; // mAh
1225
1226             bool rpmDataAvailable = false;
1227
1228 #ifdef USE_DSHOT_TELEMETRY
1229             if (motorConfig()->dev.useDshotTelemetry) {
1230                 rpm = erpmToRpm(getDshotTelemetry(i));
1231                 rpmDataAvailable = true;
1232                 invalidPct = 10000; // 100.00%
1233
1234
1235 #ifdef USE_DSHOT_TELEMETRY_STATS
1236                 if (isDshotMotorTelemetryActive(i)) {
1237                     invalidPct = getDshotTelemetryMotorInvalidPercent(i);
1238                 }
1239 #endif
1240
1241
1242                 // Provide extended dshot telemetry
1243                 if ((dshotTelemetryState.motorState[i].telemetryTypes & DSHOT_EXTENDED_TELEMETRY_MASK) != 0) {
1244                     // Temperature Celsius [0, 1, ..., 255] in degree Celsius, just like Blheli_32 and KISS
1245                     if ((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_TEMPERATURE)) != 0) {
1246                         escTemperature = dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE];
1247                     }
1248
1249                     // Current -> 0-255A step 1A
1250                     if ((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_CURRENT)) != 0) {
1251                         escCurrent = dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_CURRENT];
1252                     }
1253
1254                     // Voltage -> 0-63,75V step 0,25V
1255                     if ((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_VOLTAGE)) != 0) {
1256                         escVoltage = dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_VOLTAGE] >> 2;
1257                     }
1258                 }
1259             }
1260 #endif
1261
1262 #ifdef USE_ESC_SENSOR
1263             if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
1264                 escSensorData_t *escData = getEscSensorData(i);
1265                 if (!rpmDataAvailable) {  // We want DSHOT telemetry RPM data (if available) to have precedence
1266                     rpm = erpmToRpm(escData->rpm);
1267                     rpmDataAvailable = true;
1268                 }
1269                 escTemperature = escData->temperature;
1270                 escVoltage = escData->voltage;
1271                 escCurrent = escData->current;
1272                 escConsumption = escData->consumption;
1273             }
1274 #endif
1275
1276             sbufWriteU32(dst, (rpmDataAvailable ? rpm : 0));
1277             sbufWriteU16(dst, invalidPct);
1278             sbufWriteU8(dst, escTemperature);
1279             sbufWriteU16(dst, escVoltage);
1280             sbufWriteU16(dst, escCurrent);
1281             sbufWriteU16(dst, escConsumption);
1282         }
1283         break;
1284
1285     case MSP2_MOTOR_OUTPUT_REORDERING:
1286         {
1287             sbufWriteU8(dst, MAX_SUPPORTED_MOTORS);
1288
1289             for (unsigned i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
1290                 sbufWriteU8(dst, motorConfig()->dev.motorOutputReordering[i]);
1291             }
1292         }
1293         break;
1294
1295 #ifdef USE_VTX_COMMON
1296     case MSP2_GET_VTX_DEVICE_STATUS:
1297         {
1298             const vtxDevice_t *vtxDevice = vtxCommonDevice();
1299             vtxCommonSerializeDeviceStatus(vtxDevice, dst);
1300         }
1301         break;
1302 #endif
1303
1304 #ifdef USE_OSD
1305     case MSP2_GET_OSD_WARNINGS:
1306         {
1307             bool isBlinking;
1308             uint8_t displayAttr;
1309             char warningsBuffer[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
1310
1311             renderOsdWarning(warningsBuffer, &isBlinking, &displayAttr);
1312             const uint8_t warningsLen = strlen(warningsBuffer);
1313
1314             if (isBlinking) {
1315                 displayAttr |= DISPLAYPORT_BLINK;
1316             }
1317             sbufWriteU8(dst, displayAttr);  // see displayPortSeverity_e
1318             sbufWriteU8(dst, warningsLen);  // length byte followed by the actual characters
1319             for (unsigned i = 0; i < warningsLen; i++) {
1320                 sbufWriteU8(dst, warningsBuffer[i]);
1321             }
1322             break;
1323         }
1324 #endif
1325
1326     case MSP_RC:
1327         for (int i = 0; i < rxRuntimeState.channelCount; i++) {
1328             sbufWriteU16(dst, rcData[i]);
1329         }
1330         break;
1331
1332     case MSP_ATTITUDE:
1333         sbufWriteU16(dst, attitude.values.roll);
1334         sbufWriteU16(dst, attitude.values.pitch);
1335         sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
1336         break;
1337
1338     case MSP_ALTITUDE:
1339         sbufWriteU32(dst, getEstimatedAltitudeCm());
1340 #ifdef USE_VARIO
1341         sbufWriteU16(dst, getEstimatedVario());
1342 #else
1343         sbufWriteU16(dst, 0);
1344 #endif
1345         break;
1346
1347     case MSP_SONAR_ALTITUDE:
1348 #if defined(USE_RANGEFINDER)
1349         sbufWriteU32(dst, rangefinderGetLatestAltitude());
1350 #else
1351         sbufWriteU32(dst, 0);
1352 #endif
1353         break;
1354
1355     case MSP_BOARD_ALIGNMENT_CONFIG:
1356         sbufWriteU16(dst, boardAlignment()->rollDegrees);
1357         sbufWriteU16(dst, boardAlignment()->pitchDegrees);
1358         sbufWriteU16(dst, boardAlignment()->yawDegrees);
1359         break;
1360
1361     case MSP_ARMING_CONFIG:
1362         sbufWriteU8(dst, armingConfig()->auto_disarm_delay);
1363         sbufWriteU8(dst, 0);
1364         sbufWriteU8(dst, imuConfig()->small_angle);
1365         break;
1366
1367     case MSP_RC_TUNING:
1368         sbufWriteU8(dst, currentControlRateProfile->rcRates[FD_ROLL]);
1369         sbufWriteU8(dst, currentControlRateProfile->rcExpo[FD_ROLL]);
1370         for (int i = 0 ; i < 3; i++) {
1371             sbufWriteU8(dst, currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
1372         }
1373         sbufWriteU8(dst, 0);   // was currentControlRateProfile->tpa_rate
1374         sbufWriteU8(dst, currentControlRateProfile->thrMid8);
1375         sbufWriteU8(dst, currentControlRateProfile->thrExpo8);
1376         sbufWriteU16(dst, 0);   // was currentControlRateProfile->tpa_breakpoint
1377         sbufWriteU8(dst, currentControlRateProfile->rcExpo[FD_YAW]);
1378         sbufWriteU8(dst, currentControlRateProfile->rcRates[FD_YAW]);
1379         sbufWriteU8(dst, currentControlRateProfile->rcRates[FD_PITCH]);
1380         sbufWriteU8(dst, currentControlRateProfile->rcExpo[FD_PITCH]);
1381
1382         // added in 1.41
1383         sbufWriteU8(dst, currentControlRateProfile->throttle_limit_type);
1384         sbufWriteU8(dst, currentControlRateProfile->throttle_limit_percent);
1385
1386         // added in 1.42
1387         sbufWriteU16(dst, currentControlRateProfile->rate_limit[FD_ROLL]);
1388         sbufWriteU16(dst, currentControlRateProfile->rate_limit[FD_PITCH]);
1389         sbufWriteU16(dst, currentControlRateProfile->rate_limit[FD_YAW]);
1390
1391         // added in 1.43
1392         sbufWriteU8(dst, currentControlRateProfile->rates_type);
1393
1394         break;
1395
1396     case MSP_PID:
1397         for (int i = 0; i < PID_ITEM_COUNT; i++) {
1398             sbufWriteU8(dst, currentPidProfile->pid[i].P);
1399             sbufWriteU8(dst, currentPidProfile->pid[i].I);
1400             sbufWriteU8(dst, currentPidProfile->pid[i].D);
1401         }
1402         break;
1403
1404     case MSP_PIDNAMES:
1405         for (const char *c = pidNames; *c; c++) {
1406             sbufWriteU8(dst, *c);
1407         }
1408         break;
1409
1410     case MSP_PID_CONTROLLER:
1411         sbufWriteU8(dst, PID_CONTROLLER_BETAFLIGHT);
1412         break;
1413
1414     case MSP_MODE_RANGES:
1415         for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
1416             const modeActivationCondition_t *mac = modeActivationConditions(i);
1417             const box_t *box = findBoxByBoxId(mac->modeId);
1418             sbufWriteU8(dst, box->permanentId);
1419             sbufWriteU8(dst, mac->auxChannelIndex);
1420             sbufWriteU8(dst, mac->range.startStep);
1421             sbufWriteU8(dst, mac->range.endStep);
1422         }
1423         break;
1424
1425     case MSP_MODE_RANGES_EXTRA:
1426         sbufWriteU8(dst, MAX_MODE_ACTIVATION_CONDITION_COUNT);          // prepend number of EXTRAs array elements
1427
1428         for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
1429             const modeActivationCondition_t *mac = modeActivationConditions(i);
1430             const box_t *box = findBoxByBoxId(mac->modeId);
1431             const box_t *linkedBox = findBoxByBoxId(mac->linkedTo);
1432             sbufWriteU8(dst, box->permanentId);     // each element is aligned with MODE_RANGES by the permanentId
1433             sbufWriteU8(dst, mac->modeLogic);
1434             sbufWriteU8(dst, linkedBox->permanentId);
1435         }
1436         break;
1437
1438     case MSP_ADJUSTMENT_RANGES:
1439         for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
1440             const adjustmentRange_t *adjRange = adjustmentRanges(i);
1441             sbufWriteU8(dst, 0); // was adjRange->adjustmentIndex
1442             sbufWriteU8(dst, adjRange->auxChannelIndex);
1443             sbufWriteU8(dst, adjRange->range.startStep);
1444             sbufWriteU8(dst, adjRange->range.endStep);
1445             sbufWriteU8(dst, adjRange->adjustmentConfig);
1446             sbufWriteU8(dst, adjRange->auxSwitchChannelIndex);
1447         }
1448         break;
1449
1450     case MSP_MOTOR_CONFIG:
1451         sbufWriteU16(dst, motorConfig()->minthrottle);
1452         sbufWriteU16(dst, motorConfig()->maxthrottle);
1453         sbufWriteU16(dst, motorConfig()->mincommand);
1454
1455         // API 1.42
1456         sbufWriteU8(dst, getMotorCount());
1457         sbufWriteU8(dst, motorConfig()->motorPoleCount);
1458 #ifdef USE_DSHOT_TELEMETRY
1459         sbufWriteU8(dst, motorConfig()->dev.useDshotTelemetry);
1460 #else
1461         sbufWriteU8(dst, 0);
1462 #endif
1463
1464 #ifdef USE_ESC_SENSOR
1465         sbufWriteU8(dst, featureIsEnabled(FEATURE_ESC_SENSOR)); // ESC sensor available
1466 #else
1467         sbufWriteU8(dst, 0);
1468 #endif
1469         break;
1470
1471     // Deprecated in favor of MSP_MOTOR_TELEMETY as of API version 1.42
1472     // Used by DJI FPV
1473     case MSP_ESC_SENSOR_DATA:
1474 #if defined(USE_ESC_SENSOR)
1475         if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
1476             sbufWriteU8(dst, getMotorCount());
1477             for (int i = 0; i < getMotorCount(); i++) {
1478                 const escSensorData_t *escData = getEscSensorData(i);
1479                 sbufWriteU8(dst, escData->temperature);
1480                 sbufWriteU16(dst, escData->rpm);
1481             }
1482         } else
1483 #endif
1484 #if defined(USE_DSHOT_TELEMETRY)
1485         if (motorConfig()->dev.useDshotTelemetry) {
1486             sbufWriteU8(dst, getMotorCount());
1487             for (int i = 0; i < getMotorCount(); i++) {
1488                 sbufWriteU8(dst, dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE]);
1489                 sbufWriteU16(dst, getDshotTelemetry(i) * 100 * 2 / motorConfig()->motorPoleCount);
1490             }
1491         }
1492         else
1493 #endif
1494         {
1495             unsupportedCommand = true;
1496         }
1497
1498         break;
1499
1500 #ifdef USE_GPS
1501     case MSP_GPS_CONFIG:
1502         sbufWriteU8(dst, gpsConfig()->provider);
1503         sbufWriteU8(dst, gpsConfig()->sbasMode);
1504         sbufWriteU8(dst, gpsConfig()->autoConfig);
1505         sbufWriteU8(dst, gpsConfig()->autoBaud);
1506         // Added in API version 1.43
1507         sbufWriteU8(dst, gpsConfig()->gps_set_home_point_once);
1508         sbufWriteU8(dst, gpsConfig()->gps_ublox_use_galileo);
1509         break;
1510
1511     case MSP_RAW_GPS:
1512         sbufWriteU8(dst, STATE(GPS_FIX));
1513         sbufWriteU8(dst, gpsSol.numSat);
1514         sbufWriteU32(dst, gpsSol.llh.lat);
1515         sbufWriteU32(dst, gpsSol.llh.lon);
1516         sbufWriteU16(dst, (uint16_t)constrain(gpsSol.llh.altCm / 100, 0, UINT16_MAX)); // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. To maintain backwards compatibility compensate to 1m per lsb in MSP again.
1517         sbufWriteU16(dst, gpsSol.groundSpeed);
1518         sbufWriteU16(dst, gpsSol.groundCourse);
1519         // Added in API version 1.44
1520         sbufWriteU16(dst, gpsSol.dop.hdop);
1521         break;
1522
1523     case MSP_COMP_GPS:
1524         sbufWriteU16(dst, GPS_distanceToHome);
1525         sbufWriteU16(dst, GPS_directionToHome / 10); // resolution increased in Betaflight 4.4 by factor of 10, this maintains backwards compatibility for DJI OSD
1526         sbufWriteU8(dst, GPS_update & 1);
1527         break;
1528
1529     case MSP_GPSSVINFO:
1530         sbufWriteU8(dst, GPS_numCh);
1531        for (int i = 0; i < GPS_numCh; i++) {
1532            sbufWriteU8(dst, GPS_svinfo_chn[i]);
1533            sbufWriteU8(dst, GPS_svinfo_svid[i]);
1534            sbufWriteU8(dst, GPS_svinfo_quality[i]);
1535            sbufWriteU8(dst, GPS_svinfo_cno[i]);
1536        }
1537         break;
1538
1539 #ifdef USE_GPS_RESCUE
1540     case MSP_GPS_RESCUE:
1541         sbufWriteU16(dst, gpsRescueConfig()->maxRescueAngle);
1542         sbufWriteU16(dst, gpsRescueConfig()->initialAltitudeM);
1543         sbufWriteU16(dst, gpsRescueConfig()->descentDistanceM);
1544         sbufWriteU16(dst, gpsRescueConfig()->rescueGroundspeed);
1545         sbufWriteU16(dst, gpsRescueConfig()->throttleMin);
1546         sbufWriteU16(dst, gpsRescueConfig()->throttleMax);
1547         sbufWriteU16(dst, gpsRescueConfig()->throttleHover);
1548         sbufWriteU8(dst,  gpsRescueConfig()->sanityChecks);
1549         sbufWriteU8(dst,  gpsRescueConfig()->minSats);
1550
1551         // Added in API version 1.43
1552         sbufWriteU16(dst, gpsRescueConfig()->ascendRate);
1553         sbufWriteU16(dst, gpsRescueConfig()->descendRate);
1554         sbufWriteU8(dst, gpsRescueConfig()->allowArmingWithoutFix);
1555         sbufWriteU8(dst, gpsRescueConfig()->altitudeMode);
1556         // Added in API version 1.44
1557         sbufWriteU16(dst, gpsRescueConfig()->minRescueDth);
1558         break;
1559
1560     case MSP_GPS_RESCUE_PIDS:
1561         sbufWriteU16(dst, gpsRescueConfig()->throttleP);
1562         sbufWriteU16(dst, gpsRescueConfig()->throttleI);
1563         sbufWriteU16(dst, gpsRescueConfig()->throttleD);
1564         sbufWriteU16(dst, gpsRescueConfig()->velP);
1565         sbufWriteU16(dst, gpsRescueConfig()->velI);
1566         sbufWriteU16(dst, gpsRescueConfig()->velD);
1567         sbufWriteU16(dst, gpsRescueConfig()->yawP);
1568         break;
1569 #endif
1570 #endif
1571
1572 #if defined(USE_ACC)
1573     case MSP_ACC_TRIM:
1574         sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.pitch);
1575         sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.roll);
1576
1577         break;
1578 #endif
1579     case MSP_MIXER_CONFIG:
1580         sbufWriteU8(dst, mixerConfig()->mixerMode);
1581         sbufWriteU8(dst, mixerConfig()->yaw_motors_reversed);
1582         break;
1583
1584     case MSP_RX_CONFIG:
1585         sbufWriteU8(dst, rxConfig()->serialrx_provider);
1586         sbufWriteU16(dst, rxConfig()->maxcheck);
1587         sbufWriteU16(dst, rxConfig()->midrc);
1588         sbufWriteU16(dst, rxConfig()->mincheck);
1589         sbufWriteU8(dst, rxConfig()->spektrum_sat_bind);
1590         sbufWriteU16(dst, rxConfig()->rx_min_usec);
1591         sbufWriteU16(dst, rxConfig()->rx_max_usec);
1592         sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rcInterpolation
1593         sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rcInterpolationInterval
1594         sbufWriteU16(dst, rxConfig()->airModeActivateThreshold * 10 + 1000);
1595 #ifdef USE_RX_SPI
1596         sbufWriteU8(dst, rxSpiConfig()->rx_spi_protocol);
1597         sbufWriteU32(dst, rxSpiConfig()->rx_spi_id);
1598         sbufWriteU8(dst, rxSpiConfig()->rx_spi_rf_channel_count);
1599 #else
1600         sbufWriteU8(dst, 0);
1601         sbufWriteU32(dst, 0);
1602         sbufWriteU8(dst, 0);
1603 #endif
1604         sbufWriteU8(dst, rxConfig()->fpvCamAngleDegrees);
1605         sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rcSmoothingChannels
1606 #if defined(USE_RC_SMOOTHING_FILTER)
1607         sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rc_smoothing_type
1608         sbufWriteU8(dst, rxConfig()->rc_smoothing_setpoint_cutoff);
1609         sbufWriteU8(dst, rxConfig()->rc_smoothing_feedforward_cutoff);
1610         sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rc_smoothing_input_type
1611         sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rc_smoothing_derivative_type
1612 #else
1613         sbufWriteU8(dst, 0);
1614         sbufWriteU8(dst, 0);
1615         sbufWriteU8(dst, 0);
1616         sbufWriteU8(dst, 0);
1617         sbufWriteU8(dst, 0);
1618 #endif
1619 #if defined(USE_USB_CDC_HID)
1620         sbufWriteU8(dst, usbDevConfig()->type);
1621 #else
1622         sbufWriteU8(dst, 0);
1623 #endif
1624         // Added in MSP API 1.42
1625 #if defined(USE_RC_SMOOTHING_FILTER)
1626         sbufWriteU8(dst, rxConfig()->rc_smoothing_auto_factor_rpy);
1627 #else
1628         sbufWriteU8(dst, 0);
1629 #endif
1630         // Added in MSP API 1.44
1631 #if defined(USE_RC_SMOOTHING_FILTER)
1632         sbufWriteU8(dst, rxConfig()->rc_smoothing_mode);
1633 #else
1634         sbufWriteU8(dst, 0);
1635 #endif
1636
1637         // Added in MSP API 1.45
1638 #ifdef USE_RX_EXPRESSLRS
1639         sbufWriteData(dst, rxExpressLrsSpiConfig()->UID, sizeof(rxExpressLrsSpiConfig()->UID));
1640 #else
1641         uint8_t emptyUid[6];
1642         memset(emptyUid, 0, sizeof(emptyUid));
1643         sbufWriteData(dst, &emptyUid, sizeof(emptyUid));
1644 #endif
1645         break;
1646     case MSP_FAILSAFE_CONFIG:
1647         sbufWriteU8(dst, failsafeConfig()->failsafe_delay);
1648         sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay);
1649         sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
1650         sbufWriteU8(dst, failsafeConfig()->failsafe_switch_mode);
1651         sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay);
1652         sbufWriteU8(dst, failsafeConfig()->failsafe_procedure);
1653         break;
1654
1655     case MSP_RXFAIL_CONFIG:
1656         for (int i = 0; i < rxRuntimeState.channelCount; i++) {
1657             sbufWriteU8(dst, rxFailsafeChannelConfigs(i)->mode);
1658             sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxFailsafeChannelConfigs(i)->step));
1659         }
1660         break;
1661
1662     case MSP_RSSI_CONFIG:
1663         sbufWriteU8(dst, rxConfig()->rssi_channel);
1664         break;
1665
1666     case MSP_RX_MAP:
1667         sbufWriteData(dst, rxConfig()->rcmap, RX_MAPPABLE_CHANNEL_COUNT);
1668         break;
1669
1670     case MSP_CF_SERIAL_CONFIG:
1671         for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
1672             if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
1673                 continue;
1674             };
1675             sbufWriteU8(dst, serialConfig()->portConfigs[i].identifier);
1676             sbufWriteU16(dst, serialConfig()->portConfigs[i].functionMask);
1677             sbufWriteU8(dst, serialConfig()->portConfigs[i].msp_baudrateIndex);
1678             sbufWriteU8(dst, serialConfig()->portConfigs[i].gps_baudrateIndex);
1679             sbufWriteU8(dst, serialConfig()->portConfigs[i].telemetry_baudrateIndex);
1680             sbufWriteU8(dst, serialConfig()->portConfigs[i].blackbox_baudrateIndex);
1681         }
1682         break;
1683     case MSP2_COMMON_SERIAL_CONFIG: {
1684         uint8_t count = 0;
1685         for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
1686             if (serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
1687                 count++;
1688             }
1689         }
1690         sbufWriteU8(dst, count);
1691         for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
1692             if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
1693                 continue;
1694             };
1695             sbufWriteU8(dst, serialConfig()->portConfigs[i].identifier);
1696             sbufWriteU32(dst, serialConfig()->portConfigs[i].functionMask);
1697             sbufWriteU8(dst, serialConfig()->portConfigs[i].msp_baudrateIndex);
1698             sbufWriteU8(dst, serialConfig()->portConfigs[i].gps_baudrateIndex);
1699             sbufWriteU8(dst, serialConfig()->portConfigs[i].telemetry_baudrateIndex);
1700             sbufWriteU8(dst, serialConfig()->portConfigs[i].blackbox_baudrateIndex);
1701         }
1702         break;
1703     }
1704
1705 #ifdef USE_LED_STRIP_STATUS_MODE
1706     case MSP_LED_COLORS:
1707         for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
1708             const hsvColor_t *color = &ledStripStatusModeConfig()->colors[i];
1709             sbufWriteU16(dst, color->h);
1710             sbufWriteU8(dst, color->s);
1711             sbufWriteU8(dst, color->v);
1712         }
1713         break;
1714 #endif
1715
1716 #ifdef USE_LED_STRIP
1717     case MSP_LED_STRIP_CONFIG:
1718         for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
1719 #ifdef USE_LED_STRIP_STATUS_MODE
1720             const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[i];
1721             sbufWriteU32(dst, *ledConfig);
1722 #else
1723             sbufWriteU32(dst, 0);
1724 #endif
1725         }
1726
1727         // API 1.41 - add indicator for advanced profile support and the current profile selection
1728         // 0 = basic ledstrip available
1729         // 1 = advanced ledstrip available
1730 #ifdef USE_LED_STRIP_STATUS_MODE
1731         sbufWriteU8(dst, 1);   // advanced ledstrip available
1732 #else
1733         sbufWriteU8(dst, 0);   // only simple ledstrip available
1734 #endif
1735         sbufWriteU8(dst, ledStripConfig()->ledstrip_profile);
1736         break;
1737 #endif
1738
1739 #ifdef USE_LED_STRIP_STATUS_MODE
1740     case MSP_LED_STRIP_MODECOLOR:
1741         for (int i = 0; i < LED_MODE_COUNT; i++) {
1742             for (int j = 0; j < LED_DIRECTION_COUNT; j++) {
1743                 sbufWriteU8(dst, i);
1744                 sbufWriteU8(dst, j);
1745                 sbufWriteU8(dst, ledStripStatusModeConfig()->modeColors[i].color[j]);
1746             }
1747         }
1748
1749         for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) {
1750             sbufWriteU8(dst, LED_MODE_COUNT);
1751             sbufWriteU8(dst, j);
1752             sbufWriteU8(dst, ledStripStatusModeConfig()->specialColors.color[j]);
1753         }
1754
1755         sbufWriteU8(dst, LED_AUX_CHANNEL);
1756         sbufWriteU8(dst, 0);
1757         sbufWriteU8(dst, ledStripStatusModeConfig()->ledstrip_aux_channel);
1758         break;
1759 #endif
1760
1761     case MSP_DATAFLASH_SUMMARY:
1762         serializeDataflashSummaryReply(dst);
1763         break;
1764
1765     case MSP_BLACKBOX_CONFIG:
1766 #ifdef USE_BLACKBOX
1767         sbufWriteU8(dst, 1); //Blackbox supported
1768         sbufWriteU8(dst, blackboxConfig()->device);
1769         sbufWriteU8(dst, 1); // Rate numerator, not used anymore
1770         sbufWriteU8(dst, blackboxGetRateDenom());
1771         sbufWriteU16(dst, blackboxGetPRatio());
1772         sbufWriteU8(dst, blackboxConfig()->sample_rate);
1773         // Added in MSP API 1.45
1774         sbufWriteU32(dst, blackboxConfig()->fields_disabled_mask);
1775 #else
1776         sbufWriteU8(dst, 0); // Blackbox not supported
1777         sbufWriteU8(dst, 0);
1778         sbufWriteU8(dst, 0);
1779         sbufWriteU8(dst, 0);
1780         sbufWriteU16(dst, 0);
1781         sbufWriteU8(dst, 0);
1782         // Added in MSP API 1.45
1783         sbufWriteU32(dst, 0);
1784 #endif
1785         break;
1786
1787     case MSP_SDCARD_SUMMARY:
1788         serializeSDCardSummaryReply(dst);
1789         break;
1790
1791     case MSP_MOTOR_3D_CONFIG:
1792         sbufWriteU16(dst, flight3DConfig()->deadband3d_low);
1793         sbufWriteU16(dst, flight3DConfig()->deadband3d_high);
1794         sbufWriteU16(dst, flight3DConfig()->neutral3d);
1795         break;
1796
1797     case MSP_RC_DEADBAND:
1798         sbufWriteU8(dst, rcControlsConfig()->deadband);
1799         sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
1800         sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband);
1801         sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle);
1802         break;
1803
1804
1805     case MSP_SENSOR_ALIGNMENT: {
1806         uint8_t gyroAlignment;
1807 #ifdef USE_MULTI_GYRO
1808         switch (gyroConfig()->gyro_to_use) {
1809         case GYRO_CONFIG_USE_GYRO_2:
1810             gyroAlignment = gyroDeviceConfig(1)->alignment;
1811             break;
1812         case GYRO_CONFIG_USE_GYRO_BOTH:
1813             // for dual-gyro in "BOTH" mode we only read/write gyro 0
1814         default:
1815             gyroAlignment = gyroDeviceConfig(0)->alignment;
1816             break;
1817         }
1818 #else
1819         gyroAlignment = gyroDeviceConfig(0)->alignment;
1820 #endif
1821         sbufWriteU8(dst, gyroAlignment);
1822         sbufWriteU8(dst, gyroAlignment);  // Starting with 4.0 gyro and acc alignment are the same
1823 #if defined(USE_MAG)
1824         sbufWriteU8(dst, compassConfig()->mag_alignment);
1825 #else
1826         sbufWriteU8(dst, 0);
1827 #endif
1828
1829         // API 1.41 - Add multi-gyro indicator, selected gyro, and support for separate gyro 1 & 2 alignment
1830         sbufWriteU8(dst, getGyroDetectionFlags());
1831 #ifdef USE_MULTI_GYRO
1832         sbufWriteU8(dst, gyroConfig()->gyro_to_use);
1833         sbufWriteU8(dst, gyroDeviceConfig(0)->alignment);
1834         sbufWriteU8(dst, gyroDeviceConfig(1)->alignment);
1835 #else
1836         sbufWriteU8(dst, GYRO_CONFIG_USE_GYRO_1);
1837         sbufWriteU8(dst, gyroDeviceConfig(0)->alignment);
1838         sbufWriteU8(dst, ALIGN_DEFAULT);
1839 #endif
1840
1841         break;
1842     }
1843     case MSP_ADVANCED_CONFIG:
1844         sbufWriteU8(dst, 1);  // was gyroConfig()->gyro_sync_denom - removed in API 1.43
1845         sbufWriteU8(dst, pidConfig()->pid_process_denom);
1846         sbufWriteU8(dst, motorConfig()->dev.useUnsyncedPwm);
1847         sbufWriteU8(dst, motorConfig()->dev.motorPwmProtocol);
1848         sbufWriteU16(dst, motorConfig()->dev.motorPwmRate);
1849         sbufWriteU16(dst, motorConfig()->digitalIdleOffsetValue);
1850         sbufWriteU8(dst, 0); // DEPRECATED: gyro_use_32kHz
1851         sbufWriteU8(dst, motorConfig()->dev.motorPwmInversion);
1852         sbufWriteU8(dst, gyroConfig()->gyro_to_use);
1853         sbufWriteU8(dst, gyroConfig()->gyro_high_fsr);
1854         sbufWriteU8(dst, gyroConfig()->gyroMovementCalibrationThreshold);
1855         sbufWriteU16(dst, gyroConfig()->gyroCalibrationDuration);
1856         sbufWriteU16(dst, gyroConfig()->gyro_offset_yaw);
1857         sbufWriteU8(dst, gyroConfig()->checkOverflow);
1858         //Added in MSP API 1.42
1859         sbufWriteU8(dst, systemConfig()->debug_mode);
1860         sbufWriteU8(dst, DEBUG_COUNT);
1861
1862         break;
1863     case MSP_FILTER_CONFIG :
1864         sbufWriteU8(dst, gyroConfig()->gyro_lpf1_static_hz);
1865         sbufWriteU16(dst, currentPidProfile->dterm_lpf1_static_hz);
1866         sbufWriteU16(dst, currentPidProfile->yaw_lowpass_hz);
1867         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1);
1868         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1);
1869         sbufWriteU16(dst, currentPidProfile->dterm_notch_hz);
1870         sbufWriteU16(dst, currentPidProfile->dterm_notch_cutoff);
1871         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2);
1872         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2);
1873         sbufWriteU8(dst, currentPidProfile->dterm_lpf1_type);
1874         sbufWriteU8(dst, gyroConfig()->gyro_hardware_lpf);
1875         sbufWriteU8(dst, 0); // DEPRECATED: gyro_32khz_hardware_lpf
1876         sbufWriteU16(dst, gyroConfig()->gyro_lpf1_static_hz);
1877         sbufWriteU16(dst, gyroConfig()->gyro_lpf2_static_hz);
1878         sbufWriteU8(dst, gyroConfig()->gyro_lpf1_type);
1879         sbufWriteU8(dst, gyroConfig()->gyro_lpf2_type);
1880         sbufWriteU16(dst, currentPidProfile->dterm_lpf2_static_hz);
1881         // Added in MSP API 1.41
1882         sbufWriteU8(dst, currentPidProfile->dterm_lpf2_type);
1883 #if defined(USE_DYN_LPF)
1884         sbufWriteU16(dst, gyroConfig()->gyro_lpf1_dyn_min_hz);
1885         sbufWriteU16(dst, gyroConfig()->gyro_lpf1_dyn_max_hz);
1886         sbufWriteU16(dst, currentPidProfile->dterm_lpf1_dyn_min_hz);
1887         sbufWriteU16(dst, currentPidProfile->dterm_lpf1_dyn_max_hz);
1888 #else
1889         sbufWriteU16(dst, 0);
1890         sbufWriteU16(dst, 0);
1891         sbufWriteU16(dst, 0);
1892         sbufWriteU16(dst, 0);
1893 #endif
1894         // Added in MSP API 1.42
1895 #if defined(USE_DYN_NOTCH_FILTER)
1896         sbufWriteU8(dst, 0);  // DEPRECATED 1.43: dyn_notch_range
1897         sbufWriteU8(dst, 0);  // DEPRECATED 1.44: dyn_notch_width_percent
1898         sbufWriteU16(dst, dynNotchConfig()->dyn_notch_q);
1899         sbufWriteU16(dst, dynNotchConfig()->dyn_notch_min_hz);
1900 #else
1901         sbufWriteU8(dst, 0);
1902         sbufWriteU8(dst, 0);
1903         sbufWriteU16(dst, 0);
1904         sbufWriteU16(dst, 0);
1905 #endif
1906 #if defined(USE_RPM_FILTER)
1907         sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_harmonics);
1908         sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_min_hz);
1909 #else
1910         sbufWriteU8(dst, 0);
1911         sbufWriteU8(dst, 0);
1912 #endif
1913 #if defined(USE_DYN_NOTCH_FILTER)
1914         // Added in MSP API 1.43
1915         sbufWriteU16(dst, dynNotchConfig()->dyn_notch_max_hz);
1916 #else
1917         sbufWriteU16(dst, 0);
1918 #endif
1919 #if defined(USE_DYN_LPF)
1920         // Added in MSP API 1.44
1921         sbufWriteU8(dst, currentPidProfile->dterm_lpf1_dyn_expo);
1922 #else
1923         sbufWriteU8(dst, 0);
1924 #endif
1925 #if defined(USE_DYN_NOTCH_FILTER)
1926         sbufWriteU8(dst, dynNotchConfig()->dyn_notch_count);
1927 #else
1928         sbufWriteU8(dst, 0);
1929 #endif
1930
1931         break;
1932     case MSP_PID_ADVANCED:
1933         sbufWriteU16(dst, 0);
1934         sbufWriteU16(dst, 0);
1935         sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit
1936         sbufWriteU8(dst, 0); // reserved
1937         sbufWriteU8(dst, 0); // was vbatPidCompensation
1938 #if defined(USE_FEEDFORWARD)
1939         sbufWriteU8(dst, currentPidProfile->feedforward_transition);
1940 #else
1941         sbufWriteU8(dst, 0);
1942 #endif
1943         sbufWriteU8(dst, 0); // was low byte of currentPidProfile->dtermSetpointWeight
1944         sbufWriteU8(dst, 0); // reserved
1945         sbufWriteU8(dst, 0); // reserved
1946         sbufWriteU8(dst, 0); // reserved
1947         sbufWriteU16(dst, currentPidProfile->rateAccelLimit);
1948         sbufWriteU16(dst, currentPidProfile->yawRateAccelLimit);
1949         sbufWriteU8(dst, currentPidProfile->levelAngleLimit);
1950         sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity
1951         sbufWriteU16(dst, 0); // was currentPidProfile->itermThrottleThreshold
1952         sbufWriteU16(dst, currentPidProfile->anti_gravity_gain);
1953         sbufWriteU16(dst, 0); // was currentPidProfile->dtermSetpointWeight
1954         sbufWriteU8(dst, currentPidProfile->iterm_rotation);
1955         sbufWriteU8(dst, 0); // was currentPidProfile->smart_feedforward
1956 #if defined(USE_ITERM_RELAX)
1957         sbufWriteU8(dst, currentPidProfile->iterm_relax);
1958         sbufWriteU8(dst, currentPidProfile->iterm_relax_type);
1959 #else
1960         sbufWriteU8(dst, 0);
1961         sbufWriteU8(dst, 0);
1962 #endif
1963 #if defined(USE_ABSOLUTE_CONTROL)
1964         sbufWriteU8(dst, currentPidProfile->abs_control_gain);
1965 #else
1966         sbufWriteU8(dst, 0);
1967 #endif
1968 #if defined(USE_THROTTLE_BOOST)
1969         sbufWriteU8(dst, currentPidProfile->throttle_boost);
1970 #else
1971         sbufWriteU8(dst, 0);
1972 #endif
1973 #if defined(USE_ACRO_TRAINER)
1974         sbufWriteU8(dst, currentPidProfile->acro_trainer_angle_limit);
1975 #else
1976         sbufWriteU8(dst, 0);
1977 #endif
1978         sbufWriteU16(dst, currentPidProfile->pid[PID_ROLL].F);
1979         sbufWriteU16(dst, currentPidProfile->pid[PID_PITCH].F);
1980         sbufWriteU16(dst, currentPidProfile->pid[PID_YAW].F);
1981         sbufWriteU8(dst, 0); // was currentPidProfile->antiGravityMode
1982 #if defined(USE_D_MIN)
1983         sbufWriteU8(dst, currentPidProfile->d_min[PID_ROLL]);
1984         sbufWriteU8(dst, currentPidProfile->d_min[PID_PITCH]);
1985         sbufWriteU8(dst, currentPidProfile->d_min[PID_YAW]);
1986         sbufWriteU8(dst, currentPidProfile->d_min_gain);
1987         sbufWriteU8(dst, currentPidProfile->d_min_advance);
1988 #else
1989         sbufWriteU8(dst, 0);
1990         sbufWriteU8(dst, 0);
1991         sbufWriteU8(dst, 0);
1992         sbufWriteU8(dst, 0);
1993         sbufWriteU8(dst, 0);
1994 #endif
1995 #if defined(USE_INTEGRATED_YAW_CONTROL)
1996         sbufWriteU8(dst, currentPidProfile->use_integrated_yaw);
1997         sbufWriteU8(dst, currentPidProfile->integrated_yaw_relax);
1998 #else
1999         sbufWriteU8(dst, 0);
2000         sbufWriteU8(dst, 0);
2001 #endif
2002 #if defined(USE_ITERM_RELAX)
2003         // Added in MSP API 1.42
2004         sbufWriteU8(dst, currentPidProfile->iterm_relax_cutoff);
2005 #else
2006         sbufWriteU8(dst, 0);
2007 #endif
2008         // Added in MSP API 1.43
2009         sbufWriteU8(dst, currentPidProfile->motor_output_limit);
2010         sbufWriteU8(dst, currentPidProfile->auto_profile_cell_count);
2011 #if defined(USE_DYN_IDLE)
2012         sbufWriteU8(dst, currentPidProfile->dyn_idle_min_rpm);
2013 #else
2014         sbufWriteU8(dst, 0);
2015 #endif
2016         // Added in MSP API 1.44
2017 #if defined(USE_FEEDFORWARD)
2018         sbufWriteU8(dst, currentPidProfile->feedforward_averaging);
2019         sbufWriteU8(dst, currentPidProfile->feedforward_smooth_factor);
2020         sbufWriteU8(dst, currentPidProfile->feedforward_boost);
2021         sbufWriteU8(dst, currentPidProfile->feedforward_max_rate_limit);
2022         sbufWriteU8(dst, currentPidProfile->feedforward_jitter_factor);
2023 #else
2024         sbufWriteU8(dst, 0);
2025         sbufWriteU8(dst, 0);
2026         sbufWriteU8(dst, 0);
2027         sbufWriteU8(dst, 0);
2028         sbufWriteU8(dst, 0);
2029 #endif
2030 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
2031         sbufWriteU8(dst, currentPidProfile->vbat_sag_compensation);
2032 #else
2033         sbufWriteU8(dst, 0);
2034 #endif
2035 #if defined(USE_THRUST_LINEARIZATION)
2036         sbufWriteU8(dst, currentPidProfile->thrustLinearization);
2037 #else
2038         sbufWriteU8(dst, 0);
2039 #endif
2040         sbufWriteU8(dst, currentPidProfile->tpa_mode);
2041         sbufWriteU8(dst, currentPidProfile->tpa_rate);
2042         sbufWriteU16(dst, currentPidProfile->tpa_breakpoint);   // was currentControlRateProfile->tpa_breakpoint
2043         break;
2044     case MSP_SENSOR_CONFIG:
2045 #if defined(USE_ACC)
2046         sbufWriteU8(dst, accelerometerConfig()->acc_hardware);
2047 #else
2048         sbufWriteU8(dst, 0);
2049 #endif
2050 #ifdef USE_BARO
2051         sbufWriteU8(dst, barometerConfig()->baro_hardware);
2052 #else
2053         sbufWriteU8(dst, BARO_NONE);
2054 #endif
2055 #ifdef USE_MAG
2056         sbufWriteU8(dst, compassConfig()->mag_hardware);
2057 #else
2058         sbufWriteU8(dst, MAG_NONE);
2059 #endif
2060         break;
2061
2062 #if defined(USE_VTX_COMMON)
2063     case MSP_VTX_CONFIG:
2064         {
2065             const vtxDevice_t *vtxDevice = vtxCommonDevice();
2066             unsigned vtxStatus = 0;
2067             vtxDevType_e vtxType = VTXDEV_UNKNOWN;
2068             uint8_t deviceIsReady = 0;
2069             if (vtxDevice) {
2070                 vtxCommonGetStatus(vtxDevice, &vtxStatus);
2071                 vtxType = vtxCommonGetDeviceType(vtxDevice);
2072                 deviceIsReady = vtxCommonDeviceIsReady(vtxDevice) ? 1 : 0;
2073             }
2074             sbufWriteU8(dst, vtxType);
2075             sbufWriteU8(dst, vtxSettingsConfig()->band);
2076             sbufWriteU8(dst, vtxSettingsConfig()->channel);
2077             sbufWriteU8(dst, vtxSettingsConfig()->power);
2078             sbufWriteU8(dst, (vtxStatus & VTX_STATUS_PIT_MODE) ? 1 : 0);
2079             sbufWriteU16(dst, vtxSettingsConfig()->freq);
2080             sbufWriteU8(dst, deviceIsReady);
2081             sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm);
2082
2083             // API version 1.42
2084             sbufWriteU16(dst, vtxSettingsConfig()->pitModeFreq);
2085 #ifdef USE_VTX_TABLE
2086             sbufWriteU8(dst, 1);   // vtxtable is available
2087             sbufWriteU8(dst, vtxTableConfig()->bands);
2088             sbufWriteU8(dst, vtxTableConfig()->channels);
2089             sbufWriteU8(dst, vtxTableConfig()->powerLevels);
2090 #else
2091             sbufWriteU8(dst, 0);
2092             sbufWriteU8(dst, 0);
2093             sbufWriteU8(dst, 0);
2094             sbufWriteU8(dst, 0);
2095 #endif
2096 #ifdef USE_VTX_MSP
2097             setMspVtxDeviceStatusReady(srcDesc);
2098 #endif
2099         }
2100         break;
2101 #endif
2102
2103     case MSP_TX_INFO:
2104         sbufWriteU8(dst, rssiSource);
2105         uint8_t rtcDateTimeIsSet = 0;
2106 #ifdef USE_RTC_TIME
2107         dateTime_t dt;
2108         if (rtcGetDateTime(&dt)) {
2109             rtcDateTimeIsSet = 1;
2110         }
2111 #else
2112         rtcDateTimeIsSet = RTC_NOT_SUPPORTED;
2113 #endif
2114         sbufWriteU8(dst, rtcDateTimeIsSet);
2115
2116         break;
2117 #ifdef USE_RTC_TIME
2118     case MSP_RTC:
2119         {
2120             dateTime_t dt;
2121             if (rtcGetDateTime(&dt)) {
2122                 sbufWriteU16(dst, dt.year);
2123                 sbufWriteU8(dst, dt.month);
2124                 sbufWriteU8(dst, dt.day);
2125                 sbufWriteU8(dst, dt.hours);
2126                 sbufWriteU8(dst, dt.minutes);
2127                 sbufWriteU8(dst, dt.seconds);
2128                 sbufWriteU16(dst, dt.millis);
2129             }
2130         }
2131
2132         break;
2133 #endif
2134     default:
2135         unsupportedCommand = true;
2136     }
2137     return !unsupportedCommand;
2138 }
2139
2140
2141 #ifdef USE_SIMPLIFIED_TUNING
2142 // Reads simplified PID tuning values from MSP buffer
2143 static void readSimplifiedPids(pidProfile_t* pidProfile, sbuf_t *src)
2144 {
2145     pidProfile->simplified_pids_mode = sbufReadU8(src);
2146     pidProfile->simplified_master_multiplier = sbufReadU8(src);
2147     pidProfile->simplified_roll_pitch_ratio = sbufReadU8(src);
2148     pidProfile->simplified_i_gain = sbufReadU8(src);
2149     pidProfile->simplified_d_gain = sbufReadU8(src);
2150     pidProfile->simplified_pi_gain = sbufReadU8(src);
2151 #ifdef USE_D_MIN
2152     pidProfile->simplified_dmin_ratio = sbufReadU8(src);
2153 #else
2154     sbufReadU8(src);
2155 #endif
2156     pidProfile->simplified_feedforward_gain = sbufReadU8(src);
2157     pidProfile->simplified_pitch_pi_gain = sbufReadU8(src);
2158     sbufReadU32(src); // reserved for future use
2159     sbufReadU32(src); // reserved for future use
2160 }
2161
2162 // Writes simplified PID tuning values to MSP buffer
2163 static void writeSimplifiedPids(const pidProfile_t *pidProfile, sbuf_t *dst)
2164 {
2165     sbufWriteU8(dst, pidProfile->simplified_pids_mode);
2166     sbufWriteU8(dst, pidProfile->simplified_master_multiplier);
2167     sbufWriteU8(dst, pidProfile->simplified_roll_pitch_ratio);
2168     sbufWriteU8(dst, pidProfile->simplified_i_gain);
2169     sbufWriteU8(dst, pidProfile->simplified_d_gain);
2170     sbufWriteU8(dst, pidProfile->simplified_pi_gain);
2171 #ifdef USE_D_MIN
2172     sbufWriteU8(dst, pidProfile->simplified_dmin_ratio);
2173 #else
2174     sbufWriteU8(dst, 0);
2175 #endif
2176     sbufWriteU8(dst, pidProfile->simplified_feedforward_gain);
2177     sbufWriteU8(dst, pidProfile->simplified_pitch_pi_gain);
2178     sbufWriteU32(dst, 0); // reserved for future use
2179     sbufWriteU32(dst, 0); // reserved for future use
2180 }
2181
2182 // Reads simplified Dterm Filter values from MSP buffer
2183 static void readSimplifiedDtermFilters(pidProfile_t* pidProfile, sbuf_t *src)
2184 {
2185     pidProfile->simplified_dterm_filter = sbufReadU8(src);
2186     pidProfile->simplified_dterm_filter_multiplier = sbufReadU8(src);
2187     pidProfile->dterm_lpf1_static_hz = sbufReadU16(src);
2188     pidProfile->dterm_lpf2_static_hz = sbufReadU16(src);
2189 #if defined(USE_DYN_LPF)
2190     pidProfile->dterm_lpf1_dyn_min_hz = sbufReadU16(src);
2191     pidProfile->dterm_lpf1_dyn_max_hz = sbufReadU16(src);
2192 #else
2193     sbufReadU16(src);
2194     sbufReadU16(src);
2195 #endif
2196     sbufReadU32(src); // reserved for future use
2197     sbufReadU32(src); // reserved for future use
2198 }
2199
2200 // Writes simplified Dterm Filter values into MSP buffer
2201 static void writeSimplifiedDtermFilters(const pidProfile_t* pidProfile, sbuf_t *dst)
2202 {
2203     sbufWriteU8(dst, pidProfile->simplified_dterm_filter);
2204     sbufWriteU8(dst, pidProfile->simplified_dterm_filter_multiplier);
2205     sbufWriteU16(dst, pidProfile->dterm_lpf1_static_hz);
2206     sbufWriteU16(dst, pidProfile->dterm_lpf2_static_hz);
2207 #if defined(USE_DYN_LPF)
2208     sbufWriteU16(dst, pidProfile->dterm_lpf1_dyn_min_hz);
2209     sbufWriteU16(dst, pidProfile->dterm_lpf1_dyn_max_hz);
2210 #else
2211     sbufWriteU16(dst, 0);
2212     sbufWriteU16(dst, 0);
2213 #endif
2214     sbufWriteU32(dst, 0); // reserved for future use
2215     sbufWriteU32(dst, 0); // reserved for future use
2216 }
2217
2218 // Writes simplified Gyro Filter values from MSP buffer
2219 static void readSimplifiedGyroFilters(gyroConfig_t *gyroConfig, sbuf_t *src)
2220 {
2221     gyroConfig->simplified_gyro_filter = sbufReadU8(src);
2222     gyroConfig->simplified_gyro_filter_multiplier = sbufReadU8(src);
2223     gyroConfig->gyro_lpf1_static_hz = sbufReadU16(src);
2224     gyroConfig->gyro_lpf2_static_hz = sbufReadU16(src);
2225 #if defined(USE_DYN_LPF)
2226     gyroConfig->gyro_lpf1_dyn_min_hz = sbufReadU16(src);
2227     gyroConfig->gyro_lpf1_dyn_max_hz = sbufReadU16(src);
2228 #else
2229     sbufReadU16(src);
2230     sbufReadU16(src);
2231 #endif
2232     sbufReadU32(src); // reserved for future use
2233     sbufReadU32(src); // reserved for future use
2234 }
2235
2236 // Writes simplified Gyro Filter values into MSP buffer
2237 static void writeSimplifiedGyroFilters(const gyroConfig_t *gyroConfig, sbuf_t *dst)
2238 {
2239     sbufWriteU8(dst, gyroConfig->simplified_gyro_filter);
2240     sbufWriteU8(dst, gyroConfig->simplified_gyro_filter_multiplier);
2241     sbufWriteU16(dst, gyroConfig->gyro_lpf1_static_hz);
2242     sbufWriteU16(dst, gyroConfig->gyro_lpf2_static_hz);
2243 #if defined(USE_DYN_LPF)
2244     sbufWriteU16(dst, gyroConfig->gyro_lpf1_dyn_min_hz);
2245     sbufWriteU16(dst, gyroConfig->gyro_lpf1_dyn_max_hz);
2246 #else
2247     sbufWriteU16(dst, 0);
2248     sbufWriteU16(dst, 0);
2249 #endif
2250     sbufWriteU32(dst, 0); // reserved for future use
2251     sbufWriteU32(dst, 0); // reserved for future use
2252 }
2253
2254 // writes results of simplified PID tuning values to MSP buffer
2255 static void writePidfs(pidProfile_t* pidProfile, sbuf_t *dst)
2256 {
2257     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
2258         sbufWriteU8(dst, pidProfile->pid[i].P);
2259         sbufWriteU8(dst, pidProfile->pid[i].I);
2260         sbufWriteU8(dst, pidProfile->pid[i].D);
2261         sbufWriteU8(dst, pidProfile->d_min[i]);
2262         sbufWriteU16(dst, pidProfile->pid[i].F);
2263     }
2264 }
2265 #endif // USE_SIMPLIFIED_TUNING
2266
2267 static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
2268 {
2269
2270     switch (cmdMSP) {
2271     case MSP_BOXNAMES:
2272         {
2273             const int page = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
2274             serializeBoxReply(dst, page, &serializeBoxNameFn);
2275         }
2276         break;
2277     case MSP_BOXIDS:
2278         {
2279             const int page = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
2280             serializeBoxReply(dst, page, &serializeBoxPermanentIdFn);
2281         }
2282         break;
2283     case MSP_REBOOT:
2284         if (sbufBytesRemaining(src)) {
2285             rebootMode = sbufReadU8(src);
2286
2287             if (rebootMode >= MSP_REBOOT_COUNT
2288 #if !defined(USE_USB_MSC)
2289                 || rebootMode == MSP_REBOOT_MSC || rebootMode == MSP_REBOOT_MSC_UTC
2290 #endif
2291                 ) {
2292                 return MSP_RESULT_ERROR;
2293             }
2294         } else {
2295             rebootMode = MSP_REBOOT_FIRMWARE;
2296         }
2297
2298         sbufWriteU8(dst, rebootMode);
2299
2300 #if defined(USE_USB_MSC)
2301         if (rebootMode == MSP_REBOOT_MSC) {
2302             if (mscCheckFilesystemReady()) {
2303                 sbufWriteU8(dst, 1);
2304             } else {
2305                 sbufWriteU8(dst, 0);
2306
2307                 return MSP_RESULT_ACK;
2308             }
2309         }
2310 #endif
2311
2312 #if defined(USE_MSP_OVER_TELEMETRY)
2313         if (featureIsEnabled(FEATURE_RX_SPI) && srcDesc == getMspTelemetryDescriptor()) {
2314             dispatchAdd(&mspRebootEntry, MSP_DISPATCH_DELAY_US);
2315         } else
2316 #endif
2317         if (mspPostProcessFn) {
2318             *mspPostProcessFn = mspRebootFn;
2319         }
2320
2321         break;
2322     case MSP_MULTIPLE_MSP:
2323         {
2324             uint8_t maxMSPs = 0;
2325             if (sbufBytesRemaining(src) == 0) {
2326                 return MSP_RESULT_ERROR;
2327             }
2328             int bytesRemaining = sbufBytesRemaining(dst) - 1; // need to keep one byte for checksum
2329             mspPacket_t packetIn, packetOut;
2330             sbufInit(&packetIn.buf, src->end, src->end);
2331             uint8_t* resetInputPtr = src->ptr;
2332             while (sbufBytesRemaining(src) && bytesRemaining > 0) {
2333                 uint8_t newMSP = sbufReadU8(src);
2334                 sbufInit(&packetOut.buf, dst->ptr, dst->end);
2335                 packetIn.cmd = newMSP;
2336                 mspFcProcessCommand(srcDesc, &packetIn, &packetOut, NULL);
2337                 uint8_t mspSize = sbufPtr(&packetOut.buf) - dst->ptr;
2338                 mspSize++; // need to add length information for each MSP
2339                 bytesRemaining -= mspSize;
2340                 if (bytesRemaining >= 0) {
2341                     maxMSPs++;
2342                 }
2343             }
2344             src->ptr = resetInputPtr;
2345             sbufInit(&packetOut.buf, dst->ptr, dst->end);
2346             for (int i = 0; i < maxMSPs; i++) {
2347                 uint8_t* sizePtr = sbufPtr(&packetOut.buf);
2348                 sbufWriteU8(&packetOut.buf, 0); // dummy
2349                 packetIn.cmd = sbufReadU8(src);
2350                 mspFcProcessCommand(srcDesc, &packetIn, &packetOut, NULL);
2351                 (*sizePtr) = sbufPtr(&packetOut.buf) - (sizePtr + 1);
2352             }
2353             dst->ptr = packetOut.buf.ptr;
2354         }
2355         break;
2356
2357 #ifdef USE_VTX_TABLE
2358     case MSP_VTXTABLE_BAND:
2359         {
2360             const uint8_t band = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
2361             if (band > 0 && band <= VTX_TABLE_MAX_BANDS) {
2362                 sbufWriteU8(dst, band);  // band number (same as request)
2363                 sbufWriteU8(dst, VTX_TABLE_BAND_NAME_LENGTH); // band name length
2364                 for (int i = 0; i < VTX_TABLE_BAND_NAME_LENGTH; i++) { // band name bytes
2365                     sbufWriteU8(dst, vtxTableConfig()->bandNames[band - 1][i]);
2366                 }
2367                 sbufWriteU8(dst, vtxTableConfig()->bandLetters[band - 1]); // band letter
2368                 sbufWriteU8(dst, vtxTableConfig()->isFactoryBand[band - 1]); // CUSTOM = 0; FACTORY = 1
2369                 sbufWriteU8(dst, vtxTableConfig()->channels); // number of channel frequencies to follow
2370                 for (int i = 0; i < vtxTableConfig()->channels; i++) { // the frequency for each channel
2371                     sbufWriteU16(dst, vtxTableConfig()->frequency[band - 1][i]);
2372                 }
2373             } else {
2374                 return MSP_RESULT_ERROR;
2375             }
2376 #ifdef USE_VTX_MSP
2377             setMspVtxDeviceStatusReady(srcDesc);
2378 #endif
2379         }
2380         break;
2381
2382     case MSP_VTXTABLE_POWERLEVEL:
2383         {
2384             const uint8_t powerLevel = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
2385             if (powerLevel > 0 && powerLevel <= VTX_TABLE_MAX_POWER_LEVELS) {
2386                 sbufWriteU8(dst, powerLevel);  // powerLevel number (same as request)
2387                 sbufWriteU16(dst, vtxTableConfig()->powerValues[powerLevel - 1]);
2388                 sbufWriteU8(dst, VTX_TABLE_POWER_LABEL_LENGTH); // powerLevel label length
2389                 for (int i = 0; i < VTX_TABLE_POWER_LABEL_LENGTH; i++) { // powerlevel label bytes
2390                     sbufWriteU8(dst, vtxTableConfig()->powerLabels[powerLevel - 1][i]);
2391                 }
2392             } else {
2393                 return MSP_RESULT_ERROR;
2394             }
2395 #ifdef USE_VTX_MSP
2396             setMspVtxDeviceStatusReady(srcDesc);
2397 #endif
2398         }
2399         break;
2400 #endif // USE_VTX_TABLE
2401
2402 #ifdef USE_SIMPLIFIED_TUNING
2403     // Added in MSP API 1.44
2404     case MSP_SIMPLIFIED_TUNING:
2405         {
2406             writeSimplifiedPids(currentPidProfile, dst);
2407             writeSimplifiedDtermFilters(currentPidProfile, dst);
2408             writeSimplifiedGyroFilters(gyroConfig(), dst);
2409         }
2410         break;
2411
2412     case MSP_CALCULATE_SIMPLIFIED_PID:
2413         {
2414             pidProfile_t tempPidProfile = *currentPidProfile;
2415             readSimplifiedPids(&tempPidProfile, src);
2416             applySimplifiedTuningPids(&tempPidProfile);
2417             writePidfs(&tempPidProfile, dst);
2418         }
2419         break;
2420
2421     case MSP_CALCULATE_SIMPLIFIED_DTERM:
2422         {
2423             pidProfile_t tempPidProfile = *currentPidProfile;
2424             readSimplifiedDtermFilters(&tempPidProfile, src);
2425             applySimplifiedTuningDtermFilters(&tempPidProfile);
2426             writeSimplifiedDtermFilters(&tempPidProfile, dst);
2427         }
2428         break;
2429
2430     case MSP_CALCULATE_SIMPLIFIED_GYRO:
2431         {
2432             gyroConfig_t tempGyroConfig = *gyroConfig();
2433             readSimplifiedGyroFilters(&tempGyroConfig, src);
2434             applySimplifiedTuningGyroFilters(&tempGyroConfig);
2435             writeSimplifiedGyroFilters(&tempGyroConfig, dst);
2436         }
2437         break;
2438
2439     case MSP_VALIDATE_SIMPLIFIED_TUNING:
2440         {
2441             pidProfile_t tempPidProfile = *currentPidProfile;
2442             applySimplifiedTuningPids(&tempPidProfile);
2443             bool result = true;
2444
2445             for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
2446                 result = result &&
2447                     tempPidProfile.pid[i].P == currentPidProfile->pid[i].P &&
2448                     tempPidProfile.pid[i].I == currentPidProfile->pid[i].I &&
2449                     tempPidProfile.pid[i].D == currentPidProfile->pid[i].D &&
2450                     tempPidProfile.d_min[i] == currentPidProfile->d_min[i] &&
2451                     tempPidProfile.pid[i].F == currentPidProfile->pid[i].F;
2452             }
2453
2454             sbufWriteU8(dst, result);
2455
2456             gyroConfig_t tempGyroConfig = *gyroConfig();
2457             applySimplifiedTuningGyroFilters(&tempGyroConfig);
2458             result =
2459                 tempGyroConfig.gyro_lpf1_static_hz == gyroConfig()->gyro_lpf1_static_hz &&
2460                 tempGyroConfig.gyro_lpf2_static_hz == gyroConfig()->gyro_lpf2_static_hz;
2461
2462 #if defined(USE_DYN_LPF)
2463             result = result &&
2464                 tempGyroConfig.gyro_lpf1_dyn_min_hz == gyroConfig()->gyro_lpf1_dyn_min_hz &&
2465                 tempGyroConfig.gyro_lpf1_dyn_max_hz == gyroConfig()->gyro_lpf1_dyn_max_hz;
2466 #endif
2467
2468             sbufWriteU8(dst, result);
2469
2470             applySimplifiedTuningDtermFilters(&tempPidProfile);
2471             result =
2472                 tempPidProfile.dterm_lpf1_static_hz == currentPidProfile->dterm_lpf1_static_hz &&
2473                 tempPidProfile.dterm_lpf2_static_hz == currentPidProfile->dterm_lpf2_static_hz;
2474
2475 #if defined(USE_DYN_LPF)
2476             result = result &&
2477                 tempPidProfile.dterm_lpf1_dyn_min_hz == currentPidProfile->dterm_lpf1_dyn_min_hz &&
2478                 tempPidProfile.dterm_lpf1_dyn_max_hz == currentPidProfile->dterm_lpf1_dyn_max_hz;
2479 #endif
2480
2481             sbufWriteU8(dst, result);
2482         }
2483         break;
2484 #endif
2485
2486     case MSP_RESET_CONF:
2487         {
2488 #if defined(USE_CUSTOM_DEFAULTS)
2489             defaultsType_e defaultsType = DEFAULTS_TYPE_CUSTOM;
2490 #endif
2491             if (sbufBytesRemaining(src) >= 1) {
2492                 // Added in MSP API 1.42
2493 #if defined(USE_CUSTOM_DEFAULTS)
2494                 defaultsType = sbufReadU8(src);
2495 #else
2496                 sbufReadU8(src);
2497 #endif
2498             }
2499
2500             bool success = false;
2501             if (!ARMING_FLAG(ARMED)) {
2502 #if defined(USE_CUSTOM_DEFAULTS)
2503                 success = resetEEPROM(defaultsType == DEFAULTS_TYPE_CUSTOM);
2504 #else
2505                 success = resetEEPROM(false);
2506 #endif
2507
2508                 if (success && mspPostProcessFn) {
2509                     rebootMode = MSP_REBOOT_FIRMWARE;
2510                     *mspPostProcessFn = mspRebootFn;
2511                 }
2512             }
2513
2514             // Added in API version 1.42
2515             sbufWriteU8(dst, success);
2516         }
2517
2518         break;
2519
2520     case MSP2_GET_TEXT:
2521         {
2522             // type byte, then length byte followed by the actual characters
2523             const uint8_t textType = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
2524
2525             char* textVar;
2526
2527             switch (textType) {
2528                 case MSP2TEXT_PILOT_NAME:
2529                     textVar = pilotConfigMutable()->pilotName;
2530                     break;
2531
2532                 case MSP2TEXT_CRAFT_NAME:
2533                     textVar = pilotConfigMutable()->craftName;
2534                     break;
2535
2536                 case MSP2TEXT_PID_PROFILE_NAME:
2537                     textVar = currentPidProfile->profileName;
2538                     break;
2539
2540                 case MSP2TEXT_RATE_PROFILE_NAME:
2541                     textVar = currentControlRateProfile->profileName;
2542                     break;
2543
2544                 default:
2545                     return MSP_RESULT_ERROR;
2546             }
2547
2548             const uint8_t textLength = strlen(textVar);
2549
2550             //  type byte, then length byte followed by the actual characters
2551             sbufWriteU8(dst, textType);
2552             sbufWriteU8(dst, textLength);
2553             for (unsigned int i = 0; i < textLength; i++) {
2554                 sbufWriteU8(dst, textVar[i]);
2555             }
2556         }
2557         break;
2558
2559     default:
2560         return MSP_RESULT_CMD_UNKNOWN;
2561     }
2562     return MSP_RESULT_ACK;
2563 }
2564
2565 #ifdef USE_FLASHFS
2566 static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
2567 {
2568     const unsigned int dataSize = sbufBytesRemaining(src);
2569     const uint32_t readAddress = sbufReadU32(src);
2570     uint16_t readLength;
2571     bool allowCompression = false;
2572     bool useLegacyFormat;
2573     if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
2574         readLength = sbufReadU16(src);
2575         if (sbufBytesRemaining(src)) {
2576             allowCompression = sbufReadU8(src);
2577         }
2578         useLegacyFormat = false;
2579     } else {
2580         readLength = 128;
2581         useLegacyFormat = true;
2582     }
2583
2584     serializeDataflashReadReply(dst, readAddress, readLength, useLegacyFormat, allowCompression);
2585 }
2586 #endif
2587
2588 static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src)
2589 {
2590     uint32_t i;
2591     uint8_t value;
2592     const unsigned int dataSize = sbufBytesRemaining(src);
2593     switch (cmdMSP) {
2594     case MSP_SELECT_SETTING:
2595         value = sbufReadU8(src);
2596         if ((value & RATEPROFILE_MASK) == 0) {
2597             if (!ARMING_FLAG(ARMED)) {
2598                 if (value >= PID_PROFILE_COUNT) {
2599                     value = 0;
2600                 }
2601                 changePidProfile(value);
2602             }
2603         } else {
2604             value = value & ~RATEPROFILE_MASK;
2605
2606             if (value >= CONTROL_RATE_PROFILE_COUNT) {
2607                 value = 0;
2608             }
2609             changeControlRateProfile(value);
2610         }
2611         break;
2612
2613     case MSP_COPY_PROFILE:
2614         value = sbufReadU8(src);        // 0 = pid profile, 1 = control rate profile
2615         uint8_t dstProfileIndex = sbufReadU8(src);
2616         uint8_t srcProfileIndex = sbufReadU8(src);
2617         if (value == 0) {
2618             pidCopyProfile(dstProfileIndex, srcProfileIndex);
2619         }
2620         else if (value == 1) {
2621             copyControlRateProfile(dstProfileIndex, srcProfileIndex);
2622         }
2623         break;
2624
2625 #if defined(USE_GPS) || defined(USE_MAG)
2626     case MSP_SET_HEADING:
2627         magHold = sbufReadU16(src);
2628         break;
2629 #endif
2630
2631     case MSP_SET_RAW_RC:
2632 #ifdef USE_RX_MSP
2633         {
2634             uint8_t channelCount = dataSize / sizeof(uint16_t);
2635             if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) {
2636                 return MSP_RESULT_ERROR;
2637             } else {
2638                 uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
2639                 for (int i = 0; i < channelCount; i++) {
2640                     frame[i] = sbufReadU16(src);
2641                 }
2642                 rxMspFrameReceive(frame, channelCount);
2643             }
2644         }
2645 #endif
2646         break;
2647 #if defined(USE_ACC)
2648     case MSP_SET_ACC_TRIM:
2649         accelerometerConfigMutable()->accelerometerTrims.values.pitch = sbufReadU16(src);
2650         accelerometerConfigMutable()->accelerometerTrims.values.roll  = sbufReadU16(src);
2651
2652         break;
2653 #endif
2654     case MSP_SET_ARMING_CONFIG:
2655         armingConfigMutable()->auto_disarm_delay = sbufReadU8(src);
2656         sbufReadU8(src); // reserved
2657         if (sbufBytesRemaining(src)) {
2658           imuConfigMutable()->small_angle = sbufReadU8(src);
2659         }
2660         break;
2661
2662     case MSP_SET_PID_CONTROLLER:
2663         break;
2664
2665     case MSP_SET_PID:
2666         for (int i = 0; i < PID_ITEM_COUNT; i++) {
2667             currentPidProfile->pid[i].P = sbufReadU8(src);
2668             currentPidProfile->pid[i].I = sbufReadU8(src);
2669             currentPidProfile->pid[i].D = sbufReadU8(src);
2670         }
2671         pidInitConfig(currentPidProfile);
2672         break;
2673
2674     case MSP_SET_MODE_RANGE:
2675         i = sbufReadU8(src);
2676         if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
2677             modeActivationCondition_t *mac = modeActivationConditionsMutable(i);
2678             i = sbufReadU8(src);
2679             const box_t *box = findBoxByPermanentId(i);
2680             if (box) {
2681                 mac->modeId = box->boxId;
2682                 mac->auxChannelIndex = sbufReadU8(src);
2683                 mac->range.startStep = sbufReadU8(src);
2684                 mac->range.endStep = sbufReadU8(src);
2685                 if (sbufBytesRemaining(src) != 0) {
2686                     mac->modeLogic = sbufReadU8(src);
2687
2688                     i = sbufReadU8(src);
2689                     mac->linkedTo = findBoxByPermanentId(i)->boxId;
2690                 }
2691                 rcControlsInit();
2692             } else {
2693                 return MSP_RESULT_ERROR;
2694             }
2695         } else {
2696             return MSP_RESULT_ERROR;
2697         }
2698         break;
2699
2700     case MSP_SET_ADJUSTMENT_RANGE:
2701         i = sbufReadU8(src);
2702         if (i < MAX_ADJUSTMENT_RANGE_COUNT) {
2703             adjustmentRange_t *adjRange = adjustmentRangesMutable(i);
2704             sbufReadU8(src); // was adjRange->adjustmentIndex
2705             adjRange->auxChannelIndex = sbufReadU8(src);
2706             adjRange->range.startStep = sbufReadU8(src);
2707             adjRange->range.endStep = sbufReadU8(src);
2708             adjRange->adjustmentConfig = sbufReadU8(src);
2709             adjRange->auxSwitchChannelIndex = sbufReadU8(src);
2710
2711             activeAdjustmentRangeReset();
2712         } else {
2713             return MSP_RESULT_ERROR;
2714         }
2715         break;
2716
2717     case MSP_SET_RC_TUNING:
2718         if (sbufBytesRemaining(src) >= 10) {
2719             value = sbufReadU8(src);
2720             if (currentControlRateProfile->rcRates[FD_PITCH] == currentControlRateProfile->rcRates[FD_ROLL]) {
2721                 currentControlRateProfile->rcRates[FD_PITCH] = value;
2722             }
2723             currentControlRateProfile->rcRates[FD_ROLL] = value;
2724
2725             value = sbufReadU8(src);
2726             if (currentControlRateProfile->rcExpo[FD_PITCH] == currentControlRateProfile->rcExpo[FD_ROLL]) {
2727                 currentControlRateProfile->rcExpo[FD_PITCH] = value;
2728             }
2729             currentControlRateProfile->rcExpo[FD_ROLL] = value;
2730
2731             for (int i = 0; i < 3; i++) {
2732                 currentControlRateProfile->rates[i] = sbufReadU8(src);
2733             }
2734
2735             sbufReadU8(src);    // tpa_rate is moved to PID profile
2736             currentControlRateProfile->thrMid8 = sbufReadU8(src);
2737             currentControlRateProfile->thrExpo8 = sbufReadU8(src);
2738             sbufReadU16(src);   // tpa_breakpoint is moved to PID profile
2739
2740             if (sbufBytesRemaining(src) >= 1) {
2741                 currentControlRateProfile->rcExpo[FD_YAW] = sbufReadU8(src);
2742             }
2743
2744             if (sbufBytesRemaining(src) >= 1) {
2745                 currentControlRateProfile->rcRates[FD_YAW] = sbufReadU8(src);
2746             }
2747
2748             if (sbufBytesRemaining(src) >= 1) {
2749                 currentControlRateProfile->rcRates[FD_PITCH] = sbufReadU8(src);
2750             }
2751
2752             if (sbufBytesRemaining(src) >= 1) {
2753                 currentControlRateProfile->rcExpo[FD_PITCH] = sbufReadU8(src);
2754             }
2755
2756             // version 1.41
2757             if (sbufBytesRemaining(src) >= 2) {
2758                 currentControlRateProfile->throttle_limit_type = sbufReadU8(src);
2759                 currentControlRateProfile->throttle_limit_percent = sbufReadU8(src);
2760             }
2761
2762             // version 1.42
2763             if (sbufBytesRemaining(src) >= 6) {
2764                 currentControlRateProfile->rate_limit[FD_ROLL] = sbufReadU16(src);
2765                 currentControlRateProfile->rate_limit[FD_PITCH] = sbufReadU16(src);
2766                 currentControlRateProfile->rate_limit[FD_YAW] = sbufReadU16(src);
2767             }
2768
2769             // version 1.43
2770             if (sbufBytesRemaining(src) >= 1) {
2771                 currentControlRateProfile->rates_type = sbufReadU8(src);
2772             }
2773
2774             initRcProcessing();
2775         } else {
2776             return MSP_RESULT_ERROR;
2777         }
2778         break;
2779
2780     case MSP_SET_MOTOR_CONFIG:
2781         motorConfigMutable()->minthrottle = sbufReadU16(src);
2782         motorConfigMutable()->maxthrottle = sbufReadU16(src);
2783         motorConfigMutable()->mincommand = sbufReadU16(src);
2784
2785         // version 1.42
2786         if (sbufBytesRemaining(src) >= 2) {
2787             motorConfigMutable()->motorPoleCount = sbufReadU8(src);
2788 #if defined(USE_DSHOT_TELEMETRY)
2789             motorConfigMutable()->dev.useDshotTelemetry = sbufReadU8(src);
2790 #else
2791             sbufReadU8(src);
2792 #endif
2793         }
2794         break;
2795
2796 #ifdef USE_GPS
2797     case MSP_SET_GPS_CONFIG:
2798         gpsConfigMutable()->provider = sbufReadU8(src);
2799         gpsConfigMutable()->sbasMode = sbufReadU8(src);
2800         gpsConfigMutable()->autoConfig = sbufReadU8(src);
2801         gpsConfigMutable()->autoBaud = sbufReadU8(src);
2802         if (sbufBytesRemaining(src) >= 2) {
2803             // Added in API version 1.43
2804             gpsConfigMutable()->gps_set_home_point_once = sbufReadU8(src);
2805             gpsConfigMutable()->gps_ublox_use_galileo = sbufReadU8(src);
2806         }
2807         break;
2808
2809 #ifdef USE_GPS_RESCUE
2810         case MSP_SET_GPS_RESCUE:
2811         gpsRescueConfigMutable()->maxRescueAngle = sbufReadU16(src);
2812         gpsRescueConfigMutable()->initialAltitudeM = sbufReadU16(src);
2813         gpsRescueConfigMutable()->descentDistanceM = sbufReadU16(src);
2814         gpsRescueConfigMutable()->rescueGroundspeed = sbufReadU16(src);
2815         gpsRescueConfigMutable()->throttleMin = sbufReadU16(src);
2816         gpsRescueConfigMutable()->throttleMax = sbufReadU16(src);
2817         gpsRescueConfigMutable()->throttleHover = sbufReadU16(src);
2818         gpsRescueConfigMutable()->sanityChecks = sbufReadU8(src);
2819         gpsRescueConfigMutable()->minSats = sbufReadU8(src);
2820         if (sbufBytesRemaining(src) >= 6) {
2821             // Added in API version 1.43
2822             gpsRescueConfigMutable()->ascendRate = sbufReadU16(src);
2823             gpsRescueConfigMutable()->descendRate = sbufReadU16(src);
2824             gpsRescueConfigMutable()->allowArmingWithoutFix = sbufReadU8(src);
2825             gpsRescueConfigMutable()->altitudeMode = sbufReadU8(src);
2826         }
2827         if (sbufBytesRemaining(src) >= 2) {
2828             // Added in API version 1.44
2829             gpsRescueConfigMutable()->minRescueDth = sbufReadU16(src);
2830         }
2831         break;
2832
2833     case MSP_SET_GPS_RESCUE_PIDS:
2834         gpsRescueConfigMutable()->throttleP = sbufReadU16(src);
2835         gpsRescueConfigMutable()->throttleI = sbufReadU16(src);
2836         gpsRescueConfigMutable()->throttleD = sbufReadU16(src);
2837         gpsRescueConfigMutable()->velP = sbufReadU16(src);
2838         gpsRescueConfigMutable()->velI = sbufReadU16(src);
2839         gpsRescueConfigMutable()->velD = sbufReadU16(src);
2840         gpsRescueConfigMutable()->yawP = sbufReadU16(src);
2841         break;
2842 #endif
2843 #endif
2844
2845     case MSP_SET_MOTOR:
2846         for (int i = 0; i < getMotorCount(); i++) {
2847             motor_disarmed[i] = motorConvertFromExternal(sbufReadU16(src));
2848         }
2849         break;
2850
2851     case MSP_SET_SERVO_CONFIGURATION:
2852 #ifdef USE_SERVOS
2853         if (dataSize != 1 + 12) {
2854             return MSP_RESULT_ERROR;
2855         }
2856         i = sbufReadU8(src);
2857         if (i >= MAX_SUPPORTED_SERVOS) {
2858             return MSP_RESULT_ERROR;
2859         } else {
2860             servoParamsMutable(i)->min = sbufReadU16(src);
2861             servoParamsMutable(i)->max = sbufReadU16(src);
2862             servoParamsMutable(i)->middle = sbufReadU16(src);
2863             servoParamsMutable(i)->rate = sbufReadU8(src);
2864             servoParamsMutable(i)->forwardFromChannel = sbufReadU8(src);
2865             servoParamsMutable(i)->reversedSources = sbufReadU32(src);
2866         }
2867 #endif
2868         break;
2869
2870     case MSP_SET_SERVO_MIX_RULE:
2871 #ifdef USE_SERVOS
2872         i = sbufReadU8(src);
2873         if (i >= MAX_SERVO_RULES) {
2874             return MSP_RESULT_ERROR;
2875         } else {
2876             customServoMixersMutable(i)->targetChannel = sbufReadU8(src);
2877             customServoMixersMutable(i)->inputSource = sbufReadU8(src);
2878             customServoMixersMutable(i)->rate = sbufReadU8(src);
2879             customServoMixersMutable(i)->speed = sbufReadU8(src);
2880             customServoMixersMutable(i)->min = sbufReadU8(src);
2881             customServoMixersMutable(i)->max = sbufReadU8(src);
2882             customServoMixersMutable(i)->box = sbufReadU8(src);
2883             loadCustomServoMixer();
2884         }
2885 #endif
2886         break;
2887
2888     case MSP_SET_MOTOR_3D_CONFIG:
2889         flight3DConfigMutable()->deadband3d_low = sbufReadU16(src);
2890         flight3DConfigMutable()->deadband3d_high = sbufReadU16(src);
2891         flight3DConfigMutable()->neutral3d = sbufReadU16(src);
2892         break;
2893
2894     case MSP_SET_RC_DEADBAND:
2895         rcControlsConfigMutable()->deadband = sbufReadU8(src);
2896         rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
2897         rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src);
2898         flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src);
2899         break;
2900
2901     case MSP_SET_RESET_CURR_PID:
2902         resetPidProfile(currentPidProfile);
2903         break;
2904
2905     case MSP_SET_SENSOR_ALIGNMENT: {
2906         // maintain backwards compatibility for API < 1.41
2907         const uint8_t gyroAlignment = sbufReadU8(src);
2908         sbufReadU8(src);  // discard deprecated acc_align
2909 #if defined(USE_MAG)
2910         compassConfigMutable()->mag_alignment = sbufReadU8(src);
2911 #else
2912         sbufReadU8(src);
2913 #endif
2914
2915         if (sbufBytesRemaining(src) >= 3) {
2916             // API >= 1.41 - support the gyro_to_use and alignment for gyros 1 & 2
2917 #ifdef USE_MULTI_GYRO
2918             gyroConfigMutable()->gyro_to_use = sbufReadU8(src);
2919             gyroDeviceConfigMutable(0)->alignment = sbufReadU8(src);
2920             gyroDeviceConfigMutable(1)->alignment = sbufReadU8(src);
2921 #else
2922             sbufReadU8(src);  // unused gyro_to_use
2923             gyroDeviceConfigMutable(0)->alignment = sbufReadU8(src);
2924             sbufReadU8(src);  // unused gyro_2_sensor_align
2925 #endif
2926         } else {
2927             // maintain backwards compatibility for API < 1.41
2928 #ifdef USE_MULTI_GYRO
2929             switch (gyroConfig()->gyro_to_use) {
2930             case GYRO_CONFIG_USE_GYRO_2:
2931                 gyroDeviceConfigMutable(1)->alignment = gyroAlignment;
2932                 break;
2933             case GYRO_CONFIG_USE_GYRO_BOTH:
2934                 // For dual-gyro in "BOTH" mode we'll only update gyro 0
2935             default:
2936                 gyroDeviceConfigMutable(0)->alignment = gyroAlignment;
2937                 break;
2938             }
2939 #else
2940             gyroDeviceConfigMutable(0)->alignment = gyroAlignment;
2941 #endif
2942
2943         }
2944         break;
2945     }
2946
2947     case MSP_SET_ADVANCED_CONFIG:
2948         sbufReadU8(src); // was gyroConfigMutable()->gyro_sync_denom - removed in API 1.43
2949         pidConfigMutable()->pid_process_denom = sbufReadU8(src);
2950         motorConfigMutable()->dev.useUnsyncedPwm = sbufReadU8(src);
2951         motorConfigMutable()->dev.motorPwmProtocol = sbufReadU8(src);
2952         motorConfigMutable()->dev.motorPwmRate = sbufReadU16(src);
2953         if (sbufBytesRemaining(src) >= 2) {
2954             motorConfigMutable()->digitalIdleOffsetValue = sbufReadU16(src);
2955         }
2956         if (sbufBytesRemaining(src)) {
2957             sbufReadU8(src); // DEPRECATED: gyro_use_32khz
2958         }
2959         if (sbufBytesRemaining(src)) {
2960             motorConfigMutable()->dev.motorPwmInversion = sbufReadU8(src);
2961         }
2962         if (sbufBytesRemaining(src) >= 8) {
2963             gyroConfigMutable()->gyro_to_use = sbufReadU8(src);
2964             gyroConfigMutable()->gyro_high_fsr = sbufReadU8(src);
2965             gyroConfigMutable()->gyroMovementCalibrationThreshold = sbufReadU8(src);
2966             gyroConfigMutable()->gyroCalibrationDuration = sbufReadU16(src);
2967             gyroConfigMutable()->gyro_offset_yaw = sbufReadU16(src);
2968             gyroConfigMutable()->checkOverflow = sbufReadU8(src);
2969         }
2970         if (sbufBytesRemaining(src) >= 1) {
2971             //Added in MSP API 1.42
2972             systemConfigMutable()->debug_mode = sbufReadU8(src);
2973         }
2974
2975         validateAndFixGyroConfig();
2976
2977         break;
2978     case MSP_SET_FILTER_CONFIG:
2979         gyroConfigMutable()->gyro_lpf1_static_hz = sbufReadU8(src);
2980         currentPidProfile->dterm_lpf1_static_hz = sbufReadU16(src);
2981         currentPidProfile->yaw_lowpass_hz = sbufReadU16(src);
2982         if (sbufBytesRemaining(src) >= 8) {
2983             gyroConfigMutable()->gyro_soft_notch_hz_1 = sbufReadU16(src);
2984             gyroConfigMutable()->gyro_soft_notch_cutoff_1 = sbufReadU16(src);
2985             currentPidProfile->dterm_notch_hz = sbufReadU16(src);
2986             currentPidProfile->dterm_notch_cutoff = sbufReadU16(src);
2987         }
2988         if (sbufBytesRemaining(src) >= 4) {
2989             gyroConfigMutable()->gyro_soft_notch_hz_2 = sbufReadU16(src);
2990             gyroConfigMutable()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
2991         }
2992         if (sbufBytesRemaining(src) >= 1) {
2993             currentPidProfile->dterm_lpf1_type = sbufReadU8(src);
2994         }
2995         if (sbufBytesRemaining(src) >= 10) {
2996             gyroConfigMutable()->gyro_hardware_lpf = sbufReadU8(src);
2997             sbufReadU8(src); // DEPRECATED: gyro_32khz_hardware_lpf
2998             gyroConfigMutable()->gyro_lpf1_static_hz = sbufReadU16(src);
2999             gyroConfigMutable()->gyro_lpf2_static_hz = sbufReadU16(src);
3000             gyroConfigMutable()->gyro_lpf1_type = sbufReadU8(src);
3001             gyroConfigMutable()->gyro_lpf2_type = sbufReadU8(src);
3002             currentPidProfile->dterm_lpf2_static_hz = sbufReadU16(src);
3003         }
3004         if (sbufBytesRemaining(src) >= 9) {
3005             // Added in MSP API 1.41
3006             currentPidProfile->dterm_lpf2_type = sbufReadU8(src);
3007 #if defined(USE_DYN_LPF)
3008             gyroConfigMutable()->gyro_lpf1_dyn_min_hz = sbufReadU16(src);
3009             gyroConfigMutable()->gyro_lpf1_dyn_max_hz = sbufReadU16(src);
3010             currentPidProfile->dterm_lpf1_dyn_min_hz = sbufReadU16(src);
3011             currentPidProfile->dterm_lpf1_dyn_max_hz = sbufReadU16(src);
3012 #else
3013             sbufReadU16(src);
3014             sbufReadU16(src);
3015             sbufReadU16(src);
3016             sbufReadU16(src);
3017 #endif
3018         }
3019         if (sbufBytesRemaining(src) >= 8) {
3020             // Added in MSP API 1.42
3021 #if defined(USE_DYN_NOTCH_FILTER)
3022             sbufReadU8(src); // DEPRECATED 1.43: dyn_notch_range
3023             sbufReadU8(src); // DEPRECATED 1.44: dyn_notch_width_percent
3024             dynNotchConfigMutable()->dyn_notch_q = sbufReadU16(src);
3025             dynNotchConfigMutable()->dyn_notch_min_hz = sbufReadU16(src);
3026 #else
3027             sbufReadU8(src);
3028             sbufReadU8(src);
3029             sbufReadU16(src);
3030             sbufReadU16(src);
3031 #endif
3032 #if defined(USE_RPM_FILTER)
3033             rpmFilterConfigMutable()->rpm_filter_harmonics = sbufReadU8(src);
3034             rpmFilterConfigMutable()->rpm_filter_min_hz = sbufReadU8(src);
3035 #else
3036             sbufReadU8(src);
3037             sbufReadU8(src);
3038 #endif
3039         }
3040         if (sbufBytesRemaining(src) >= 2) {
3041 #if defined(USE_DYN_NOTCH_FILTER)
3042             // Added in MSP API 1.43
3043             dynNotchConfigMutable()->dyn_notch_max_hz = sbufReadU16(src);
3044 #else
3045             sbufReadU16(src);
3046 #endif
3047         }
3048         if (sbufBytesRemaining(src) >= 2) {
3049             // Added in MSP API 1.44
3050 #if defined(USE_DYN_LPF)
3051             currentPidProfile->dterm_lpf1_dyn_expo = sbufReadU8(src);
3052 #else
3053             sbufReadU8(src);
3054 #endif
3055 #if defined(USE_DYN_NOTCH_FILTER)
3056             dynNotchConfigMutable()->dyn_notch_count = sbufReadU8(src);
3057 #else
3058             sbufReadU8(src);
3059 #endif
3060         }
3061
3062         // reinitialize the gyro filters with the new values
3063         validateAndFixGyroConfig();
3064         gyroInitFilters();
3065         // reinitialize the PID filters with the new values
3066         pidInitFilters(currentPidProfile);
3067
3068         break;
3069     case MSP_SET_PID_ADVANCED:
3070         sbufReadU16(src);
3071         sbufReadU16(src);
3072         sbufReadU16(src); // was pidProfile.yaw_p_limit
3073         sbufReadU8(src); // reserved
3074         sbufReadU8(src); // was vbatPidCompensation
3075 #if defined(USE_FEEDFORWARD)
3076         currentPidProfile->feedforward_transition = sbufReadU8(src);
3077 #else
3078         sbufReadU8(src);
3079 #endif
3080         sbufReadU8(src); // was low byte of currentPidProfile->dtermSetpointWeight
3081         sbufReadU8(src); // reserved
3082         sbufReadU8(src); // reserved
3083         sbufReadU8(src); // reserved
3084         currentPidProfile->rateAccelLimit = sbufReadU16(src);
3085         currentPidProfile->yawRateAccelLimit = sbufReadU16(src);
3086         if (sbufBytesRemaining(src) >= 2) {
3087             currentPidProfile->levelAngleLimit = sbufReadU8(src);
3088             sbufReadU8(src); // was pidProfile.levelSensitivity
3089         }
3090         if (sbufBytesRemaining(src) >= 4) {
3091             sbufReadU16(src); // was currentPidProfile->itermThrottleThreshold
3092             currentPidProfile->anti_gravity_gain = sbufReadU16(src);
3093         }
3094         if (sbufBytesRemaining(src) >= 2) {
3095             sbufReadU16(src); // was currentPidProfile->dtermSetpointWeight
3096         }
3097         if (sbufBytesRemaining(src) >= 14) {
3098             // Added in MSP API 1.40
3099             currentPidProfile->iterm_rotation = sbufReadU8(src);
3100             sbufReadU8(src); // was currentPidProfile->smart_feedforward
3101 #if defined(USE_ITERM_RELAX)
3102             currentPidProfile->iterm_relax = sbufReadU8(src);
3103             currentPidProfile->iterm_relax_type = sbufReadU8(src);
3104 #else
3105             sbufReadU8(src);
3106             sbufReadU8(src);
3107 #endif
3108 #if defined(USE_ABSOLUTE_CONTROL)
3109             currentPidProfile->abs_control_gain = sbufReadU8(src);
3110 #else
3111             sbufReadU8(src);
3112 #endif
3113 #if defined(USE_THROTTLE_BOOST)
3114             currentPidProfile->throttle_boost = sbufReadU8(src);
3115 #else
3116             sbufReadU8(src);
3117 #endif
3118 #if defined(USE_ACRO_TRAINER)
3119             currentPidProfile->acro_trainer_angle_limit = sbufReadU8(src);
3120 #else
3121             sbufReadU8(src);
3122 #endif
3123             // PID controller feedforward terms
3124             currentPidProfile->pid[PID_ROLL].F = sbufReadU16(src);
3125             currentPidProfile->pid[PID_PITCH].F = sbufReadU16(src);
3126             currentPidProfile->pid[PID_YAW].F = sbufReadU16(src);
3127             sbufReadU8(src); // was currentPidProfile->antiGravityMode
3128         }
3129         if (sbufBytesRemaining(src) >= 7) {
3130             // Added in MSP API 1.41
3131 #if defined(USE_D_MIN)
3132             currentPidProfile->d_min[PID_ROLL] = sbufReadU8(src);
3133             currentPidProfile->d_min[PID_PITCH] = sbufReadU8(src);
3134             currentPidProfile->d_min[PID_YAW] = sbufReadU8(src);
3135             currentPidProfile->d_min_gain = sbufReadU8(src);
3136             currentPidProfile->d_min_advance = sbufReadU8(src);
3137 #else
3138             sbufReadU8(src);
3139             sbufReadU8(src);
3140             sbufReadU8(src);
3141             sbufReadU8(src);
3142             sbufReadU8(src);
3143 #endif
3144 #if defined(USE_INTEGRATED_YAW_CONTROL)
3145             currentPidProfile->use_integrated_yaw = sbufReadU8(src);
3146             currentPidProfile->integrated_yaw_relax = sbufReadU8(src);
3147 #else
3148             sbufReadU8(src);
3149             sbufReadU8(src);
3150 #endif
3151         }
3152         if(sbufBytesRemaining(src) >= 1) {
3153             // Added in MSP API 1.42
3154 #if defined(USE_ITERM_RELAX)
3155             currentPidProfile->iterm_relax_cutoff = sbufReadU8(src);
3156 #else
3157             sbufReadU8(src);
3158 #endif
3159         }
3160         if (sbufBytesRemaining(src) >= 3) {
3161             // Added in MSP API 1.43
3162             currentPidProfile->motor_output_limit = sbufReadU8(src);
3163             currentPidProfile->auto_profile_cell_count = sbufReadU8(src);
3164 #if defined(USE_DYN_IDLE)
3165             currentPidProfile->dyn_idle_min_rpm = sbufReadU8(src);
3166 #else
3167             sbufReadU8(src);
3168 #endif
3169         }
3170         if (sbufBytesRemaining(src) >= 7) {
3171             // Added in MSP API 1.44
3172 #if defined(USE_FEEDFORWARD)
3173             currentPidProfile->feedforward_averaging = sbufReadU8(src);
3174             currentPidProfile->feedforward_smooth_factor = sbufReadU8(src);
3175             currentPidProfile->feedforward_boost = sbufReadU8(src);
3176             currentPidProfile->feedforward_max_rate_limit = sbufReadU8(src);
3177             currentPidProfile->feedforward_jitter_factor = sbufReadU8(src);
3178 #else
3179             sbufReadU8(src);
3180             sbufReadU8(src);
3181             sbufReadU8(src);
3182             sbufReadU8(src);
3183             sbufReadU8(src);
3184 #endif
3185
3186 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
3187             currentPidProfile->vbat_sag_compensation = sbufReadU8(src);
3188 #else
3189             sbufReadU8(src);
3190 #endif
3191 #if defined(USE_THRUST_LINEARIZATION)
3192             currentPidProfile->thrustLinearization = sbufReadU8(src);
3193 #else
3194             sbufReadU8(src);
3195 #endif
3196         }
3197         if (sbufBytesRemaining(src) >= 4) {
3198             // Added in API 1.45
3199             currentPidProfile->tpa_mode = sbufReadU8(src);
3200             currentPidProfile->tpa_rate = MIN(sbufReadU8(src), TPA_MAX);
3201             currentPidProfile->tpa_breakpoint = sbufReadU16(src);
3202         }
3203
3204         pidInitConfig(currentPidProfile);
3205         initEscEndpoints();
3206         mixerInitProfile();
3207
3208         break;
3209     case MSP_SET_SENSOR_CONFIG:
3210 #if defined(USE_ACC)
3211         accelerometerConfigMutable()->acc_hardware = sbufReadU8(src);
3212 #else
3213         sbufReadU8(src);
3214 #endif
3215 #if defined(USE_BARO)
3216         barometerConfigMutable()->baro_hardware = sbufReadU8(src);
3217 #else
3218         sbufReadU8(src);
3219 #endif
3220 #if defined(USE_MAG)
3221         compassConfigMutable()->mag_hardware = sbufReadU8(src);
3222 #else
3223         sbufReadU8(src);
3224 #endif
3225         break;
3226
3227 #ifdef USE_ACC
3228     case MSP_ACC_CALIBRATION:
3229         if (!ARMING_FLAG(ARMED))
3230             accStartCalibration();
3231         break;
3232 #endif
3233
3234 #if defined(USE_MAG)
3235     case MSP_MAG_CALIBRATION:
3236         if (!ARMING_FLAG(ARMED)) {
3237             compassStartCalibration();
3238         }
3239 #endif
3240
3241         break;
3242     case MSP_EEPROM_WRITE:
3243         if (ARMING_FLAG(ARMED)) {
3244             return MSP_RESULT_ERROR;
3245         }
3246
3247         // This is going to take some time and won't be done where real-time performance is needed so
3248         // ignore how long it takes to avoid confusing the scheduler
3249         schedulerIgnoreTaskStateTime();
3250
3251 #if defined(USE_MSP_OVER_TELEMETRY)
3252         if (featureIsEnabled(FEATURE_RX_SPI) && srcDesc == getMspTelemetryDescriptor()) {
3253             dispatchAdd(&writeReadEepromEntry, MSP_DISPATCH_DELAY_US);
3254         } else
3255 #endif
3256         {
3257             writeReadEeprom(NULL);
3258         }
3259
3260         break;
3261
3262 #ifdef USE_BLACKBOX
3263     case MSP_SET_BLACKBOX_CONFIG:
3264         // Don't allow config to be updated while Blackbox is logging
3265         if (blackboxMayEditConfig()) {
3266             blackboxConfigMutable()->device = sbufReadU8(src);
3267             const int rateNum = sbufReadU8(src); // was rate_num
3268             const int rateDenom = sbufReadU8(src); // was rate_denom
3269             uint16_t pRatio = 0;
3270             if (sbufBytesRemaining(src) >= 2) {
3271                 // p_ratio specified, so use it directly
3272                 pRatio = sbufReadU16(src);
3273             } else {
3274                 // p_ratio not specified in MSP, so calculate it from old rateNum and rateDenom
3275                 pRatio = blackboxCalculatePDenom(rateNum, rateDenom);
3276             }
3277
3278             if (sbufBytesRemaining(src) >= 1) {
3279                 // sample_rate specified, so use it directly
3280                 blackboxConfigMutable()->sample_rate = sbufReadU8(src);
3281             } else {
3282                 // sample_rate not specified in MSP, so calculate it from old p_ratio
3283                 blackboxConfigMutable()->sample_rate = blackboxCalculateSampleRate(pRatio);
3284             }
3285
3286             // Added in MSP API 1.45
3287             if (sbufBytesRemaining(src) >= 4) {
3288                 blackboxConfigMutable()->fields_disabled_mask = sbufReadU32(src);
3289             }
3290         }
3291         break;
3292 #endif
3293
3294 #ifdef USE_VTX_COMMON
3295     case MSP_SET_VTX_CONFIG:
3296         {
3297             vtxDevice_t *vtxDevice = vtxCommonDevice();
3298             vtxDevType_e vtxType = VTXDEV_UNKNOWN;
3299             if (vtxDevice) {
3300                 vtxType = vtxCommonGetDeviceType(vtxDevice);
3301             }
3302             uint16_t newFrequency = sbufReadU16(src);
3303             if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) {  // Value is band and channel
3304                 const uint8_t newBand = (newFrequency / 8) + 1;
3305                 const uint8_t newChannel = (newFrequency % 8) + 1;
3306                 vtxSettingsConfigMutable()->band = newBand;
3307                 vtxSettingsConfigMutable()->channel = newChannel;
3308                 vtxSettingsConfigMutable()->freq = vtxCommonLookupFrequency(vtxDevice, newBand, newChannel);
3309             } else if (newFrequency <= VTX_SETTINGS_MAX_FREQUENCY_MHZ) { // Value is frequency in MHz
3310                 vtxSettingsConfigMutable()->band = 0;
3311                 vtxSettingsConfigMutable()->freq = newFrequency;
3312             }
3313
3314             if (sbufBytesRemaining(src) >= 2) {
3315                 vtxSettingsConfigMutable()->power = sbufReadU8(src);
3316                 const uint8_t newPitmode = sbufReadU8(src);
3317                 if (vtxType != VTXDEV_UNKNOWN) {
3318                     // Delegate pitmode to vtx directly
3319                     unsigned vtxCurrentStatus;
3320                     vtxCommonGetStatus(vtxDevice, &vtxCurrentStatus);
3321                     if ((bool)(vtxCurrentStatus & VTX_STATUS_PIT_MODE) != (bool)newPitmode) {
3322                         vtxCommonSetPitMode(vtxDevice, newPitmode);
3323                     }
3324                 }
3325             }
3326
3327             if (sbufBytesRemaining(src)) {
3328                     vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src);
3329             }
3330
3331             // API version 1.42 - this parameter kept separate since clients may already be supplying
3332             if (sbufBytesRemaining(src) >= 2) {
3333                 vtxSettingsConfigMutable()->pitModeFreq = sbufReadU16(src);
3334             }
3335
3336             // API version 1.42 - extensions for non-encoded versions of the band, channel or frequency
3337             if (sbufBytesRemaining(src) >= 4) {
3338                 // Added standalone values for band, channel and frequency to move
3339                 // away from the flawed encoded combined method originally implemented.
3340                 uint8_t newBand = sbufReadU8(src);
3341                 const uint8_t newChannel = sbufReadU8(src);
3342                 uint16_t newFreq = sbufReadU16(src);
3343                 if (newBand) {
3344                     newFreq = vtxCommonLookupFrequency(vtxDevice, newBand, newChannel);
3345                 }
3346                 vtxSettingsConfigMutable()->band = newBand;
3347                 vtxSettingsConfigMutable()->channel = newChannel;
3348                 vtxSettingsConfigMutable()->freq = newFreq;
3349             }
3350
3351             // API version 1.42 - extensions for vtxtable support
3352             if (sbufBytesRemaining(src) >= 4) {
3353 #ifdef USE_VTX_TABLE
3354                 const uint8_t newBandCount = sbufReadU8(src);
3355                 const uint8_t newChannelCount = sbufReadU8(src);
3356                 const uint8_t newPowerCount = sbufReadU8(src);
3357
3358                 if ((newBandCount > VTX_TABLE_MAX_BANDS) ||
3359                     (newChannelCount > VTX_TABLE_MAX_CHANNELS) ||
3360                     (newPowerCount > VTX_TABLE_MAX_POWER_LEVELS)) {
3361                     return MSP_RESULT_ERROR;
3362                 }
3363                 vtxTableConfigMutable()->bands = newBandCount;
3364                 vtxTableConfigMutable()->channels = newChannelCount;
3365                 vtxTableConfigMutable()->powerLevels = newPowerCount;
3366
3367                 // boolean to determine whether the vtxtable should be cleared in
3368                 // expectation that the detailed band/channel and power level messages
3369                 // will follow to repopulate the tables
3370                 if (sbufReadU8(src)) {
3371                     for (int i = 0; i < VTX_TABLE_MAX_BANDS; i++) {
3372                         vtxTableConfigClearBand(vtxTableConfigMutable(), i);
3373                         vtxTableConfigClearChannels(vtxTableConfigMutable(), i, 0);
3374                     }
3375                     vtxTableConfigClearPowerLabels(vtxTableConfigMutable(), 0);
3376                     vtxTableConfigClearPowerValues(vtxTableConfigMutable(), 0);
3377                 }
3378 #else
3379                 sbufReadU8(src);
3380                 sbufReadU8(src);
3381                 sbufReadU8(src);
3382                 sbufReadU8(src);
3383 #endif
3384             }
3385 #ifdef USE_VTX_MSP
3386             setMspVtxDeviceStatusReady(srcDesc);
3387 #endif
3388         }
3389         break;
3390 #endif
3391
3392 #ifdef USE_VTX_TABLE
3393     case MSP_SET_VTXTABLE_BAND:
3394         {
3395             char bandName[VTX_TABLE_BAND_NAME_LENGTH + 1];
3396             memset(bandName, 0, VTX_TABLE_BAND_NAME_LENGTH + 1);
3397             uint16_t frequencies[VTX_TABLE_MAX_CHANNELS];
3398             const uint8_t band = sbufReadU8(src);
3399             const uint8_t bandNameLength = sbufReadU8(src);
3400             for (int i = 0; i < bandNameLength; i++) {
3401                 const char nameChar = sbufReadU8(src);
3402                 if (i < VTX_TABLE_BAND_NAME_LENGTH) {
3403                     bandName[i] = toupper(nameChar);
3404                 }
3405             }
3406             const char bandLetter = toupper(sbufReadU8(src));
3407             const bool isFactoryBand = (bool)sbufReadU8(src);
3408             const uint8_t channelCount = sbufReadU8(src);
3409             for (int i = 0; i < channelCount; i++) {
3410                 const uint16_t frequency = sbufReadU16(src);
3411                 if (i < vtxTableConfig()->channels) {
3412                     frequencies[i] = frequency;
3413                 }
3414             }
3415
3416             if (band > 0 && band <= vtxTableConfig()->bands) {
3417                 vtxTableStrncpyWithPad(vtxTableConfigMutable()->bandNames[band - 1], bandName, VTX_TABLE_BAND_NAME_LENGTH);
3418                 vtxTableConfigMutable()->bandLetters[band - 1] = bandLetter;
3419                 vtxTableConfigMutable()->isFactoryBand[band - 1] = isFactoryBand;
3420                 for (int i = 0; i < vtxTableConfig()->channels; i++) {
3421                     vtxTableConfigMutable()->frequency[band - 1][i] = frequencies[i];
3422                 }
3423                 // If this is the currently selected band then reset the frequency
3424                 if (band == vtxSettingsConfig()->band) {
3425                     uint16_t newFreq = 0;
3426                     if (vtxSettingsConfig()->channel > 0 && vtxSettingsConfig()->channel <= vtxTableConfig()->channels) {
3427                         newFreq = frequencies[vtxSettingsConfig()->channel - 1];
3428                     }
3429                     vtxSettingsConfigMutable()->freq = newFreq;
3430                 }
3431                 vtxTableNeedsInit = true;  // reinintialize vtxtable after eeprom write
3432             } else {
3433                 return MSP_RESULT_ERROR;
3434             }
3435 #ifdef USE_VTX_MSP
3436             setMspVtxDeviceStatusReady(srcDesc);
3437 #endif
3438         }
3439         break;
3440
3441     case MSP_SET_VTXTABLE_POWERLEVEL:
3442         {
3443             char powerLevelLabel[VTX_TABLE_POWER_LABEL_LENGTH + 1];
3444             memset(powerLevelLabel, 0, VTX_TABLE_POWER_LABEL_LENGTH + 1);
3445             const uint8_t powerLevel = sbufReadU8(src);
3446             const uint16_t powerValue = sbufReadU16(src);
3447             const uint8_t powerLevelLabelLength = sbufReadU8(src);
3448             for (int i = 0; i < powerLevelLabelLength; i++) {
3449                 const char labelChar = sbufReadU8(src);
3450                 if (i < VTX_TABLE_POWER_LABEL_LENGTH) {
3451                     powerLevelLabel[i] = toupper(labelChar);
3452                 }
3453             }
3454
3455             if (powerLevel > 0 && powerLevel <= vtxTableConfig()->powerLevels) {
3456                 vtxTableConfigMutable()->powerValues[powerLevel - 1] = powerValue;
3457                 vtxTableStrncpyWithPad(vtxTableConfigMutable()->powerLabels[powerLevel - 1], powerLevelLabel, VTX_TABLE_POWER_LABEL_LENGTH);
3458                 vtxTableNeedsInit = true;  // reinintialize vtxtable after eeprom write
3459             } else {
3460                 return MSP_RESULT_ERROR;
3461             }
3462 #ifdef USE_VTX_MSP
3463             setMspVtxDeviceStatusReady(srcDesc);
3464 #endif
3465         }
3466         break;
3467 #endif
3468
3469     case MSP2_SET_MOTOR_OUTPUT_REORDERING:
3470         {
3471             const uint8_t arraySize = sbufReadU8(src);
3472
3473             for (unsigned i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
3474                 uint8_t value = i;
3475
3476                 if (i < arraySize) {
3477                     value = sbufReadU8(src);
3478                 }
3479
3480                 motorConfigMutable()->dev.motorOutputReordering[i] = value;
3481             }
3482         }
3483         break;
3484
3485 #ifdef USE_DSHOT
3486     case MSP2_SEND_DSHOT_COMMAND:
3487         {
3488             const bool armed = ARMING_FLAG(ARMED);
3489
3490             if (!armed) {
3491                 const uint8_t commandType = sbufReadU8(src);
3492                 const uint8_t motorIndex = sbufReadU8(src);
3493                 const uint8_t commandCount = sbufReadU8(src);
3494
3495                 if (DSHOT_CMD_TYPE_BLOCKING == commandType) {
3496                     motorDisable();
3497                 }
3498
3499                 for (uint8_t i = 0; i < commandCount; i++) {
3500                     const uint8_t commandIndex = sbufReadU8(src);
3501                     dshotCommandWrite(motorIndex, getMotorCount(), commandIndex, commandType);
3502                 }
3503
3504                 if (DSHOT_CMD_TYPE_BLOCKING == commandType) {
3505                     motorEnable();
3506                 }
3507             }
3508         }
3509         break;
3510 #endif
3511
3512 #ifdef USE_SIMPLIFIED_TUNING
3513     // Added in MSP API 1.44
3514     case MSP_SET_SIMPLIFIED_TUNING:
3515         {
3516             readSimplifiedPids(currentPidProfile, src);
3517             readSimplifiedDtermFilters(currentPidProfile, src);
3518             readSimplifiedGyroFilters(gyroConfigMutable(), src);
3519             applySimplifiedTuning(currentPidProfile, gyroConfigMutable());
3520         }
3521         break;
3522 #endif
3523
3524 #ifdef USE_CAMERA_CONTROL
3525     case MSP_CAMERA_CONTROL:
3526         {
3527             if (ARMING_FLAG(ARMED)) {
3528                 return MSP_RESULT_ERROR;
3529             }
3530
3531             const uint8_t key = sbufReadU8(src);
3532             cameraControlKeyPress(key, 0);
3533         }
3534         break;
3535 #endif
3536
3537     case MSP_SET_ARMING_DISABLED:
3538         {
3539             const uint8_t command = sbufReadU8(src);
3540             uint8_t disableRunawayTakeoff = 0;
3541 #ifndef USE_RUNAWAY_TAKEOFF
3542             UNUSED(disableRunawayTakeoff);
3543 #endif
3544             if (sbufBytesRemaining(src)) {
3545                 disableRunawayTakeoff = sbufReadU8(src);
3546             }
3547             if (command) {
3548                 mspArmingDisableByDescriptor(srcDesc);
3549                 setArmingDisabled(ARMING_DISABLED_MSP);
3550                 if (ARMING_FLAG(ARMED)) {
3551                     disarm(DISARM_REASON_ARMING_DISABLED);
3552                 }
3553 #ifdef USE_RUNAWAY_TAKEOFF
3554                 runawayTakeoffTemporaryDisable(false);
3555 #endif
3556             } else {
3557                 mspArmingEnableByDescriptor(srcDesc);
3558                 if (mspIsMspArmingEnabled()) {
3559                     unsetArmingDisabled(ARMING_DISABLED_MSP);
3560 #ifdef USE_RUNAWAY_TAKEOFF
3561                     runawayTakeoffTemporaryDisable(disableRunawayTakeoff);
3562 #endif
3563                 }
3564             }
3565         }
3566         break;
3567
3568 #if defined(USE_FLASHFS) && defined(USE_BLACKBOX)
3569     case MSP_DATAFLASH_ERASE:
3570         blackboxEraseAll();
3571
3572         break;
3573 #endif
3574
3575 #ifdef USE_GPS
3576     case MSP_SET_RAW_GPS:
3577         gpsSetFixState(sbufReadU8(src));
3578         gpsSol.numSat = sbufReadU8(src);
3579         gpsSol.llh.lat = sbufReadU32(src);
3580         gpsSol.llh.lon = sbufReadU32(src);
3581         gpsSol.llh.altCm = sbufReadU16(src) * 100; // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. Received MSP altitudes in 1m per lsb have to upscaled.
3582         gpsSol.groundSpeed = sbufReadU16(src);
3583         GPS_update |= GPS_MSP_UPDATE;        // MSP data signalisation to GPS functions
3584         break;
3585 #endif // USE_GPS
3586     case MSP_SET_FEATURE_CONFIG:
3587         featureConfigReplace(sbufReadU32(src));
3588         break;
3589
3590 #ifdef USE_BEEPER
3591     case MSP_SET_BEEPER_CONFIG:
3592         beeperConfigMutable()->beeper_off_flags = sbufReadU32(src);
3593         if (sbufBytesRemaining(src) >= 1) {
3594             beeperConfigMutable()->dshotBeaconTone = sbufReadU8(src);
3595         }
3596         if (sbufBytesRemaining(src) >= 4) {
3597             beeperConfigMutable()->dshotBeaconOffFlags = sbufReadU32(src);
3598         }
3599         break;
3600 #endif
3601
3602     case MSP_SET_BOARD_ALIGNMENT_CONFIG:
3603         boardAlignmentMutable()->rollDegrees = sbufReadU16(src);
3604         boardAlignmentMutable()->pitchDegrees = sbufReadU16(src);
3605         boardAlignmentMutable()->yawDegrees = sbufReadU16(src);
3606         break;
3607
3608     case MSP_SET_MIXER_CONFIG:
3609 #ifndef USE_QUAD_MIXER_ONLY
3610         mixerConfigMutable()->mixerMode = sbufReadU8(src);
3611 #else
3612         sbufReadU8(src);
3613 #endif
3614         if (sbufBytesRemaining(src) >= 1) {
3615             mixerConfigMutable()->yaw_motors_reversed = sbufReadU8(src);
3616         }
3617         break;
3618
3619     case MSP_SET_RX_CONFIG:
3620         rxConfigMutable()->serialrx_provider = sbufReadU8(src);
3621         rxConfigMutable()->maxcheck = sbufReadU16(src);
3622         rxConfigMutable()->midrc = sbufReadU16(src);
3623         rxConfigMutable()->mincheck = sbufReadU16(src);
3624         rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src);
3625         if (sbufBytesRemaining(src) >= 4) {
3626             rxConfigMutable()->rx_min_usec = sbufReadU16(src);
3627             rxConfigMutable()->rx_max_usec = sbufReadU16(src);
3628         }
3629         if (sbufBytesRemaining(src) >= 4) {
3630             sbufReadU8(src); // not required in API 1.44, was rxConfigMutable()->rcInterpolation
3631             sbufReadU8(src); // not required in API 1.44, was rxConfigMutable()->rcInterpolationInterval
3632             rxConfigMutable()->airModeActivateThreshold = (sbufReadU16(src) - 1000) / 10;
3633         }
3634         if (sbufBytesRemaining(src) >= 6) {
3635 #ifdef USE_RX_SPI
3636             rxSpiConfigMutable()->rx_spi_protocol = sbufReadU8(src);
3637             rxSpiConfigMutable()->rx_spi_id = sbufReadU32(src);
3638             rxSpiConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(src);
3639 #else
3640             sbufReadU8(src);
3641             sbufReadU32(src);
3642             sbufReadU8(src);
3643 #endif
3644         }
3645         if (sbufBytesRemaining(src) >= 1) {
3646             rxConfigMutable()->fpvCamAngleDegrees = sbufReadU8(src);
3647         }
3648         if (sbufBytesRemaining(src) >= 6) {
3649             // Added in MSP API 1.40
3650             sbufReadU8(src); // not required in API 1.44, was rxConfigMutable()->rcSmoothingChannels
3651 #if defined(USE_RC_SMOOTHING_FILTER)
3652             sbufReadU8(src); // not required in API 1.44, was rc_smoothing_type
3653             configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_setpoint_cutoff, sbufReadU8(src));
3654             configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_feedforward_cutoff, sbufReadU8(src));
3655             sbufReadU8(src); // not required in API 1.44, was rc_smoothing_input_type
3656             sbufReadU8(src); // not required in API 1.44, was rc_smoothing_derivative_type
3657 #else
3658             sbufReadU8(src);
3659             sbufReadU8(src);
3660             sbufReadU8(src);
3661             sbufReadU8(src);
3662             sbufReadU8(src);
3663 #endif
3664         }
3665         if (sbufBytesRemaining(src) >= 1) {
3666             // Added in MSP API 1.40
3667             // Kept separate from the section above to work around missing Configurator support in version < 10.4.2
3668 #if defined(USE_USB_CDC_HID)
3669             usbDevConfigMutable()->type = sbufReadU8(src);
3670 #else
3671             sbufReadU8(src);
3672 #endif
3673         }
3674         if (sbufBytesRemaining(src) >= 1) {
3675             // Added in MSP API 1.42
3676 #if defined(USE_RC_SMOOTHING_FILTER)
3677             // Added extra validation/range constraint for rc_smoothing_auto_factor as a workaround for a bug in
3678             // the 10.6 configurator where it was possible to submit an invalid out-of-range value. We might be
3679             // able to remove the constraint at some point in the future once the affected versions are deprecated
3680             // enough that the risk is low.
3681             configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_auto_factor_rpy, constrain(sbufReadU8(src), RC_SMOOTHING_AUTO_FACTOR_MIN, RC_SMOOTHING_AUTO_FACTOR_MAX));
3682 #else
3683             sbufReadU8(src);
3684 #endif
3685         }
3686         if (sbufBytesRemaining(src) >= 1) {
3687             // Added in MSP API 1.44
3688 #if defined(USE_RC_SMOOTHING_FILTER)
3689             configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_mode, sbufReadU8(src));
3690 #else
3691             sbufReadU8(src);
3692 #endif
3693         }
3694         if (sbufBytesRemaining(src) >= 6) {
3695             // Added in MSP API 1.45
3696 #ifdef USE_RX_EXPRESSLRS
3697             sbufReadData(src, rxExpressLrsSpiConfigMutable()->UID, 6);
3698 #else
3699             uint8_t emptyUid[6];
3700             sbufReadData(src, emptyUid, 6);
3701 #endif
3702         }
3703         break;
3704     case MSP_SET_FAILSAFE_CONFIG:
3705         failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
3706         failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src);
3707         failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src);
3708         failsafeConfigMutable()->failsafe_switch_mode = sbufReadU8(src);
3709         failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src);
3710         failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src);
3711         break;
3712
3713     case MSP_SET_RXFAIL_CONFIG:
3714         i = sbufReadU8(src);
3715         if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) {
3716             rxFailsafeChannelConfigsMutable(i)->mode = sbufReadU8(src);
3717             rxFailsafeChannelConfigsMutable(i)->step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
3718         } else {
3719             return MSP_RESULT_ERROR;
3720         }
3721         break;
3722
3723     case MSP_SET_RSSI_CONFIG:
3724         rxConfigMutable()->rssi_channel = sbufReadU8(src);
3725         break;
3726
3727     case MSP_SET_RX_MAP:
3728         for (int i = 0; i < RX_MAPPABLE_CHANNEL_COUNT; i++) {
3729             rxConfigMutable()->rcmap[i] = sbufReadU8(src);
3730         }
3731         break;
3732
3733     case MSP_SET_CF_SERIAL_CONFIG:
3734         {
3735             uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4);
3736
3737             if (dataSize % portConfigSize != 0) {
3738                 return MSP_RESULT_ERROR;
3739             }
3740
3741             uint8_t remainingPortsInPacket = dataSize / portConfigSize;
3742
3743             while (remainingPortsInPacket--) {
3744                 uint8_t identifier = sbufReadU8(src);
3745
3746                 serialPortConfig_t *portConfig = serialFindPortConfigurationMutable(identifier);
3747
3748                 if (!portConfig) {
3749                     return MSP_RESULT_ERROR;
3750                 }
3751
3752                 portConfig->identifier = identifier;
3753                 portConfig->functionMask = sbufReadU16(src);
3754                 portConfig->msp_baudrateIndex = sbufReadU8(src);
3755                 portConfig->gps_baudrateIndex = sbufReadU8(src);
3756                 portConfig->telemetry_baudrateIndex = sbufReadU8(src);
3757                 portConfig->blackbox_baudrateIndex = sbufReadU8(src);
3758             }
3759         }
3760         break;
3761     case MSP2_COMMON_SET_SERIAL_CONFIG: {
3762         if (dataSize < 1) {
3763             return MSP_RESULT_ERROR;
3764         }
3765         unsigned count = sbufReadU8(src);
3766         unsigned portConfigSize = (dataSize - 1) / count;
3767         unsigned expectedPortSize = sizeof(uint8_t) + sizeof(uint32_t) + (sizeof(uint8_t) * 4);
3768         if (portConfigSize < expectedPortSize) {
3769             return MSP_RESULT_ERROR;
3770         }
3771         for (unsigned ii = 0; ii < count; ii++) {
3772             unsigned start = sbufBytesRemaining(src);
3773             uint8_t identifier = sbufReadU8(src);
3774             serialPortConfig_t *portConfig = serialFindPortConfigurationMutable(identifier);
3775
3776             if (!portConfig) {
3777                 return MSP_RESULT_ERROR;
3778             }
3779
3780             portConfig->identifier = identifier;
3781             portConfig->functionMask = sbufReadU32(src);
3782             portConfig->msp_baudrateIndex = sbufReadU8(src);
3783             portConfig->gps_baudrateIndex = sbufReadU8(src);
3784             portConfig->telemetry_baudrateIndex = sbufReadU8(src);
3785             portConfig->blackbox_baudrateIndex = sbufReadU8(src);
3786             // Skip unknown bytes
3787             while (start - sbufBytesRemaining(src) < portConfigSize && sbufBytesRemaining(src)) {
3788                 sbufReadU8(src);
3789             }
3790         }
3791         break;
3792     }
3793
3794 #ifdef USE_LED_STRIP_STATUS_MODE
3795     case MSP_SET_LED_COLORS:
3796         for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
3797             hsvColor_t *color = &ledStripStatusModeConfigMutable()->colors[i];
3798             color->h = sbufReadU16(src);
3799             color->s = sbufReadU8(src);
3800             color->v = sbufReadU8(src);
3801         }
3802         break;
3803 #endif
3804
3805 #ifdef USE_LED_STRIP
3806     case MSP_SET_LED_STRIP_CONFIG:
3807         {
3808             i = sbufReadU8(src);
3809             if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) {
3810                 return MSP_RESULT_ERROR;
3811             }
3812 #ifdef USE_LED_STRIP_STATUS_MODE
3813             ledConfig_t *ledConfig = &ledStripStatusModeConfigMutable()->ledConfigs[i];
3814             *ledConfig = sbufReadU32(src);
3815             reevaluateLedConfig();
3816 #else
3817             sbufReadU32(src);
3818 #endif
3819             // API 1.41 - selected ledstrip_profile
3820             if (sbufBytesRemaining(src) >= 1) {
3821                 ledStripConfigMutable()->ledstrip_profile = sbufReadU8(src);
3822             }
3823         }
3824         break;
3825 #endif
3826
3827 #ifdef USE_LED_STRIP_STATUS_MODE
3828     case MSP_SET_LED_STRIP_MODECOLOR:
3829         {
3830             ledModeIndex_e modeIdx = sbufReadU8(src);
3831             int funIdx = sbufReadU8(src);
3832             int color = sbufReadU8(src);
3833
3834             if (!setModeColor(modeIdx, funIdx, color)) {
3835                 return MSP_RESULT_ERROR;
3836             }
3837         }
3838         break;
3839 #endif
3840
3841     case MSP_SET_NAME:
3842         memset(pilotConfigMutable()->craftName, 0, ARRAYLEN(pilotConfig()->craftName));
3843         for (unsigned int i = 0; i < MIN(MAX_NAME_LENGTH, dataSize); i++) {
3844             pilotConfigMutable()->craftName[i] = sbufReadU8(src);
3845         }
3846 #ifdef USE_OSD
3847         osdAnalyzeActiveElements();
3848 #endif
3849         break;
3850
3851 #ifdef USE_RTC_TIME
3852     case MSP_SET_RTC:
3853         {
3854             // Use seconds and milliseconds to make senders
3855             // easier to implement. Generating a 64 bit value
3856             // might not be trivial in some platforms.
3857             int32_t secs = (int32_t)sbufReadU32(src);
3858             uint16_t millis = sbufReadU16(src);
3859             rtcTime_t t = rtcTimeMake(secs, millis);
3860             rtcSet(&t);
3861         }
3862
3863         break;
3864 #endif
3865
3866     case MSP_SET_TX_INFO:
3867         setRssiMsp(sbufReadU8(src));
3868
3869         break;
3870
3871 #if defined(USE_BOARD_INFO)
3872     case MSP_SET_BOARD_INFO:
3873         if (!boardInformationIsSet()) {
3874             uint8_t length = sbufReadU8(src);
3875             char boardName[MAX_BOARD_NAME_LENGTH + 1];
3876             sbufReadData(src, boardName, MIN(length, MAX_BOARD_NAME_LENGTH));
3877             if (length > MAX_BOARD_NAME_LENGTH) {
3878                 sbufAdvance(src, length - MAX_BOARD_NAME_LENGTH);
3879                 length = MAX_BOARD_NAME_LENGTH;
3880             }
3881             boardName[length] = '\0';
3882             length = sbufReadU8(src);
3883             char manufacturerId[MAX_MANUFACTURER_ID_LENGTH + 1];
3884             sbufReadData(src, manufacturerId, MIN(length, MAX_MANUFACTURER_ID_LENGTH));
3885             if (length > MAX_MANUFACTURER_ID_LENGTH) {
3886                 sbufAdvance(src, length - MAX_MANUFACTURER_ID_LENGTH);
3887                 length = MAX_MANUFACTURER_ID_LENGTH;
3888             }
3889             manufacturerId[length] = '\0';
3890
3891             setBoardName(boardName);
3892             setManufacturerId(manufacturerId);
3893             persistBoardInformation();
3894         } else {
3895             return MSP_RESULT_ERROR;
3896         }
3897
3898         break;
3899 #if defined(USE_SIGNATURE)
3900     case MSP_SET_SIGNATURE:
3901         if (!signatureIsSet()) {
3902             uint8_t signature[SIGNATURE_LENGTH];
3903             sbufReadData(src, signature, SIGNATURE_LENGTH);
3904             setSignature(signature);
3905             persistSignature();
3906         } else {
3907             return MSP_RESULT_ERROR;
3908         }
3909
3910         break;
3911 #endif
3912 #endif // USE_BOARD_INFO
3913 #if defined(USE_RX_BIND)
3914     case MSP2_BETAFLIGHT_BIND:
3915         if (!startRxBind()) {
3916             return MSP_RESULT_ERROR;
3917         }
3918
3919         break;
3920 #endif
3921
3922     case MSP2_SET_TEXT:
3923         {
3924             // type byte, then length byte followed by the actual characters
3925             const uint8_t textType = sbufReadU8(src);
3926
3927             char* textVar;
3928             const uint8_t textLength = MIN(MAX_NAME_LENGTH, sbufReadU8(src));
3929             switch (textType) {
3930                 case MSP2TEXT_PILOT_NAME:
3931                     textVar = pilotConfigMutable()->pilotName;
3932                     break;
3933
3934                 case MSP2TEXT_CRAFT_NAME:
3935                     textVar = pilotConfigMutable()->craftName;
3936                     break;
3937
3938                 case MSP2TEXT_PID_PROFILE_NAME:
3939                     textVar = currentPidProfile->profileName;
3940                     break;
3941
3942                 case MSP2TEXT_RATE_PROFILE_NAME:
3943                     textVar = currentControlRateProfile->profileName;
3944                     break;
3945
3946                 default:
3947                     return MSP_RESULT_ERROR;
3948             }
3949
3950             memset(textVar, 0, strlen(textVar));
3951             for (unsigned int i = 0; i < textLength; i++) {
3952                 textVar[i] = sbufReadU8(src);
3953             }
3954
3955 #ifdef USE_OSD
3956             if (textType == MSP2TEXT_PILOT_NAME || textType == MSP2TEXT_CRAFT_NAME) {
3957                 osdAnalyzeActiveElements();
3958             }
3959 #endif
3960         }
3961         break;
3962
3963     default:
3964         // we do not know how to handle the (valid) message, indicate error MSP $M!
3965         return MSP_RESULT_ERROR;
3966     }
3967     return MSP_RESULT_ACK;
3968 }
3969
3970 static mspResult_e mspCommonProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
3971 {
3972     UNUSED(mspPostProcessFn);
3973     const unsigned int dataSize = sbufBytesRemaining(src);
3974     UNUSED(dataSize); // maybe unused due to compiler options
3975
3976     switch (cmdMSP) {
3977 #ifdef USE_TRANSPONDER
3978     case MSP_SET_TRANSPONDER_CONFIG: {
3979         // Backward compatibility to BFC 3.1.1 is lost for this message type
3980
3981         uint8_t provider = sbufReadU8(src);
3982         uint8_t bytesRemaining = dataSize - 1;
3983
3984         if (provider > TRANSPONDER_PROVIDER_COUNT) {
3985             return MSP_RESULT_ERROR;
3986         }
3987
3988         const uint8_t requirementIndex = provider - 1;
3989         const uint8_t transponderDataSize = transponderRequirements[requirementIndex].dataLength;
3990
3991         transponderConfigMutable()->provider = provider;
3992
3993         if (provider == TRANSPONDER_NONE) {
3994             break;
3995         }
3996
3997         if (bytesRemaining != transponderDataSize) {
3998             return MSP_RESULT_ERROR;
3999         }
4000
4001         if (provider != transponderConfig()->provider) {
4002             transponderStopRepeating();
4003         }
4004
4005         memset(transponderConfigMutable()->data, 0, sizeof(transponderConfig()->data));
4006
4007         for (unsigned int i = 0; i < transponderDataSize; i++) {
4008             transponderConfigMutable()->data[i] = sbufReadU8(src);
4009         }
4010         transponderUpdateData();
4011         break;
4012     }
4013 #endif
4014
4015     case MSP_SET_VOLTAGE_METER_CONFIG: {
4016         int8_t id = sbufReadU8(src);
4017
4018         //
4019         // find and configure an ADC voltage sensor
4020         //
4021         int8_t voltageSensorADCIndex;
4022         for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) {
4023             if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) {
4024                 break;
4025             }
4026         }
4027
4028         if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) {
4029             voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src);
4030             voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src);
4031             voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src);
4032         } else {
4033             // if we had any other types of voltage sensor to configure, this is where we'd do it.
4034             sbufReadU8(src);
4035             sbufReadU8(src);
4036             sbufReadU8(src);
4037         }
4038         break;
4039     }
4040
4041     case MSP_SET_CURRENT_METER_CONFIG: {
4042         int id = sbufReadU8(src);
4043
4044         switch (id) {
4045             case CURRENT_METER_ID_BATTERY_1:
4046                 currentSensorADCConfigMutable()->scale = sbufReadU16(src);
4047                 currentSensorADCConfigMutable()->offset = sbufReadU16(src);
4048                 break;
4049 #ifdef USE_VIRTUAL_CURRENT_METER
4050             case CURRENT_METER_ID_VIRTUAL_1:
4051                 currentSensorVirtualConfigMutable()->scale = sbufReadU16(src);
4052                 currentSensorVirtualConfigMutable()->offset = sbufReadU16(src);
4053                 break;
4054 #endif
4055             default:
4056                 sbufReadU16(src);
4057                 sbufReadU16(src);
4058                 break;
4059         }
4060         break;
4061     }
4062
4063     case MSP_SET_BATTERY_CONFIG:
4064         batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src) * 10;      // vbatlevel_warn1 in MWC2.3 GUI
4065         batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src) * 10;      // vbatlevel_warn2 in MWC2.3 GUI
4066         batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src) * 10;  // vbatlevel when buzzer starts to alert
4067         batteryConfigMutable()->batteryCapacity = sbufReadU16(src);
4068         batteryConfigMutable()->voltageMeterSource = sbufReadU8(src);
4069         batteryConfigMutable()->currentMeterSource = sbufReadU8(src);
4070         if (sbufBytesRemaining(src) >= 6) {
4071             batteryConfigMutable()->vbatmincellvoltage = sbufReadU16(src);
4072             batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU16(src);
4073             batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU16(src);
4074         }
4075         break;
4076
4077 #if defined(USE_OSD)
4078     case MSP_SET_OSD_CONFIG:
4079         {
4080             const uint8_t addr = sbufReadU8(src);
4081
4082             if ((int8_t)addr == -1) {
4083                 /* Set general OSD settings */
4084                 videoSystem_e video_system = sbufReadU8(src);
4085 #ifndef USE_OSD_HD
4086                 if (video_system == VIDEO_SYSTEM_HD) {
4087                     video_system = VIDEO_SYSTEM_AUTO;
4088                 }
4089 #endif
4090
4091                 if ((video_system == VIDEO_SYSTEM_HD) && (vcdProfile()->video_system != VIDEO_SYSTEM_HD)) {
4092                     // If switching to HD, don't wait for the VTX to communicate the correct resolution, just
4093                     // increase the canvas size to the HD default as that is what the user will expect
4094                     osdConfigMutable()->canvas_cols = OSD_HD_COLS;
4095                     osdConfigMutable()->canvas_rows = OSD_HD_ROWS;
4096                 }
4097
4098                 vcdProfileMutable()->video_system = video_system;
4099
4100                 osdConfigMutable()->units = sbufReadU8(src);
4101
4102                 // Alarms
4103                 osdConfigMutable()->rssi_alarm = sbufReadU8(src);
4104                 osdConfigMutable()->cap_alarm = sbufReadU16(src);
4105                 sbufReadU16(src); // Skip unused (previously fly timer)
4106                 osdConfigMutable()->alt_alarm = sbufReadU16(src);
4107
4108                 if (sbufBytesRemaining(src) >= 2) {
4109                     /* Enabled warnings */
4110                     // API < 1.41 supports only the low 16 bits
4111                     osdConfigMutable()->enabledWarnings = sbufReadU16(src);
4112                 }
4113
4114                 if (sbufBytesRemaining(src) >= 4) {
4115                     // 32bit version of enabled warnings (API >= 1.41)
4116                     osdConfigMutable()->enabledWarnings = sbufReadU32(src);
4117                 }
4118
4119                 if (sbufBytesRemaining(src) >= 1) {
4120                     // API >= 1.41
4121                     // selected OSD profile
4122 #ifdef USE_OSD_PROFILES
4123                     changeOsdProfileIndex(sbufReadU8(src));
4124 #else
4125                     sbufReadU8(src);
4126 #endif // USE_OSD_PROFILES
4127                 }
4128
4129                 if (sbufBytesRemaining(src) >= 1) {
4130                     // API >= 1.41
4131                     // OSD stick overlay mode
4132
4133 #ifdef USE_OSD_STICK_OVERLAY
4134                     osdConfigMutable()->overlay_radio_mode = sbufReadU8(src);
4135 #else
4136                     sbufReadU8(src);
4137 #endif // USE_OSD_STICK_OVERLAY
4138
4139                 }
4140
4141                 if (sbufBytesRemaining(src) >= 2) {
4142                     // API >= 1.43
4143                     // OSD camera frame element width/height
4144                     osdConfigMutable()->camera_frame_width = sbufReadU8(src);
4145                     osdConfigMutable()->camera_frame_height = sbufReadU8(src);
4146                 }
4147             } else if ((int8_t)addr == -2) {
4148                 // Timers
4149                 uint8_t index = sbufReadU8(src);
4150                 if (index > OSD_TIMER_COUNT) {
4151                   return MSP_RESULT_ERROR;
4152                 }
4153                 osdConfigMutable()->timers[index] = sbufReadU16(src);
4154
4155                 return MSP_RESULT_ERROR;
4156             } else {
4157                 const uint16_t value = sbufReadU16(src);
4158
4159                 /* Get screen index, 0 is post flight statistics, 1 and above are in flight OSD screens */
4160                 const uint8_t screen = (sbufBytesRemaining(src) >= 1) ? sbufReadU8(src) : 1;
4161
4162                 if (screen == 0 && addr < OSD_STAT_COUNT) {
4163                     /* Set statistic item enable */
4164                     osdStatSetState(addr, (value != 0));
4165                 } else if (addr < OSD_ITEM_COUNT) {
4166                     /* Set element positions */
4167                     osdElementConfigMutable()->item_pos[addr] = value;
4168                     osdAnalyzeActiveElements();
4169                 } else {
4170                   return MSP_RESULT_ERROR;
4171                 }
4172             }
4173         }
4174         break;
4175
4176     case MSP_OSD_CHAR_WRITE:
4177         {
4178             osdCharacter_t chr;
4179             size_t osdCharacterBytes;
4180             uint16_t addr;
4181             if (dataSize >= OSD_CHAR_VISIBLE_BYTES + 2) {
4182                 if (dataSize >= OSD_CHAR_BYTES + 2) {
4183                     // 16 bit address, full char with metadata
4184                     addr = sbufReadU16(src);
4185                     osdCharacterBytes = OSD_CHAR_BYTES;
4186                 } else if (dataSize >= OSD_CHAR_BYTES + 1) {
4187                     // 8 bit address, full char with metadata
4188                     addr = sbufReadU8(src);
4189                     osdCharacterBytes = OSD_CHAR_BYTES;
4190                 } else {
4191                     // 16 bit character address, only visible char bytes
4192                     addr = sbufReadU16(src);
4193                     osdCharacterBytes = OSD_CHAR_VISIBLE_BYTES;
4194                 }
4195             } else {
4196                 // 8 bit character address, only visible char bytes
4197                 addr = sbufReadU8(src);
4198                 osdCharacterBytes = OSD_CHAR_VISIBLE_BYTES;
4199             }
4200             for (unsigned ii = 0; ii < MIN(osdCharacterBytes, sizeof(chr.data)); ii++) {
4201                 chr.data[ii] = sbufReadU8(src);
4202             }
4203             displayPort_t *osdDisplayPort = osdGetDisplayPort(NULL);
4204             if (!osdDisplayPort) {
4205                 return MSP_RESULT_ERROR;
4206             }
4207
4208             if (!displayWriteFontCharacter(osdDisplayPort, addr, &chr)) {
4209                 return MSP_RESULT_ERROR;
4210             }
4211         }
4212         break;
4213
4214 #ifdef USE_OSD_HD
4215     case MSP_SET_OSD_CANVAS:
4216         {
4217             osdConfigMutable()->canvas_cols = sbufReadU8(src);
4218             osdConfigMutable()->canvas_rows = sbufReadU8(src);
4219
4220             if ((vcdProfile()->video_system != VIDEO_SYSTEM_HD) ||
4221                 (osdConfig()->displayPortDevice != OSD_DISPLAYPORT_DEVICE_MSP)) {
4222                 // An HD VTX has communicated it's canvas size, so we must be in HD mode
4223                 vcdProfileMutable()->video_system = VIDEO_SYSTEM_HD;
4224                 // And using MSP displayport
4225                 osdConfigMutable()->displayPortDevice = OSD_DISPLAYPORT_DEVICE_MSP;
4226
4227                 // Save settings and reboot or the user won't see the effect and will have to manually save
4228                 writeEEPROM();
4229                 systemReset();
4230             }
4231         }
4232         break;
4233 #endif //USE_OSD_HD
4234 #endif // OSD
4235
4236     default:
4237         return mspProcessInCommand(srcDesc, cmdMSP, src);
4238     }
4239     return MSP_RESULT_ACK;
4240 }
4241
4242 /*
4243  * Returns MSP_RESULT_ACK, MSP_RESULT_ERROR or MSP_RESULT_NO_REPLY
4244  */
4245 mspResult_e mspFcProcessCommand(mspDescriptor_t srcDesc, mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
4246 {
4247     int ret = MSP_RESULT_ACK;
4248     sbuf_t *dst = &reply->buf;
4249     sbuf_t *src = &cmd->buf;
4250     const int16_t cmdMSP = cmd->cmd;
4251     // initialize reply by default
4252     reply->cmd = cmd->cmd;
4253
4254     if (mspCommonProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
4255         ret = MSP_RESULT_ACK;
4256     } else if (mspProcessOutCommand(srcDesc, cmdMSP, dst)) {
4257         ret = MSP_RESULT_ACK;
4258     } else if ((ret = mspFcProcessOutCommandWithArg(srcDesc, cmdMSP, src, dst, mspPostProcessFn)) != MSP_RESULT_CMD_UNKNOWN) {
4259         /* ret */;
4260     } else if (cmdMSP == MSP_SET_PASSTHROUGH) {
4261         mspFcSetPassthroughCommand(dst, src, mspPostProcessFn);
4262         ret = MSP_RESULT_ACK;
4263 #ifdef USE_FLASHFS
4264     } else if (cmdMSP == MSP_DATAFLASH_READ) {
4265         mspFcDataFlashReadCommand(dst, src);
4266         ret = MSP_RESULT_ACK;
4267 #endif
4268     } else {
4269         ret = mspCommonProcessInCommand(srcDesc, cmdMSP, src, mspPostProcessFn);
4270     }
4271     reply->result = ret;
4272     return ret;
4273 }
4274
4275 void mspFcProcessReply(mspPacket_t *reply)
4276 {
4277     sbuf_t *src = &reply->buf;
4278     UNUSED(src); // potentially unused depending on compile options.
4279
4280     switch (reply->cmd) {
4281     case MSP_ANALOG:
4282         {
4283             uint8_t batteryVoltage = sbufReadU8(src);
4284             uint16_t mAhDrawn = sbufReadU16(src);
4285             uint16_t rssi = sbufReadU16(src);
4286             uint16_t amperage = sbufReadU16(src);
4287
4288             UNUSED(rssi);
4289             UNUSED(batteryVoltage);
4290             UNUSED(amperage);
4291             UNUSED(mAhDrawn);
4292
4293 #ifdef USE_MSP_CURRENT_METER
4294             currentMeterMSPSet(amperage, mAhDrawn);
4295 #endif
4296         }
4297         break;
4298     }
4299 }
4300
4301 void mspInit(void)
4302 {
4303     initActiveBoxIds();
4304 }