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