src/main/msp/msp.c

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