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