src/main/fc/fc_msp.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21 #include <math.h>
  22
  23 #include "platform.h"
  24
  25 #include "blackbox/blackbox.h"
  26
  27 #include "build/build_config.h"
  28 #include "build/debug.h"
  29 #include "build/version.h"
  30
  31 #include "common/axis.h"
  32 #include "common/color.h"
  33 #include "common/maths.h"
  34 #include "common/streambuf.h"
  35
  36 #include "config/config_eeprom.h"
  37 #include "config/feature.h"
  38 #include "config/parameter_group.h"
  39 #include "config/parameter_group_ids.h"
  40
  41 #include "drivers/accgyro.h"
  42 #include "drivers/bus_i2c.h"
  43 #include "drivers/compass.h"
  44 #include "drivers/flash.h"
  45 #include "drivers/io.h"
  46 #include "drivers/max7456.h"
  47 #include "drivers/pwm_output.h"
  48 #include "drivers/sdcard.h"
  49 #include "drivers/serial.h"
  50 #include "drivers/serial_escserial.h"
  51 #include "drivers/system.h"
  52 #include "drivers/vcd.h"
  53 #include "drivers/vtx_common.h"
  54 #include "drivers/vtx_soft_spi_rtc6705.h"
  55 #include "drivers/transponder_ir.h"
  56
  57 #include "fc/config.h"
  58 #include "fc/controlrate_profile.h"
  59 #include "fc/fc_core.h"
  60 #include "fc/fc_msp.h"
  61 #include "fc/fc_rc.h"
  62 #include "fc/rc_adjustments.h"
  63 #include "fc/runtime_config.h"
  64
  65 #ifdef USE_FC
  66 #include "flight/altitudehold.h"
  67 #include "flight/failsafe.h"
  68 #include "flight/imu.h"
  69 #include "flight/mixer.h"
  70 #include "flight/navigation.h"
  71 #include "flight/pid.h"
  72 #include "flight/servos.h"
  73 #endif
  74
  75 #include "io/asyncfatfs/asyncfatfs.h"
  76 #include "io/beeper.h"
  77 #include "io/flashfs.h"
  78 #include "io/gimbal.h"
  79 #include "io/gps.h"
  80 #include "io/ledstrip.h"
  81 #include "io/motors.h"
  82 #include "io/osd.h"
  83 #include "io/osd_slave.h"
  84 #include "io/serial.h"
  85 #include "io/serial_4way.h"
  86 #include "io/servos.h"
  87 #include "io/transponder_ir.h"
  88 #include "io/vtx.h"
  89
  90 #include "msp/msp.h"
  91 #include "msp/msp_protocol.h"
  92 #include "msp/msp_serial.h"
  93
  94 #ifdef USE_FC
  95 #include "rx/msp.h"
  96 #include "rx/rx.h"
  97 #endif
  98
  99 #include "scheduler/scheduler.h"
 100
 101 #include "sensors/battery.h"
 102
 103 #ifdef USE_FC
 104 #include "sensors/acceleration.h"
 105 #include "sensors/barometer.h"
 106 #include "sensors/boardalignment.h"
 107 #include "sensors/compass.h"
 108 #include "sensors/gyro.h"
 109 #include "sensors/sensors.h"
 110 #include "sensors/sonar.h"
 111
 112 #include "telemetry/telemetry.h"
 113 #endif
 114
 115 #ifdef USE_HARDWARE_REVISION_DETECTION
 116 #include "hardware_revision.h"
 117 #endif
 118
 119 static const char * const flightControllerIdentifier = CLEANFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
 120 static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
 121
 122 #ifdef USE_FC
 123 static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
 124     { BOXARM, "ARM", 0 },
 125     { BOXANGLE, "ANGLE", 1 },
 126     { BOXHORIZON, "HORIZON", 2 },
 127     { BOXBARO, "BARO", 3 },
 128     { BOXANTIGRAVITY, "ANTI GRAVITY", 4 },
 129     { BOXMAG, "MAG", 5 },
 130     { BOXHEADFREE, "HEADFREE", 6 },
 131     { BOXHEADADJ, "HEADADJ", 7 },
 132     { BOXCAMSTAB, "CAMSTAB", 8 },
 133     { BOXCAMTRIG, "CAMTRIG", 9 },
 134     { BOXGPSHOME, "GPS HOME", 10 },
 135     { BOXGPSHOLD, "GPS HOLD", 11 },
 136     { BOXPASSTHRU, "PASSTHRU", 12 },
 137     { BOXBEEPERON, "BEEPER", 13 },
 138     { BOXLEDMAX, "LEDMAX", 14 },
 139     { BOXLEDLOW, "LEDLOW", 15 },
 140     { BOXLLIGHTS, "LLIGHTS", 16 },
 141     { BOXCALIB, "CALIB", 17 },
 142     { BOXGOV, "GOVERNOR", 18 },
 143     { BOXOSD, "OSD SW", 19 },
 144     { BOXTELEMETRY, "TELEMETRY", 20 },
 145     { BOXGTUNE, "GTUNE", 21 },
 146     { BOXSONAR, "SONAR", 22 },
 147     { BOXSERVO1, "SERVO1", 23 },
 148     { BOXSERVO2, "SERVO2", 24 },
 149     { BOXSERVO3, "SERVO3", 25 },
 150     { BOXBLACKBOX, "BLACKBOX", 26 },
 151     { BOXFAILSAFE, "FAILSAFE", 27 },
 152     { BOXAIRMODE, "AIR MODE", 28 },
 153     { BOX3DDISABLESWITCH, "DISABLE 3D SWITCH", 29},
 154     { BOXFPVANGLEMIX, "FPV ANGLE MIX", 30},
 155     { BOXBLACKBOXERASE, "BLACKBOX ERASE (>30s)", 31 },
 156     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 157 };
 158
 159 // this is calculated at startup based on enabled features.
 160 static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT];
 161 // this is the number of filled indexes in above array
 162 static uint8_t activeBoxIdCount = 0;
 163
 164 static const char pidnames[] =
 165     "ROLL;"
 166     "PITCH;"
 167     "YAW;"
 168     "ALT;"
 169     "Pos;"
 170     "PosR;"
 171     "NavR;"
 172     "LEVEL;"
 173     "MAG;"
 174     "VEL;";
 175
 176 typedef enum {
 177     MSP_SDCARD_STATE_NOT_PRESENT = 0,
 178     MSP_SDCARD_STATE_FATAL       = 1,
 179     MSP_SDCARD_STATE_CARD_INIT   = 2,
 180     MSP_SDCARD_STATE_FS_INIT     = 3,
 181     MSP_SDCARD_STATE_READY       = 4
 182 } mspSDCardState_e;
 183
 184 typedef enum {
 185     MSP_SDCARD_FLAG_SUPPORTTED   = 1
 186 } mspSDCardFlags_e;
 187
 188 #define RATEPROFILE_MASK (1 << 7)
 189 #endif
 190
 191 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
 192 #define ESC_4WAY 0xff
 193
 194 uint8_t escMode;
 195 uint8_t escPortIndex = 0;
 196
 197 #ifdef USE_ESCSERIAL
 198 static void mspEscPassthroughFn(serialPort_t *serialPort)
 199 {
 200     escEnablePassthrough(serialPort, escPortIndex, escMode);
 201 }
 202 #endif
 203
 204 static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
 205 {
 206     const unsigned int dataSize = sbufBytesRemaining(src);
 207     if (dataSize == 0) {
 208         // Legacy format
 209
 210         escMode = ESC_4WAY;
 211     } else {
 212         escMode = sbufReadU8(src);
 213         escPortIndex = sbufReadU8(src);
 214     }
 215
 216     switch(escMode) {
 217     case ESC_4WAY:
 218         // get channel number
 219         // switch all motor lines HI
 220         // reply with the count of ESC found
 221         sbufWriteU8(dst, esc4wayInit());
 222
 223         if (mspPostProcessFn) {
 224             *mspPostProcessFn = esc4wayProcess;
 225         }
 226
 227         break;
 228 #ifdef USE_ESCSERIAL
 229     case PROTOCOL_SIMONK:
 230     case PROTOCOL_BLHELI:
 231     case PROTOCOL_KISS:
 232     case PROTOCOL_KISSALL:
 233     case PROTOCOL_CASTLE:
 234         if (escPortIndex < getMotorCount() || (escMode == PROTOCOL_KISS && escPortIndex == ALL_ESCS)) {
 235             sbufWriteU8(dst, 1);
 236
 237             if (mspPostProcessFn) {
 238                 *mspPostProcessFn = mspEscPassthroughFn;
 239             }
 240
 241             break;
 242         }
 243 #endif
 244     default:
 245         sbufWriteU8(dst, 0);
 246     }
 247 }
 248 #endif
 249
 250 static void mspRebootFn(serialPort_t *serialPort)
 251 {
 252     UNUSED(serialPort);
 253
 254 #ifdef USE_FC
 255     stopPwmAllMotors();
 256 #endif
 257     systemReset();
 258
 259     // control should never return here.
 260     while (true) ;
 261 }
 262
 263 #ifdef USE_FC
 264 const box_t *findBoxByBoxId(uint8_t boxId)
 265 {
 266     for (uint8_t boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
 267         const box_t *candidate = &boxes[boxIndex];
 268         if (candidate->boxId == boxId) {
 269             return candidate;
 270         }
 271     }
 272     return NULL;
 273 }
 274
 275 const box_t *findBoxByPermanentId(uint8_t permenantId)
 276 {
 277     for (uint8_t boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
 278         const box_t *candidate = &boxes[boxIndex];
 279         if (candidate->permanentId == permenantId) {
 280             return candidate;
 281         }
 282     }
 283     return NULL;
 284 }
 285
 286 static void serializeBoxNamesReply(sbuf_t *dst)
 287 {
 288     for (int i = 0; i < activeBoxIdCount; i++) {
 289         const int activeBoxId = activeBoxIds[i];
 290         const box_t *box = findBoxByBoxId(activeBoxId);
 291         if (!box) {
 292             continue;
 293         }
 294
 295         sbufWriteData(dst, box->boxName, strlen(box->boxName));
 296         sbufWriteU8(dst, ';');
 297     }
 298 }
 299
 300 void initActiveBoxIds(void)
 301 {
 302     // calculate used boxes based on features and fill availableBoxes[] array
 303     memset(activeBoxIds, 0xFF, sizeof(activeBoxIds));
 304
 305     activeBoxIdCount = 0;
 306     activeBoxIds[activeBoxIdCount++] = BOXARM;
 307
 308     if (!feature(FEATURE_AIRMODE)) {
 309         activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
 310     }
 311
 312     if (!feature(FEATURE_ANTI_GRAVITY)) {
 313         activeBoxIds[activeBoxIdCount++] = BOXANTIGRAVITY;
 314     }
 315
 316     if (sensors(SENSOR_ACC)) {
 317         activeBoxIds[activeBoxIdCount++] = BOXANGLE;
 318         activeBoxIds[activeBoxIdCount++] = BOXHORIZON;
 319         activeBoxIds[activeBoxIdCount++] = BOXHEADFREE;
 320     }
 321
 322 #ifdef BARO
 323     if (sensors(SENSOR_BARO)) {
 324         activeBoxIds[activeBoxIdCount++] = BOXBARO;
 325     }
 326 #endif
 327
 328 #ifdef MAG
 329     if (sensors(SENSOR_MAG)) {
 330         activeBoxIds[activeBoxIdCount++] = BOXMAG;
 331         activeBoxIds[activeBoxIdCount++] = BOXHEADADJ;
 332     }
 333 #endif
 334
 335 #ifdef GPS
 336     if (feature(FEATURE_GPS)) {
 337         activeBoxIds[activeBoxIdCount++] = BOXGPSHOME;
 338         activeBoxIds[activeBoxIdCount++] = BOXGPSHOLD;
 339     }
 340 #endif
 341
 342 #ifdef SONAR
 343     if (feature(FEATURE_SONAR)) {
 344         activeBoxIds[activeBoxIdCount++] = BOXSONAR;
 345     }
 346 #endif
 347
 348     if (feature(FEATURE_FAILSAFE)) {
 349         activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE;
 350     }
 351
 352     if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) {
 353         activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
 354     }
 355
 356     activeBoxIds[activeBoxIdCount++] = BOXBEEPERON;
 357
 358 #ifdef LED_STRIP
 359     if (feature(FEATURE_LED_STRIP)) {
 360         activeBoxIds[activeBoxIdCount++] = BOXLEDLOW;
 361     }
 362 #endif
 363
 364 #ifdef BLACKBOX
 365     activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX;
 366 #ifdef USE_FLASHFS
 367     activeBoxIds[activeBoxIdCount++] = BOXBLACKBOXERASE;
 368 #endif
 369 #endif
 370
 371     activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX;
 372
 373     if (feature(FEATURE_3D)) {
 374         activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH;
 375     }
 376
 377     if (feature(FEATURE_SERVO_TILT)) {
 378         activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB;
 379     }
 380
 381     if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
 382         activeBoxIds[activeBoxIdCount++] = BOXCALIB;
 383     }
 384
 385     activeBoxIds[activeBoxIdCount++] = BOXOSD;
 386
 387 #ifdef TELEMETRY
 388     if (feature(FEATURE_TELEMETRY) && telemetryConfig()->telemetry_switch) {
 389         activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY;
 390     }
 391 #endif
 392
 393 #ifdef USE_SERVOS
 394     if (mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) {
 395         activeBoxIds[activeBoxIdCount++] = BOXSERVO1;
 396         activeBoxIds[activeBoxIdCount++] = BOXSERVO2;
 397         activeBoxIds[activeBoxIdCount++] = BOXSERVO3;
 398     }
 399 #endif
 400 }
 401
 402 #define IS_ENABLED(mask) (mask == 0 ? 0 : 1)
 403
 404 static uint32_t packFlightModeFlags(void)
 405 {
 406     // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
 407     // Requires new Multiwii protocol version to fix
 408     // It would be preferable to setting the enabled bits based on BOXINDEX.
 409     const uint32_t tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE |
 410         IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON |
 411         IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << BOXBARO |
 412         IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG |
 413         IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE |
 414         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ |
 415         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB |
 416         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMTRIG)) << BOXCAMTRIG |
 417         IS_ENABLED(FLIGHT_MODE(GPS_HOME_MODE)) << BOXGPSHOME |
 418         IS_ENABLED(FLIGHT_MODE(GPS_HOLD_MODE)) << BOXGPSHOLD |
 419         IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU |
 420         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON |
 421         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX)) << BOXLEDMAX |
 422         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW |
 423         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS |
 424         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB)) << BOXCALIB |
 425         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
 426         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
 427         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
 428         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
 429         IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
 430         IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
 431         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
 432         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) << BOXBLACKBOXERASE |
 433         IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE |
 434         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE |
 435         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY)) << BOXANTIGRAVITY |
 436         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)) << BOXFPVANGLEMIX;
 437
 438     uint32_t ret = 0;
 439     for (int i = 0; i < activeBoxIdCount; i++) {
 440         const uint32_t flag = (tmp & (1 << activeBoxIds[i]));
 441         if (flag) {
 442             ret |= 1 << i;
 443         }
 444     }
 445     return ret;
 446 }
 447
 448 static void serializeSDCardSummaryReply(sbuf_t *dst)
 449 {
 450 #ifdef USE_SDCARD
 451     uint8_t flags = MSP_SDCARD_FLAG_SUPPORTTED;
 452     uint8_t state = 0;
 453
 454     sbufWriteU8(dst, flags);
 455
 456     // Merge the card and filesystem states together
 457     if (!sdcard_isInserted()) {
 458         state = MSP_SDCARD_STATE_NOT_PRESENT;
 459     } else if (!sdcard_isFunctional()) {
 460         state = MSP_SDCARD_STATE_FATAL;
 461     } else {
 462         switch (afatfs_getFilesystemState()) {
 463         case AFATFS_FILESYSTEM_STATE_READY:
 464             state = MSP_SDCARD_STATE_READY;
 465
 466             break;
 467         case AFATFS_FILESYSTEM_STATE_INITIALIZATION:
 468             if (sdcard_isInitialized()) {
 469                 state = MSP_SDCARD_STATE_FS_INIT;
 470             } else {
 471                 state = MSP_SDCARD_STATE_CARD_INIT;
 472             }
 473
 474             break;
 475         case AFATFS_FILESYSTEM_STATE_FATAL:
 476         case AFATFS_FILESYSTEM_STATE_UNKNOWN:
 477         default:
 478             state = MSP_SDCARD_STATE_FATAL;
 479
 480             break;
 481         }
 482     }
 483
 484     sbufWriteU8(dst, state);
 485     sbufWriteU8(dst, afatfs_getLastError());
 486     // Write free space and total space in kilobytes
 487     sbufWriteU32(dst, afatfs_getContiguousFreeSpace() / 1024);
 488     sbufWriteU32(dst, sdcard_getMetadata()->numBlocks / 2); // Block size is half a kilobyte
 489 #else
 490     sbufWriteU8(dst, 0);
 491     sbufWriteU8(dst, 0);
 492     sbufWriteU8(dst, 0);
 493     sbufWriteU32(dst, 0);
 494     sbufWriteU32(dst, 0);
 495 #endif
 496 }
 497
 498 static void serializeDataflashSummaryReply(sbuf_t *dst)
 499 {
 500 #ifdef USE_FLASHFS
 501     const flashGeometry_t *geometry = flashfsGetGeometry();
 502     uint8_t flags = (flashfsIsReady() ? 1 : 0) | 2 /* FlashFS is supported */;
 503
 504     sbufWriteU8(dst, flags);
 505     sbufWriteU32(dst, geometry->sectors);
 506     sbufWriteU32(dst, geometry->totalSize);
 507     sbufWriteU32(dst, flashfsGetOffset()); // Effectively the current number of bytes stored on the volume
 508 #else
 509     sbufWriteU8(dst, 0); // FlashFS is neither ready nor supported
 510     sbufWriteU32(dst, 0);
 511     sbufWriteU32(dst, 0);
 512     sbufWriteU32(dst, 0);
 513 #endif
 514 }
 515
 516 #ifdef USE_FLASHFS
 517 static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uint16_t size, bool useLegacyFormat)
 518 {
 519     BUILD_BUG_ON(MSP_PORT_DATAFLASH_INFO_SIZE < 16);
 520
 521     uint16_t readLen = size;
 522     const int bytesRemainingInBuf = sbufBytesRemaining(dst) - MSP_PORT_DATAFLASH_INFO_SIZE;
 523     if (readLen > bytesRemainingInBuf) {
 524         readLen = bytesRemainingInBuf;
 525     }
 526     // size will be lower than that requested if we reach end of volume
 527     if (readLen > flashfsGetSize() - address) {
 528         // truncate the request
 529         readLen = flashfsGetSize() - address;
 530     }
 531     sbufWriteU32(dst, address);
 532     if (!useLegacyFormat) {
 533         // new format supports variable read lengths
 534         sbufWriteU16(dst, readLen);
 535         sbufWriteU8(dst, 0); // placeholder for compression format
 536     }
 537
 538     // bytesRead will equal readLen
 539     const int bytesRead = flashfsReadAbs(address, sbufPtr(dst), readLen);
 540     sbufAdvance(dst, bytesRead);
 541
 542     if (useLegacyFormat) {
 543         // pad the buffer with zeros
 544         for (int i = bytesRead; i < size; i++) {
 545             sbufWriteU8(dst, 0);
 546         }
 547     }
 548 }
 549 #endif
 550 #endif
 551
 552 /*
 553  * Returns true if the command was processd, false otherwise.
 554  * May set mspPostProcessFunc to a function to be called once the command has been processed
 555  */
 556 static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
 557 {
 558     switch (cmdMSP) {
 559     case MSP_API_VERSION:
 560         sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
 561         sbufWriteU8(dst, API_VERSION_MAJOR);
 562         sbufWriteU8(dst, API_VERSION_MINOR);
 563         break;
 564
 565     case MSP_FC_VARIANT:
 566         sbufWriteData(dst, flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
 567         break;
 568
 569     case MSP_FC_VERSION:
 570         sbufWriteU8(dst, FC_VERSION_MAJOR);
 571         sbufWriteU8(dst, FC_VERSION_MINOR);
 572         sbufWriteU8(dst, FC_VERSION_PATCH_LEVEL);
 573         break;
 574
 575     case MSP_BOARD_INFO:
 576         sbufWriteData(dst, boardIdentifier, BOARD_IDENTIFIER_LENGTH);
 577 #ifdef USE_HARDWARE_REVISION_DETECTION
 578         sbufWriteU16(dst, hardwareRevision);
 579 #else
 580         sbufWriteU16(dst, 0); // No other build targets currently have hardware revision detection.
 581 #endif
 582 #ifdef USE_OSD_SLAVE
 583         sbufWriteU8(dst, 1);  // 1 == OSD
 584 #else
 585 #if defined(OSD) && defined(USE_MAX7456)
 586         sbufWriteU8(dst, 2);  // 2 == FC with OSD
 587 #else
 588         sbufWriteU8(dst, 0);  // 0 == FC
 589 #endif
 590 #endif
 591         break;
 592
 593     case MSP_BUILD_INFO:
 594         sbufWriteData(dst, buildDate, BUILD_DATE_LENGTH);
 595         sbufWriteData(dst, buildTime, BUILD_TIME_LENGTH);
 596         sbufWriteData(dst, shortGitRevision, GIT_SHORT_REVISION_LENGTH);
 597         break;
 598
 599
 600     case MSP_REBOOT:
 601         if (mspPostProcessFn) {
 602             *mspPostProcessFn = mspRebootFn;
 603         }
 604         break;
 605
 606     case MSP_ANALOG:
 607         sbufWriteU8(dst, (uint8_t)constrain(getBatteryVoltage(), 0, 255));
 608         sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
 609         sbufWriteU16(dst, 0); // rssi
 610         sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send current in 0.01 A steps, range is -320A to 320A
 611         break;
 612
 613
 614     case MSP_DEBUG:
 615         // output some useful QA statistics
 616         // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000);         // XX0YY [crystal clock : core clock]
 617
 618         for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) {
 619             sbufWriteU16(dst, debug[i]);      // 4 variables are here for general monitoring purpose
 620         }
 621         break;
 622
 623
 624     case MSP_UID:
 625         sbufWriteU32(dst, U_ID_0);
 626         sbufWriteU32(dst, U_ID_1);
 627         sbufWriteU32(dst, U_ID_2);
 628         break;
 629
 630     case MSP_FEATURE_CONFIG:
 631         sbufWriteU32(dst, featureMask());
 632         break;
 633
 634     case MSP_BATTERY_STATE: {
 635         // battery characteristics
 636         sbufWriteU8(dst, (uint8_t)constrain(getBatteryCellCount(), 0, 255)); // 0 indicates battery not detected.
 637         sbufWriteU16(dst, batteryConfig()->batteryCapacity); // in mAh
 638
 639         // battery state
 640         sbufWriteU8(dst, (uint8_t)constrain(getBatteryVoltage(), 0, 255)); // in 0.1V steps
 641         sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
 642         sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send current in 0.01 A steps, range is -320A to 320A
 643
 644         // battery alerts
 645         sbufWriteU8(dst, (uint8_t)getBatteryState());
 646         break;
 647     }
 648     case MSP_VOLTAGE_METERS:
 649         // write out id and voltage meter values, once for each meter we support
 650         for (int i = 0; i < supportedVoltageMeterCount; i++) {
 651
 652             voltageMeter_t meter;
 653             uint8_t id = (uint8_t)voltageMeterIds[i];
 654             voltageMeterRead(id, &meter);
 655
 656             sbufWriteU8(dst, id);
 657             sbufWriteU8(dst, (uint8_t)constrain(meter.filtered, 0, 255));
 658         }
 659         break;
 660
 661     case MSP_CURRENT_METERS:
 662         // write out id and current meter values, once for each meter we support
 663         for (int i = 0; i < supportedCurrentMeterCount; i++) {
 664
 665             currentMeter_t meter;
 666             uint8_t id = (uint8_t)currentMeterIds[i];
 667             currentMeterRead(id, &meter);
 668
 669             sbufWriteU8(dst, id);
 670             sbufWriteU16(dst, (uint16_t)constrain(meter.mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
 671             sbufWriteU16(dst, (uint16_t)constrain(meter.amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps (mA). Negative range is truncated to zero
 672         }
 673         break;
 674
 675     case MSP_VOLTAGE_METER_CONFIG:
 676         // by using a sensor type and a sub-frame length it's possible to configure any type of voltage meter,
 677         // e.g. an i2c/spi/can sensor or any sensor not built directly into the FC such as ESC/RX/SPort/SBus that has
 678         // different configuration requirements.
 679         BUILD_BUG_ON(VOLTAGE_SENSOR_ADC_VBAT != 0); // VOLTAGE_SENSOR_ADC_VBAT should be the first index,
 680         sbufWriteU8(dst, MAX_VOLTAGE_SENSOR_ADC); // voltage meters in payload
 681         for (int i = VOLTAGE_SENSOR_ADC_VBAT; i < MAX_VOLTAGE_SENSOR_ADC; i++) {
 682             const uint8_t adcSensorSubframeLength = 1 + 1 + 1 + 1 + 1; // length of id, type, vbatscale, vbatresdivval, vbatresdivmultipler, in bytes
 683             sbufWriteU8(dst, adcSensorSubframeLength); // ADC sensor sub-frame length
 684
 685             sbufWriteU8(dst, voltageMeterADCtoIDMap[i]); // id of the sensor
 686             sbufWriteU8(dst, VOLTAGE_SENSOR_TYPE_ADC_RESISTOR_DIVIDER); // indicate the type of sensor that the next part of the payload is for
 687
 688             sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatscale);
 689             sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivval);
 690             sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivmultiplier);
 691         }
 692         // if we had any other voltage sensors, this is where we would output any needed configuration
 693         break;
 694
 695     case MSP_CURRENT_METER_CONFIG: {
 696         // the ADC and VIRTUAL sensors have the same configuration requirements, however this API reflects
 697         // that this situation may change and allows us to support configuration of any current sensor with
 698         // specialist configuration requirements.
 699
 700         sbufWriteU8(dst, 2); // current meters in payload (adc + virtual)
 701
 702         const uint8_t adcSensorSubframeLength = 1 + 1 + 2 + 2; // length of id, type, scale, offset, in bytes
 703         sbufWriteU8(dst, adcSensorSubframeLength);
 704         sbufWriteU8(dst, CURRENT_METER_ID_BATTERY_1); // the id of the sensor
 705         sbufWriteU8(dst, CURRENT_SENSOR_ADC); // indicate the type of sensor that the next part of the payload is for
 706         sbufWriteU16(dst, currentSensorADCConfig()->scale);
 707         sbufWriteU16(dst, currentSensorADCConfig()->offset);
 708
 709         const int8_t virtualSensorSubframeLength = 1 + 1 + 2 + 2; // length of id, type, scale, offset, in bytes
 710         sbufWriteU8(dst, virtualSensorSubframeLength);
 711         sbufWriteU8(dst, CURRENT_METER_ID_VIRTUAL_1); // the id of the sensor
 712         sbufWriteU8(dst, CURRENT_SENSOR_VIRTUAL); // indicate the type of sensor that the next part of the payload is for
 713         sbufWriteU16(dst, currentSensorVirtualConfig()->scale);
 714         sbufWriteU16(dst, currentSensorVirtualConfig()->offset);
 715
 716         // if we had any other current sensors, this is where we would output any needed configuration
 717         break;
 718     }
 719     case MSP_BATTERY_CONFIG:
 720         sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage);
 721         sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage);
 722         sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage);
 723         sbufWriteU16(dst, batteryConfig()->batteryCapacity);
 724         sbufWriteU8(dst, batteryConfig()->voltageMeterSource);
 725         sbufWriteU8(dst, batteryConfig()->currentMeterSource);
 726         break;
 727
 728
 729     case MSP_TRANSPONDER_CONFIG: {
 730 #ifdef TRANSPONDER
 731         sbufWriteU8(dst, TRANSPONDER_PROVIDER_COUNT);
 732         for (unsigned int i = 0; i < TRANSPONDER_PROVIDER_COUNT; i++) {
 733             sbufWriteU8(dst, transponderRequirements[i].provider);
 734             sbufWriteU8(dst, transponderRequirements[i].dataLength);
 735         }
 736
 737         uint8_t provider = transponderConfig()->provider;
 738         sbufWriteU8(dst, provider);
 739
 740         if (provider) {
 741             uint8_t requirementIndex = provider - 1;
 742             uint8_t providerDataLength = transponderRequirements[requirementIndex].dataLength;
 743
 744             for (unsigned int i = 0; i < providerDataLength; i++) {
 745                 sbufWriteU8(dst, transponderConfig()->data[i]);
 746             }
 747         }
 748 #else
 749         sbufWriteU8(dst, 0); // no providers
 750 #endif
 751         break;
 752     }
 753     case MSP_OSD_CONFIG: {
 754
 755 #define OSD_FLAGS_OSD_FEATURE           (1 << 0)
 756 #define OSD_FLAGS_OSD_SLAVE             (1 << 1)
 757 #define OSD_FLAGS_RESERVED_1            (1 << 2)
 758 #define OSD_FLAGS_RESERVED_2            (1 << 3)
 759 #define OSD_FLAGS_OSD_HARDWARE_MAX_7456 (1 << 4)
 760
 761         uint8_t osdFlags = 0;
 762 #if defined(OSD)
 763         osdFlags |= OSD_FLAGS_OSD_FEATURE;
 764 #endif
 765 #if defined(USE_OSD_SLAVE)
 766         osdFlags |= OSD_FLAGS_OSD_SLAVE;
 767 #endif
 768 #ifdef USE_MAX7456
 769         osdFlags |= OSD_FLAGS_OSD_HARDWARE_MAX_7456;
 770 #endif
 771
 772         sbufWriteU8(dst, osdFlags);
 773
 774 #ifdef USE_MAX7456
 775         // send video system (AUTO/PAL/NTSC)
 776         sbufWriteU8(dst, vcdProfile()->video_system);
 777 #else
 778         sbufWriteU8(dst, 0);
 779 #endif
 780
 781 #ifdef OSD
 782         // OSD specific, not applicable to OSD slaves.
 783         sbufWriteU8(dst, osdConfig()->units);
 784         sbufWriteU8(dst, osdConfig()->rssi_alarm);
 785         sbufWriteU16(dst, osdConfig()->cap_alarm);
 786         sbufWriteU16(dst, osdConfig()->time_alarm);
 787         sbufWriteU16(dst, osdConfig()->alt_alarm);
 788         for (int i = 0; i < OSD_ITEM_COUNT; i++) {
 789             sbufWriteU16(dst, osdConfig()->item_pos[i]);
 790         }
 791 #endif
 792         break;
 793     }
 794
 795     default:
 796         return false;
 797     }
 798     return true;
 799 }
 800
 801 #ifdef USE_OSD_SLAVE
 802 static bool mspOsdSlaveProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
 803 {
 804     UNUSED(mspPostProcessFn);
 805
 806     switch (cmdMSP) {
 807     case MSP_STATUS_EX:
 808         sbufWriteU16(dst, 0); // task delta
 809 #ifdef USE_I2C
 810         sbufWriteU16(dst, i2cGetErrorCounter());
 811 #else
 812         sbufWriteU16(dst, 0);
 813 #endif
 814         sbufWriteU16(dst, 0); // sensors
 815         sbufWriteU32(dst, 0); // flight modes
 816         sbufWriteU8(dst, 0); // profile
 817         sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
 818         sbufWriteU8(dst, 1); // max profiles
 819         sbufWriteU8(dst, 0); // control rate profile
 820         break;
 821
 822     case MSP_STATUS:
 823         sbufWriteU16(dst, 0); // task delta
 824 #ifdef USE_I2C
 825         sbufWriteU16(dst, i2cGetErrorCounter());
 826 #else
 827         sbufWriteU16(dst, 0);
 828 #endif
 829         sbufWriteU16(dst, 0); // sensors
 830         sbufWriteU32(dst, 0); // flight modes
 831         sbufWriteU8(dst, 0); // profile
 832         sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
 833         sbufWriteU16(dst, 0); // gyro cycle time
 834         break;
 835
 836     default:
 837         return false;
 838     }
 839     return true;
 840 }
 841 #endif
 842
 843 #ifdef USE_FC
 844 static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
 845 {
 846     UNUSED(mspPostProcessFn);
 847
 848     switch (cmdMSP) {
 849     case MSP_STATUS_EX:
 850         sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID));
 851 #ifdef USE_I2C
 852         sbufWriteU16(dst, i2cGetErrorCounter());
 853 #else
 854         sbufWriteU16(dst, 0);
 855 #endif
 856         sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
 857         sbufWriteU32(dst, packFlightModeFlags());
 858         sbufWriteU8(dst, getCurrentPidProfileIndex());
 859         sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
 860         sbufWriteU8(dst, MAX_PROFILE_COUNT);
 861         sbufWriteU8(dst, getCurrentControlRateProfileIndex());
 862         break;
 863
 864     case MSP_STATUS:
 865         sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID));
 866 #ifdef USE_I2C
 867         sbufWriteU16(dst, i2cGetErrorCounter());
 868 #else
 869         sbufWriteU16(dst, 0);
 870 #endif
 871         sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
 872         sbufWriteU32(dst, packFlightModeFlags());
 873         sbufWriteU8(dst, getCurrentPidProfileIndex());
 874         sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
 875         sbufWriteU16(dst, 0); // gyro cycle time
 876         break;
 877
 878     case MSP_RAW_IMU:
 879         {
 880             // Hack scale due to choice of units for sensor data in multiwii
 881             const uint8_t scale = (acc.dev.acc_1G > 512) ? 4 : 1;
 882             for (int i = 0; i < 3; i++) {
 883                 sbufWriteU16(dst, acc.accSmooth[i] / scale);
 884             }
 885             for (int i = 0; i < 3; i++) {
 886                 sbufWriteU16(dst, gyroRateDps(i));
 887             }
 888             for (int i = 0; i < 3; i++) {
 889                 sbufWriteU16(dst, mag.magADC[i]);
 890             }
 891         }
 892         break;
 893
 894     case MSP_NAME:
 895         {
 896             const int nameLen = strlen(systemConfig()->name);
 897             for (int i = 0; i < nameLen; i++) {
 898                 sbufWriteU8(dst, systemConfig()->name[i]);
 899             }
 900         }
 901         break;
 902
 903 #ifdef USE_SERVOS
 904     case MSP_SERVO:
 905         sbufWriteData(dst, &servo, MAX_SUPPORTED_SERVOS * 2);
 906         break;
 907     case MSP_SERVO_CONFIGURATIONS:
 908         for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
 909             sbufWriteU16(dst, servoParams(i)->min);
 910             sbufWriteU16(dst, servoParams(i)->max);
 911             sbufWriteU16(dst, servoParams(i)->middle);
 912             sbufWriteU8(dst, servoParams(i)->rate);
 913             sbufWriteU8(dst, servoParams(i)->forwardFromChannel);
 914             sbufWriteU32(dst, servoParams(i)->reversedSources);
 915         }
 916         break;
 917     case MSP_SERVO_MIX_RULES:
 918         for (int i = 0; i < MAX_SERVO_RULES; i++) {
 919             sbufWriteU8(dst, customServoMixers(i)->targetChannel);
 920             sbufWriteU8(dst, customServoMixers(i)->inputSource);
 921             sbufWriteU8(dst, customServoMixers(i)->rate);
 922             sbufWriteU8(dst, customServoMixers(i)->speed);
 923             sbufWriteU8(dst, customServoMixers(i)->min);
 924             sbufWriteU8(dst, customServoMixers(i)->max);
 925             sbufWriteU8(dst, customServoMixers(i)->box);
 926         }
 927         break;
 928 #endif
 929
 930     case MSP_MOTOR:
 931         for (unsigned i = 0; i < 8; i++) {
 932             if (i >= MAX_SUPPORTED_MOTORS || !pwmGetMotors()[i].enabled) {
 933                 sbufWriteU16(dst, 0);
 934                 continue;
 935             }
 936
 937             sbufWriteU16(dst, convertMotorToExternal(motor[i]));
 938         }
 939         break;
 940
 941     case MSP_RC:
 942         for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
 943             sbufWriteU16(dst, rcData[i]);
 944         }
 945         break;
 946
 947     case MSP_ATTITUDE:
 948         sbufWriteU16(dst, attitude.values.roll);
 949         sbufWriteU16(dst, attitude.values.pitch);
 950         sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
 951         break;
 952
 953     case MSP_ALTITUDE:
 954 #if defined(BARO) || defined(SONAR)
 955         sbufWriteU32(dst, altitudeHoldGetEstimatedAltitude());
 956 #else
 957         sbufWriteU32(dst, 0);
 958 #endif
 959         sbufWriteU16(dst, vario);
 960         break;
 961
 962     case MSP_SONAR_ALTITUDE:
 963 #if defined(SONAR)
 964         sbufWriteU32(dst, sonarGetLatestAltitude());
 965 #else
 966         sbufWriteU32(dst, 0);
 967 #endif
 968         break;
 969
 970     case MSP_BOARD_ALIGNMENT_CONFIG:
 971         sbufWriteU16(dst, boardAlignment()->rollDegrees);
 972         sbufWriteU16(dst, boardAlignment()->pitchDegrees);
 973         sbufWriteU16(dst, boardAlignment()->yawDegrees);
 974         break;
 975
 976     case MSP_ARMING_CONFIG:
 977         sbufWriteU8(dst, armingConfig()->auto_disarm_delay);
 978         sbufWriteU8(dst, armingConfig()->disarm_kill_switch);
 979         break;
 980
 981     case MSP_RC_TUNING:
 982         sbufWriteU8(dst, currentControlRateProfile->rcRate8);
 983         sbufWriteU8(dst, currentControlRateProfile->rcExpo8);
 984         for (int i = 0 ; i < 3; i++) {
 985             sbufWriteU8(dst, currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
 986         }
 987         sbufWriteU8(dst, currentControlRateProfile->dynThrPID);
 988         sbufWriteU8(dst, currentControlRateProfile->thrMid8);
 989         sbufWriteU8(dst, currentControlRateProfile->thrExpo8);
 990         sbufWriteU16(dst, currentControlRateProfile->tpa_breakpoint);
 991         sbufWriteU8(dst, currentControlRateProfile->rcYawExpo8);
 992         sbufWriteU8(dst, currentControlRateProfile->rcYawRate8);
 993         break;
 994
 995     case MSP_PID:
 996         for (int i = 0; i < PID_ITEM_COUNT; i++) {
 997             sbufWriteU8(dst, currentPidProfile->P8[i]);
 998             sbufWriteU8(dst, currentPidProfile->I8[i]);
 999             sbufWriteU8(dst, currentPidProfile->D8[i]);
1000         }
1001         break;
1002
1003     case MSP_PIDNAMES:
1004         for (const char *c = pidnames; *c; c++) {
1005             sbufWriteU8(dst, *c);
1006         }
1007         break;
1008
1009     case MSP_PID_CONTROLLER:
1010         sbufWriteU8(dst, PID_CONTROLLER_BETAFLIGHT);
1011         break;
1012
1013     case MSP_MODE_RANGES:
1014         for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
1015             const modeActivationCondition_t *mac = modeActivationConditions(i);
1016             const box_t *box = findBoxByBoxId(mac->modeId);
1017             sbufWriteU8(dst, box->permanentId);
1018             sbufWriteU8(dst, mac->auxChannelIndex);
1019             sbufWriteU8(dst, mac->range.startStep);
1020             sbufWriteU8(dst, mac->range.endStep);
1021         }
1022         break;
1023
1024     case MSP_ADJUSTMENT_RANGES:
1025         for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
1026             const adjustmentRange_t *adjRange = adjustmentRanges(i);
1027             sbufWriteU8(dst, adjRange->adjustmentIndex);
1028             sbufWriteU8(dst, adjRange->auxChannelIndex);
1029             sbufWriteU8(dst, adjRange->range.startStep);
1030             sbufWriteU8(dst, adjRange->range.endStep);
1031             sbufWriteU8(dst, adjRange->adjustmentFunction);
1032             sbufWriteU8(dst, adjRange->auxSwitchChannelIndex);
1033         }
1034         break;
1035
1036     case MSP_BOXNAMES:
1037         serializeBoxNamesReply(dst);
1038         break;
1039
1040     case MSP_BOXIDS:
1041         for (int i = 0; i < activeBoxIdCount; i++) {
1042             const box_t *box = findBoxByBoxId(activeBoxIds[i]);
1043             if (!box) {
1044                 continue;
1045             }
1046             sbufWriteU8(dst, box->permanentId);
1047         }
1048         break;
1049
1050     case MSP_MOTOR_CONFIG:
1051         sbufWriteU16(dst, motorConfig()->minthrottle);
1052         sbufWriteU16(dst, motorConfig()->maxthrottle);
1053         sbufWriteU16(dst, motorConfig()->mincommand);
1054         break;
1055
1056 #ifdef MAG
1057     case MSP_COMPASS_CONFIG:
1058         sbufWriteU16(dst, compassConfig()->mag_declination / 10);
1059         break;
1060 #endif
1061
1062 #ifdef GPS
1063     case MSP_GPS_CONFIG:
1064         sbufWriteU8(dst, gpsConfig()->provider);
1065         sbufWriteU8(dst, gpsConfig()->sbasMode);
1066         sbufWriteU8(dst, gpsConfig()->autoConfig);
1067         sbufWriteU8(dst, gpsConfig()->autoBaud);
1068         break;
1069
1070     case MSP_RAW_GPS:
1071         sbufWriteU8(dst, STATE(GPS_FIX));
1072         sbufWriteU8(dst, GPS_numSat);
1073         sbufWriteU32(dst, GPS_coord[LAT]);
1074         sbufWriteU32(dst, GPS_coord[LON]);
1075         sbufWriteU16(dst, GPS_altitude);
1076         sbufWriteU16(dst, GPS_speed);
1077         sbufWriteU16(dst, GPS_ground_course);
1078         break;
1079
1080     case MSP_COMP_GPS:
1081         sbufWriteU16(dst, GPS_distanceToHome);
1082         sbufWriteU16(dst, GPS_directionToHome);
1083         sbufWriteU8(dst, GPS_update & 1);
1084         break;
1085
1086     case MSP_GPSSVINFO:
1087         sbufWriteU8(dst, GPS_numCh);
1088            for (int i = 0; i < GPS_numCh; i++) {
1089                sbufWriteU8(dst, GPS_svinfo_chn[i]);
1090                sbufWriteU8(dst, GPS_svinfo_svid[i]);
1091                sbufWriteU8(dst, GPS_svinfo_quality[i]);
1092                sbufWriteU8(dst, GPS_svinfo_cno[i]);
1093            }
1094         break;
1095 #endif
1096
1097     case MSP_ACC_TRIM:
1098         sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.pitch);
1099         sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.roll);
1100         break;
1101
1102     case MSP_MIXER_CONFIG:
1103         sbufWriteU8(dst, mixerConfig()->mixerMode);
1104         break;
1105
1106     case MSP_RX_CONFIG:
1107         sbufWriteU8(dst, rxConfig()->serialrx_provider);
1108         sbufWriteU16(dst, rxConfig()->maxcheck);
1109         sbufWriteU16(dst, rxConfig()->midrc);
1110         sbufWriteU16(dst, rxConfig()->mincheck);
1111         sbufWriteU8(dst, rxConfig()->spektrum_sat_bind);
1112         sbufWriteU16(dst, rxConfig()->rx_min_usec);
1113         sbufWriteU16(dst, rxConfig()->rx_max_usec);
1114         sbufWriteU8(dst, rxConfig()->rcInterpolation);
1115         sbufWriteU8(dst, rxConfig()->rcInterpolationInterval);
1116         sbufWriteU16(dst, rxConfig()->airModeActivateThreshold);
1117         sbufWriteU8(dst, rxConfig()->rx_spi_protocol);
1118         sbufWriteU32(dst, rxConfig()->rx_spi_id);
1119         sbufWriteU8(dst, rxConfig()->rx_spi_rf_channel_count);
1120         sbufWriteU8(dst, rxConfig()->fpvCamAngleDegrees);
1121         break;
1122
1123     case MSP_FAILSAFE_CONFIG:
1124         sbufWriteU8(dst, failsafeConfig()->failsafe_delay);
1125         sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay);
1126         sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
1127         sbufWriteU8(dst, failsafeConfig()->failsafe_kill_switch);
1128         sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay);
1129         sbufWriteU8(dst, failsafeConfig()->failsafe_procedure);
1130         break;
1131
1132     case MSP_RXFAIL_CONFIG:
1133         for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
1134             sbufWriteU8(dst, rxFailsafeChannelConfigs(i)->mode);
1135             sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxFailsafeChannelConfigs(i)->step));
1136         }
1137         break;
1138
1139     case MSP_RSSI_CONFIG:
1140         sbufWriteU8(dst, rxConfig()->rssi_channel);
1141         break;
1142
1143     case MSP_RX_MAP:
1144         sbufWriteData(dst, rxConfig()->rcmap, MAX_MAPPABLE_RX_INPUTS);
1145         break;
1146
1147     case MSP_CF_SERIAL_CONFIG:
1148         for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
1149             if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
1150                 continue;
1151             };
1152             sbufWriteU8(dst, serialConfig()->portConfigs[i].identifier);
1153             sbufWriteU16(dst, serialConfig()->portConfigs[i].functionMask);
1154             sbufWriteU8(dst, serialConfig()->portConfigs[i].msp_baudrateIndex);
1155             sbufWriteU8(dst, serialConfig()->portConfigs[i].gps_baudrateIndex);
1156             sbufWriteU8(dst, serialConfig()->portConfigs[i].telemetry_baudrateIndex);
1157             sbufWriteU8(dst, serialConfig()->portConfigs[i].blackbox_baudrateIndex);
1158         }
1159         break;
1160
1161 #ifdef LED_STRIP
1162     case MSP_LED_COLORS:
1163         for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
1164             const hsvColor_t *color = &ledStripConfig()->colors[i];
1165             sbufWriteU16(dst, color->h);
1166             sbufWriteU8(dst, color->s);
1167             sbufWriteU8(dst, color->v);
1168         }
1169         break;
1170
1171     case MSP_LED_STRIP_CONFIG:
1172         for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
1173             const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
1174             sbufWriteU32(dst, *ledConfig);
1175         }
1176         break;
1177
1178     case MSP_LED_STRIP_MODECOLOR:
1179         for (int i = 0; i < LED_MODE_COUNT; i++) {
1180             for (int j = 0; j < LED_DIRECTION_COUNT; j++) {
1181                 sbufWriteU8(dst, i);
1182                 sbufWriteU8(dst, j);
1183                 sbufWriteU8(dst, ledStripConfig()->modeColors[i].color[j]);
1184             }
1185         }
1186
1187         for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) {
1188             sbufWriteU8(dst, LED_MODE_COUNT);
1189             sbufWriteU8(dst, j);
1190             sbufWriteU8(dst, ledStripConfig()->specialColors.color[j]);
1191         }
1192
1193         sbufWriteU8(dst, LED_AUX_CHANNEL);
1194         sbufWriteU8(dst, 0);
1195         sbufWriteU8(dst, ledStripConfig()->ledstrip_aux_channel);
1196         break;
1197 #endif
1198
1199     case MSP_DATAFLASH_SUMMARY:
1200         serializeDataflashSummaryReply(dst);
1201         break;
1202
1203     case MSP_BLACKBOX_CONFIG:
1204 #ifdef BLACKBOX
1205         sbufWriteU8(dst, 1); //Blackbox supported
1206         sbufWriteU8(dst, blackboxConfig()->device);
1207         sbufWriteU8(dst, blackboxConfig()->rate_num);
1208         sbufWriteU8(dst, blackboxConfig()->rate_denom);
1209 #else
1210         sbufWriteU8(dst, 0); // Blackbox not supported
1211         sbufWriteU8(dst, 0);
1212         sbufWriteU8(dst, 0);
1213         sbufWriteU8(dst, 0);
1214 #endif
1215         break;
1216
1217     case MSP_SDCARD_SUMMARY:
1218         serializeSDCardSummaryReply(dst);
1219         break;
1220
1221     case MSP_MOTOR_3D_CONFIG:
1222         sbufWriteU16(dst, flight3DConfig()->deadband3d_low);
1223         sbufWriteU16(dst, flight3DConfig()->deadband3d_high);
1224         sbufWriteU16(dst, flight3DConfig()->neutral3d);
1225         break;
1226
1227     case MSP_RC_DEADBAND:
1228         sbufWriteU8(dst, rcControlsConfig()->deadband);
1229         sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
1230         sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband);
1231         sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle);
1232         break;
1233
1234     case MSP_SENSOR_ALIGNMENT:
1235         sbufWriteU8(dst, gyroConfig()->gyro_align);
1236         sbufWriteU8(dst, accelerometerConfig()->acc_align);
1237         sbufWriteU8(dst, compassConfig()->mag_align);
1238         break;
1239
1240     case MSP_ADVANCED_CONFIG:
1241         if (gyroConfig()->gyro_lpf) {
1242             sbufWriteU8(dst, 8); // If gyro_lpf != OFF then looptime is set to 1000
1243             sbufWriteU8(dst, 1);
1244         } else {
1245             sbufWriteU8(dst, gyroConfig()->gyro_sync_denom);
1246             sbufWriteU8(dst, pidConfig()->pid_process_denom);
1247         }
1248         sbufWriteU8(dst, motorConfig()->dev.useUnsyncedPwm);
1249         sbufWriteU8(dst, motorConfig()->dev.motorPwmProtocol);
1250         sbufWriteU16(dst, motorConfig()->dev.motorPwmRate);
1251         sbufWriteU16(dst, (uint16_t)lrintf(motorConfig()->digitalIdleOffsetPercent * 100));
1252         sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
1253         //!!TODO gyro_isr_update to be added pending decision
1254         //sbufWriteU8(dst, gyroConfig()->gyro_isr_update);
1255         sbufWriteU8(dst, motorConfig()->dev.motorPwmInversion);
1256         break;
1257
1258     case MSP_FILTER_CONFIG :
1259         sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz);
1260         sbufWriteU16(dst, currentPidProfile->dterm_lpf_hz);
1261         sbufWriteU16(dst, currentPidProfile->yaw_lpf_hz);
1262         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1);
1263         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1);
1264         sbufWriteU16(dst, currentPidProfile->dterm_notch_hz);
1265         sbufWriteU16(dst, currentPidProfile->dterm_notch_cutoff);
1266         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2);
1267         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2);
1268         break;
1269
1270     case MSP_PID_ADVANCED:
1271         sbufWriteU16(dst, 0);
1272         sbufWriteU16(dst, 0);
1273         sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit
1274         sbufWriteU8(dst, 0); // reserved
1275         sbufWriteU8(dst, currentPidProfile->vbatPidCompensation);
1276         sbufWriteU8(dst, currentPidProfile->setpointRelaxRatio);
1277         sbufWriteU8(dst, currentPidProfile->dtermSetpointWeight);
1278         sbufWriteU8(dst, 0); // reserved
1279         sbufWriteU8(dst, 0); // reserved
1280         sbufWriteU8(dst, 0); // reserved
1281         sbufWriteU16(dst, (uint16_t)lrintf(currentPidProfile->rateAccelLimit * 10));
1282         sbufWriteU16(dst, (uint16_t)lrintf(currentPidProfile->yawRateAccelLimit * 10));
1283         sbufWriteU8(dst, currentPidProfile->levelAngleLimit);
1284         sbufWriteU8(dst, currentPidProfile->levelSensitivity);
1285         break;
1286
1287     case MSP_SENSOR_CONFIG:
1288         sbufWriteU8(dst, accelerometerConfig()->acc_hardware);
1289         sbufWriteU8(dst, barometerConfig()->baro_hardware);
1290         sbufWriteU8(dst, compassConfig()->mag_hardware);
1291         break;
1292
1293 #if defined(VTX_COMMON)
1294     case MSP_VTX_CONFIG:
1295         {
1296             uint8_t deviceType = vtxCommonGetDeviceType();
1297             if (deviceType != VTXDEV_UNKNOWN) {
1298
1299                 uint8_t band=0, channel=0;
1300                 vtxCommonGetBandChan(&band,&channel);
1301
1302                 uint8_t powerIdx=0; // debug
1303                 vtxCommonGetPowerIndex(&powerIdx);
1304
1305                 uint8_t pitmode=0;
1306                 vtxCommonGetPitmode(&pitmode);
1307
1308                 sbufWriteU8(dst, deviceType);
1309                 sbufWriteU8(dst, band);
1310                 sbufWriteU8(dst, channel);
1311                 sbufWriteU8(dst, powerIdx);
1312                 sbufWriteU8(dst, pitmode);
1313                 // future extensions here...
1314             }
1315             else {
1316                 sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX detected
1317             }
1318         }
1319 #endif
1320         break;
1321
1322     default:
1323         return false;
1324     }
1325     return true;
1326 }
1327 #endif
1328
1329 #ifdef GPS
1330 static void mspFcWpCommand(sbuf_t *dst, sbuf_t *src)
1331 {
1332     uint8_t wp_no;
1333     int32_t lat = 0, lon = 0;
1334     wp_no = sbufReadU8(src);    // get the wp number
1335     if (wp_no == 0) {
1336         lat = GPS_home[LAT];
1337         lon = GPS_home[LON];
1338     } else if (wp_no == 16) {
1339         lat = GPS_hold[LAT];
1340         lon = GPS_hold[LON];
1341     }
1342     sbufWriteU8(dst, wp_no);
1343     sbufWriteU32(dst, lat);
1344     sbufWriteU32(dst, lon);
1345     sbufWriteU32(dst, AltHold);           // altitude (cm) will come here -- temporary implementation to test feature with apps
1346     sbufWriteU16(dst, 0);                 // heading  will come here (deg)
1347     sbufWriteU16(dst, 0);                 // time to stay (ms) will come here
1348     sbufWriteU8(dst, 0);                  // nav flag will come here
1349 }
1350 #endif
1351
1352 #ifdef USE_FLASHFS
1353 static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
1354 {
1355     const unsigned int dataSize = sbufBytesRemaining(src);
1356     const uint32_t readAddress = sbufReadU32(src);
1357     uint16_t readLength;
1358     bool useLegacyFormat;
1359     if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
1360         readLength = sbufReadU16(src);
1361         useLegacyFormat = false;
1362     } else {
1363         readLength = 128;
1364         useLegacyFormat = true;
1365     }
1366
1367     serializeDataflashReadReply(dst, readAddress, readLength, useLegacyFormat);
1368 }
1369 #endif
1370
1371 #ifdef USE_OSD_SLAVE
1372 static mspResult_e mspOsdSlaveProcessInCommand(uint8_t cmdMSP, sbuf_t *src) {
1373     UNUSED(cmdMSP);
1374     UNUSED(src);
1375     // Nothing OSD SLAVE specific yet.
1376     return MSP_RESULT_ERROR;
1377 }
1378 #endif
1379
1380 #ifdef USE_FC
1381 static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
1382 {
1383     uint32_t i;
1384     uint8_t value;
1385     const unsigned int dataSize = sbufBytesRemaining(src);
1386 #ifdef GPS
1387     uint8_t wp_no;
1388     int32_t lat = 0, lon = 0, alt = 0;
1389 #endif
1390     switch (cmdMSP) {
1391     case MSP_SELECT_SETTING:
1392         value = sbufReadU8(src);
1393         if ((value & RATEPROFILE_MASK) == 0) {
1394             if (!ARMING_FLAG(ARMED)) {
1395                 if (value >= MAX_PROFILE_COUNT) {
1396                     value = 0;
1397                 }
1398                 changePidProfile(value);
1399             }
1400         } else {
1401             value = value & ~RATEPROFILE_MASK;
1402
1403             if (value >= CONTROL_RATE_PROFILE_COUNT) {
1404                 value = 0;
1405             }
1406             changeControlRateProfile(value);
1407         }
1408         break;
1409
1410 #if defined(GPS) || defined(MAG)
1411     case MSP_SET_HEADING:
1412         magHold = sbufReadU16(src);
1413         break;
1414 #endif
1415
1416     case MSP_SET_RAW_RC:
1417 #ifdef USE_RX_MSP
1418         {
1419             uint8_t channelCount = dataSize / sizeof(uint16_t);
1420             if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) {
1421                 return MSP_RESULT_ERROR;
1422             } else {
1423                 uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
1424                 for (int i = 0; i < channelCount; i++) {
1425                     frame[i] = sbufReadU16(src);
1426                 }
1427                 rxMspFrameReceive(frame, channelCount);
1428             }
1429         }
1430 #endif
1431         break;
1432     case MSP_SET_ACC_TRIM:
1433         accelerometerConfigMutable()->accelerometerTrims.values.pitch = sbufReadU16(src);
1434         accelerometerConfigMutable()->accelerometerTrims.values.roll  = sbufReadU16(src);
1435         break;
1436     case MSP_SET_ARMING_CONFIG:
1437         armingConfigMutable()->auto_disarm_delay = sbufReadU8(src);
1438         armingConfigMutable()->disarm_kill_switch = sbufReadU8(src);
1439         break;
1440
1441     case MSP_SET_PID_CONTROLLER:
1442         break;
1443
1444     case MSP_SET_PID:
1445         for (int i = 0; i < PID_ITEM_COUNT; i++) {
1446             currentPidProfile->P8[i] = sbufReadU8(src);
1447             currentPidProfile->I8[i] = sbufReadU8(src);
1448             currentPidProfile->D8[i] = sbufReadU8(src);
1449         }
1450         pidInitConfig(currentPidProfile);
1451         break;
1452
1453     case MSP_SET_MODE_RANGE:
1454         i = sbufReadU8(src);
1455         if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
1456             modeActivationCondition_t *mac = modeActivationConditionsMutable(i);
1457             i = sbufReadU8(src);
1458             const box_t *box = findBoxByPermanentId(i);
1459             if (box) {
1460                 mac->modeId = box->boxId;
1461                 mac->auxChannelIndex = sbufReadU8(src);
1462                 mac->range.startStep = sbufReadU8(src);
1463                 mac->range.endStep = sbufReadU8(src);
1464
1465                 useRcControlsConfig(modeActivationConditions(0), currentPidProfile);
1466             } else {
1467                 return MSP_RESULT_ERROR;
1468             }
1469         } else {
1470             return MSP_RESULT_ERROR;
1471         }
1472         break;
1473
1474     case MSP_SET_ADJUSTMENT_RANGE:
1475         i = sbufReadU8(src);
1476         if (i < MAX_ADJUSTMENT_RANGE_COUNT) {
1477             adjustmentRange_t *adjRange = adjustmentRangesMutable(i);
1478             i = sbufReadU8(src);
1479             if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) {
1480                 adjRange->adjustmentIndex = i;
1481                 adjRange->auxChannelIndex = sbufReadU8(src);
1482                 adjRange->range.startStep = sbufReadU8(src);
1483                 adjRange->range.endStep = sbufReadU8(src);
1484                 adjRange->adjustmentFunction = sbufReadU8(src);
1485                 adjRange->auxSwitchChannelIndex = sbufReadU8(src);
1486             } else {
1487                 return MSP_RESULT_ERROR;
1488             }
1489         } else {
1490             return MSP_RESULT_ERROR;
1491         }
1492         break;
1493
1494     case MSP_SET_RC_TUNING:
1495         if (dataSize >= 10) {
1496             currentControlRateProfile->rcRate8 = sbufReadU8(src);
1497             currentControlRateProfile->rcExpo8 = sbufReadU8(src);
1498             for (int i = 0; i < 3; i++) {
1499                 value = sbufReadU8(src);
1500                 currentControlRateProfile->rates[i] = MIN(value, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
1501             }
1502             value = sbufReadU8(src);
1503             currentControlRateProfile->dynThrPID = MIN(value, CONTROL_RATE_CONFIG_TPA_MAX);
1504             currentControlRateProfile->thrMid8 = sbufReadU8(src);
1505             currentControlRateProfile->thrExpo8 = sbufReadU8(src);
1506             currentControlRateProfile->tpa_breakpoint = sbufReadU16(src);
1507             if (dataSize >= 11) {
1508                 currentControlRateProfile->rcYawExpo8 = sbufReadU8(src);
1509             }
1510             if (dataSize >= 12) {
1511                 currentControlRateProfile->rcYawRate8 = sbufReadU8(src);
1512             }
1513             generateThrottleCurve();
1514         } else {
1515             return MSP_RESULT_ERROR;
1516         }
1517         break;
1518
1519     case MSP_SET_MOTOR_CONFIG:
1520         motorConfigMutable()->minthrottle = sbufReadU16(src);
1521         motorConfigMutable()->maxthrottle = sbufReadU16(src);
1522         motorConfigMutable()->mincommand = sbufReadU16(src);
1523         break;
1524
1525 #ifdef GPS
1526     case MSP_SET_GPS_CONFIG:
1527         gpsConfigMutable()->provider = sbufReadU8(src);
1528         gpsConfigMutable()->sbasMode = sbufReadU8(src);
1529         gpsConfigMutable()->autoConfig = sbufReadU8(src);
1530         gpsConfigMutable()->autoBaud = sbufReadU8(src);
1531         break;
1532 #endif
1533
1534 #ifdef MAG
1535     case MSP_SET_COMPASS_CONFIG:
1536         compassConfigMutable()->mag_declination = sbufReadU16(src) * 10;
1537         break;
1538 #endif
1539
1540     case MSP_SET_MOTOR:
1541         for (int i = 0; i < getMotorCount(); i++) {
1542             motor_disarmed[i] = convertExternalToMotor(sbufReadU16(src));
1543         }
1544         break;
1545
1546     case MSP_SET_SERVO_CONFIGURATION:
1547 #ifdef USE_SERVOS
1548         if (dataSize != 1 + 14) {
1549             return MSP_RESULT_ERROR;
1550         }
1551         i = sbufReadU8(src);
1552         if (i >= MAX_SUPPORTED_SERVOS) {
1553             return MSP_RESULT_ERROR;
1554         } else {
1555             servoParamsMutable(i)->min = sbufReadU16(src);
1556             servoParamsMutable(i)->max = sbufReadU16(src);
1557             servoParamsMutable(i)->middle = sbufReadU16(src);
1558             servoParamsMutable(i)->rate = sbufReadU8(src);
1559             servoParamsMutable(i)->forwardFromChannel = sbufReadU8(src);
1560             servoParamsMutable(i)->reversedSources = sbufReadU32(src);
1561         }
1562 #endif
1563         break;
1564
1565     case MSP_SET_SERVO_MIX_RULE:
1566 #ifdef USE_SERVOS
1567         i = sbufReadU8(src);
1568         if (i >= MAX_SERVO_RULES) {
1569             return MSP_RESULT_ERROR;
1570         } else {
1571             customServoMixersMutable(i)->targetChannel = sbufReadU8(src);
1572             customServoMixersMutable(i)->inputSource = sbufReadU8(src);
1573             customServoMixersMutable(i)->rate = sbufReadU8(src);
1574             customServoMixersMutable(i)->speed = sbufReadU8(src);
1575             customServoMixersMutable(i)->min = sbufReadU8(src);
1576             customServoMixersMutable(i)->max = sbufReadU8(src);
1577             customServoMixersMutable(i)->box = sbufReadU8(src);
1578             loadCustomServoMixer();
1579         }
1580 #endif
1581         break;
1582
1583     case MSP_SET_MOTOR_3D_CONFIG:
1584         flight3DConfigMutable()->deadband3d_low = sbufReadU16(src);
1585         flight3DConfigMutable()->deadband3d_high = sbufReadU16(src);
1586         flight3DConfigMutable()->neutral3d = sbufReadU16(src);
1587         break;
1588
1589     case MSP_SET_RC_DEADBAND:
1590         rcControlsConfigMutable()->deadband = sbufReadU8(src);
1591         rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
1592         rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src);
1593         flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src);
1594         break;
1595
1596     case MSP_SET_RESET_CURR_PID:
1597         resetPidProfile(currentPidProfile);
1598         break;
1599     case MSP_SET_SENSOR_ALIGNMENT:
1600         gyroConfigMutable()->gyro_align = sbufReadU8(src);
1601         accelerometerConfigMutable()->acc_align = sbufReadU8(src);
1602         compassConfigMutable()->mag_align = sbufReadU8(src);
1603         break;
1604
1605     case MSP_SET_ADVANCED_CONFIG:
1606         gyroConfigMutable()->gyro_sync_denom = sbufReadU8(src);
1607         pidConfigMutable()->pid_process_denom = sbufReadU8(src);
1608         motorConfigMutable()->dev.useUnsyncedPwm = sbufReadU8(src);
1609 #ifdef USE_DSHOT
1610         motorConfigMutable()->dev.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
1611 #else
1612         motorConfigMutable()->dev.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
1613 #endif
1614         motorConfigMutable()->dev.motorPwmRate = sbufReadU16(src);
1615         if (sbufBytesRemaining(src) >= 2) {
1616             motorConfigMutable()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f;
1617         }
1618         if (sbufBytesRemaining(src)) {
1619             gyroConfigMutable()->gyro_use_32khz = sbufReadU8(src);
1620         }
1621         //!!TODO gyro_isr_update to be added pending decision
1622         /*if (sbufBytesRemaining(src)) {
1623             gyroConfigMutable()->gyro_isr_update = sbufReadU8(src);
1624         }*/
1625         validateAndFixGyroConfig();
1626
1627         if (sbufBytesRemaining(src)) {
1628             motorConfigMutable()->dev.motorPwmInversion = sbufReadU8(src);
1629         }
1630         break;
1631
1632     case MSP_SET_FILTER_CONFIG:
1633         gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src);
1634         currentPidProfile->dterm_lpf_hz = sbufReadU16(src);
1635         currentPidProfile->yaw_lpf_hz = sbufReadU16(src);
1636         if (dataSize > 5) {
1637             gyroConfigMutable()->gyro_soft_notch_hz_1 = sbufReadU16(src);
1638             gyroConfigMutable()->gyro_soft_notch_cutoff_1 = sbufReadU16(src);
1639             currentPidProfile->dterm_notch_hz = sbufReadU16(src);
1640             currentPidProfile->dterm_notch_cutoff = sbufReadU16(src);
1641         }
1642         if (dataSize > 13) {
1643             gyroConfigMutable()->gyro_soft_notch_hz_2 = sbufReadU16(src);
1644             gyroConfigMutable()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
1645         }
1646         // reinitialize the gyro filters with the new values
1647         validateAndFixGyroConfig();
1648         gyroInitFilters();
1649         // reinitialize the PID filters with the new values
1650         pidInitFilters(currentPidProfile);
1651         break;
1652
1653     case MSP_SET_PID_ADVANCED:
1654         sbufReadU16(src);
1655         sbufReadU16(src);
1656         sbufReadU16(src); // was pidProfile.yaw_p_limit
1657         sbufReadU8(src); // reserved
1658         currentPidProfile->vbatPidCompensation = sbufReadU8(src);
1659         currentPidProfile->setpointRelaxRatio = sbufReadU8(src);
1660         currentPidProfile->dtermSetpointWeight = sbufReadU8(src);
1661         sbufReadU8(src); // reserved
1662         sbufReadU8(src); // reserved
1663         sbufReadU8(src); // reserved
1664         currentPidProfile->rateAccelLimit = sbufReadU16(src) / 10.0f;
1665         currentPidProfile->yawRateAccelLimit = sbufReadU16(src) / 10.0f;
1666         if (dataSize > 17) {
1667             currentPidProfile->levelAngleLimit = sbufReadU8(src);
1668             currentPidProfile->levelSensitivity = sbufReadU8(src);
1669         }
1670         pidInitConfig(currentPidProfile);
1671         break;
1672
1673     case MSP_SET_SENSOR_CONFIG:
1674         accelerometerConfigMutable()->acc_hardware = sbufReadU8(src);
1675         barometerConfigMutable()->baro_hardware = sbufReadU8(src);
1676         compassConfigMutable()->mag_hardware = sbufReadU8(src);
1677         break;
1678
1679     case MSP_RESET_CONF:
1680         if (!ARMING_FLAG(ARMED)) {
1681             resetEEPROM();
1682             readEEPROM();
1683         }
1684         break;
1685
1686     case MSP_ACC_CALIBRATION:
1687         if (!ARMING_FLAG(ARMED))
1688             accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
1689         break;
1690
1691     case MSP_MAG_CALIBRATION:
1692         if (!ARMING_FLAG(ARMED))
1693             ENABLE_STATE(CALIBRATE_MAG);
1694         break;
1695
1696     case MSP_EEPROM_WRITE:
1697         if (ARMING_FLAG(ARMED)) {
1698             return MSP_RESULT_ERROR;
1699         }
1700         writeEEPROM();
1701         readEEPROM();
1702         break;
1703
1704 #ifdef BLACKBOX
1705     case MSP_SET_BLACKBOX_CONFIG:
1706         // Don't allow config to be updated while Blackbox is logging
1707         if (blackboxMayEditConfig()) {
1708             blackboxConfigMutable()->device = sbufReadU8(src);
1709             blackboxConfigMutable()->rate_num = sbufReadU8(src);
1710             blackboxConfigMutable()->rate_denom = sbufReadU8(src);
1711         }
1712         break;
1713 #endif
1714
1715 #if defined(USE_RTC6705) || defined(VTX_COMMON)
1716     case MSP_SET_VTX_CONFIG:
1717         {
1718             uint16_t tmp = sbufReadU16(src);
1719 #if defined(USE_RTC6705)
1720             if  (tmp < 40)
1721                 vtxConfigMutable()->vtx_channel = tmp;
1722             if (current_vtx_channel != vtxConfig()->vtx_channel) {
1723                 current_vtx_channel = vtxConfig()->vtx_channel;
1724                 rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
1725             }
1726 #else
1727             if (vtxCommonGetDeviceType() != VTXDEV_UNKNOWN) {
1728
1729                 uint8_t band    = (tmp / 8) + 1;
1730                 uint8_t channel = (tmp % 8) + 1;
1731
1732                 uint8_t current_band=0, current_channel=0;
1733                 vtxCommonGetBandChan(&current_band,&current_channel);
1734                 if ((current_band != band) || (current_channel != channel))
1735                     vtxCommonSetBandChan(band,channel);
1736
1737                 if (sbufBytesRemaining(src) < 2)
1738                     break;
1739
1740                 uint8_t power = sbufReadU8(src);
1741                 uint8_t current_power = 0;
1742                 vtxCommonGetPowerIndex(&current_power);
1743                 if (current_power != power)
1744                     vtxCommonSetPowerByIndex(power);
1745
1746                 uint8_t pitmode = sbufReadU8(src);
1747                 uint8_t current_pitmode = 0;
1748                 vtxCommonGetPitmode(&current_pitmode);
1749                 if (current_pitmode != pitmode)
1750                     vtxCommonSetPitmode(pitmode);
1751             }
1752 #endif
1753         }
1754         break;
1755 #endif
1756
1757 #ifdef USE_FLASHFS
1758     case MSP_DATAFLASH_ERASE:
1759         flashfsEraseCompletely();
1760         break;
1761 #endif
1762
1763 #ifdef GPS
1764     case MSP_SET_RAW_GPS:
1765         if (sbufReadU8(src)) {
1766             ENABLE_STATE(GPS_FIX);
1767         } else {
1768             DISABLE_STATE(GPS_FIX);
1769         }
1770         GPS_numSat = sbufReadU8(src);
1771         GPS_coord[LAT] = sbufReadU32(src);
1772         GPS_coord[LON] = sbufReadU32(src);
1773         GPS_altitude = sbufReadU16(src);
1774         GPS_speed = sbufReadU16(src);
1775         GPS_update |= 2;        // New data signalisation to GPS functions // FIXME Magic Numbers
1776         break;
1777     case MSP_SET_WP:
1778         wp_no = sbufReadU8(src);    //get the wp number
1779         lat = sbufReadU32(src);
1780         lon = sbufReadU32(src);
1781         alt = sbufReadU32(src);     // to set altitude (cm)
1782         sbufReadU16(src);           // future: to set heading (deg)
1783         sbufReadU16(src);           // future: to set time to stay (ms)
1784         sbufReadU8(src);            // future: to set nav flag
1785         if (wp_no == 0) {
1786             GPS_home[LAT] = lat;
1787             GPS_home[LON] = lon;
1788             DISABLE_FLIGHT_MODE(GPS_HOME_MODE);        // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS
1789             ENABLE_STATE(GPS_FIX_HOME);
1790             if (alt != 0)
1791                 AltHold = alt;          // temporary implementation to test feature with apps
1792         } else if (wp_no == 16) {       // OK with SERIAL GPS  --  NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS
1793             GPS_hold[LAT] = lat;
1794             GPS_hold[LON] = lon;
1795             if (alt != 0)
1796                 AltHold = alt;          // temporary implementation to test feature with apps
1797             nav_mode = NAV_MODE_WP;
1798             GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
1799         }
1800         break;
1801 #endif
1802     case MSP_SET_FEATURE_CONFIG:
1803         featureClearAll();
1804         featureSet(sbufReadU32(src)); // features bitmap
1805         break;
1806
1807     case MSP_SET_BOARD_ALIGNMENT_CONFIG:
1808         boardAlignmentMutable()->rollDegrees = sbufReadU16(src);
1809         boardAlignmentMutable()->pitchDegrees = sbufReadU16(src);
1810         boardAlignmentMutable()->yawDegrees = sbufReadU16(src);
1811         break;
1812
1813     case MSP_SET_MIXER_CONFIG:
1814 #ifndef USE_QUAD_MIXER_ONLY
1815         mixerConfigMutable()->mixerMode = sbufReadU8(src);
1816 #else
1817         sbufReadU8(src);
1818 #endif
1819         break;
1820
1821     case MSP_SET_RX_CONFIG:
1822         rxConfigMutable()->serialrx_provider = sbufReadU8(src);
1823         rxConfigMutable()->maxcheck = sbufReadU16(src);
1824         rxConfigMutable()->midrc = sbufReadU16(src);
1825         rxConfigMutable()->mincheck = sbufReadU16(src);
1826         rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src);
1827         if (dataSize > 8) {
1828             rxConfigMutable()->rx_min_usec = sbufReadU16(src);
1829             rxConfigMutable()->rx_max_usec = sbufReadU16(src);
1830         }
1831         if (dataSize > 12) {
1832             rxConfigMutable()->rcInterpolation = sbufReadU8(src);
1833             rxConfigMutable()->rcInterpolationInterval = sbufReadU8(src);
1834             rxConfigMutable()->airModeActivateThreshold = sbufReadU16(src);
1835         }
1836         if (dataSize > 16) {
1837             rxConfigMutable()->rx_spi_protocol = sbufReadU8(src);
1838             rxConfigMutable()->rx_spi_id = sbufReadU32(src);
1839             rxConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(src);
1840         }
1841         if (dataSize > 22) {
1842             rxConfigMutable()->fpvCamAngleDegrees = sbufReadU8(src);
1843         }
1844         break;
1845
1846     case MSP_SET_FAILSAFE_CONFIG:
1847         failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
1848         failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src);
1849         failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src);
1850         failsafeConfigMutable()->failsafe_kill_switch = sbufReadU8(src);
1851         failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src);
1852         failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src);
1853         break;
1854
1855     case MSP_SET_RXFAIL_CONFIG:
1856         i = sbufReadU8(src);
1857         if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) {
1858             rxFailsafeChannelConfigsMutable(i)->mode = sbufReadU8(src);
1859             rxFailsafeChannelConfigsMutable(i)->step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
1860         } else {
1861             return MSP_RESULT_ERROR;
1862         }
1863         break;
1864
1865     case MSP_SET_RSSI_CONFIG:
1866         rxConfigMutable()->rssi_channel = sbufReadU8(src);
1867         break;
1868
1869     case MSP_SET_RX_MAP:
1870         for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) {
1871             rxConfigMutable()->rcmap[i] = sbufReadU8(src);
1872         }
1873         break;
1874
1875     case MSP_SET_CF_SERIAL_CONFIG:
1876         {
1877             uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4);
1878
1879             if (dataSize % portConfigSize != 0) {
1880                 return MSP_RESULT_ERROR;
1881             }
1882
1883             uint8_t remainingPortsInPacket = dataSize / portConfigSize;
1884
1885             while (remainingPortsInPacket--) {
1886                 uint8_t identifier = sbufReadU8(src);
1887
1888                 serialPortConfig_t *portConfig = serialFindPortConfiguration(identifier);
1889                 if (!portConfig) {
1890                     return MSP_RESULT_ERROR;
1891                 }
1892
1893                 portConfig->identifier = identifier;
1894                 portConfig->functionMask = sbufReadU16(src);
1895                 portConfig->msp_baudrateIndex = sbufReadU8(src);
1896                 portConfig->gps_baudrateIndex = sbufReadU8(src);
1897                 portConfig->telemetry_baudrateIndex = sbufReadU8(src);
1898                 portConfig->blackbox_baudrateIndex = sbufReadU8(src);
1899             }
1900         }
1901         break;
1902
1903 #ifdef LED_STRIP
1904     case MSP_SET_LED_COLORS:
1905         for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
1906             hsvColor_t *color = &ledStripConfigMutable()->colors[i];
1907             color->h = sbufReadU16(src);
1908             color->s = sbufReadU8(src);
1909             color->v = sbufReadU8(src);
1910         }
1911         break;
1912
1913     case MSP_SET_LED_STRIP_CONFIG:
1914         {
1915             i = sbufReadU8(src);
1916             if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) {
1917                 return MSP_RESULT_ERROR;
1918             }
1919             ledConfig_t *ledConfig = &ledStripConfigMutable()->ledConfigs[i];
1920             *ledConfig = sbufReadU32(src);
1921             reevaluateLedConfig();
1922         }
1923         break;
1924
1925     case MSP_SET_LED_STRIP_MODECOLOR:
1926         {
1927             ledModeIndex_e modeIdx = sbufReadU8(src);
1928             int funIdx = sbufReadU8(src);
1929             int color = sbufReadU8(src);
1930
1931             if (!setModeColor(modeIdx, funIdx, color))
1932                 return MSP_RESULT_ERROR;
1933         }
1934         break;
1935 #endif
1936
1937     case MSP_SET_NAME:
1938         memset(systemConfigMutable()->name, 0, ARRAYLEN(systemConfig()->name));
1939         for (unsigned int i = 0; i < MIN(MAX_NAME_LENGTH, dataSize); i++) {
1940             systemConfigMutable()->name[i] = sbufReadU8(src);
1941         }
1942         break;
1943
1944     default:
1945         // we do not know how to handle the (valid) message, indicate error MSP $M!
1946         return MSP_RESULT_ERROR;
1947     }
1948     return MSP_RESULT_ACK;
1949 }
1950 #endif
1951
1952 static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
1953 {
1954     const unsigned int dataSize = sbufBytesRemaining(src);
1955     UNUSED(dataSize); // maybe unused due to compiler options
1956
1957     switch (cmdMSP) {
1958 #ifdef TRANSPONDER
1959     case MSP_SET_TRANSPONDER_CONFIG: {
1960         uint8_t provider = sbufReadU8(src);
1961         uint8_t bytesRemaining = dataSize - 1;
1962
1963         if (provider > TRANSPONDER_PROVIDER_COUNT) {
1964             return MSP_RESULT_ERROR;
1965         }
1966
1967         const uint8_t requirementIndex = provider - 1;
1968         const uint8_t transponderDataSize = transponderRequirements[requirementIndex].dataLength;
1969
1970         transponderConfigMutable()->provider = provider;
1971
1972         if (provider == TRANSPONDER_NONE) {
1973             break;
1974         }
1975
1976         if (bytesRemaining != transponderDataSize) {
1977             return MSP_RESULT_ERROR;
1978         }
1979
1980         if (provider != transponderConfig()->provider) {
1981             transponderStopRepeating();
1982         }
1983
1984         memset(transponderConfigMutable()->data, 0, sizeof(transponderConfig()->data));
1985
1986         for (unsigned int i = 0; i < transponderDataSize; i++) {
1987             transponderConfigMutable()->data[i] = sbufReadU8(src);
1988         }
1989         transponderUpdateData();
1990         break;
1991     }
1992 #endif
1993
1994     case MSP_SET_VOLTAGE_METER_CONFIG: {
1995         int id = sbufReadU8(src);
1996
1997         //
1998         // find and configure an ADC voltage sensor
1999         //
2000         int voltageSensorADCIndex;
2001         for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) {
2002             if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) {
2003                 break;
2004             }
2005         }
2006
2007         if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) {
2008             voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src);
2009             voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src);
2010             voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src);
2011         } else {
2012             // if we had any other types of voltage sensor to configure, this is where we'd do it.
2013             return -1;
2014         }
2015         break;
2016     }
2017
2018     case MSP_SET_CURRENT_METER_CONFIG: {
2019         int id = sbufReadU8(src);
2020
2021         switch (id) {
2022             case CURRENT_METER_ID_BATTERY_1:
2023                 currentSensorADCConfigMutable()->scale = sbufReadU16(src);
2024                 currentSensorADCConfigMutable()->offset = sbufReadU16(src);
2025                 break;
2026             case CURRENT_METER_ID_VIRTUAL_1:
2027                 currentSensorVirtualConfigMutable()->scale = sbufReadU16(src);
2028                 currentSensorVirtualConfigMutable()->offset = sbufReadU16(src);
2029                 break;
2030
2031             default:
2032                 return -1;
2033         }
2034
2035         break;
2036     }
2037
2038     case MSP_SET_BATTERY_CONFIG:
2039         batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src);      // vbatlevel_warn1 in MWC2.3 GUI
2040         batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src);      // vbatlevel_warn2 in MWC2.3 GUI
2041         batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src);  // vbatlevel when buzzer starts to alert
2042         batteryConfigMutable()->batteryCapacity = sbufReadU16(src);
2043         batteryConfigMutable()->voltageMeterSource = sbufReadU8(src);
2044         batteryConfigMutable()->currentMeterSource = sbufReadU8(src);
2045         break;
2046
2047 #if defined(OSD) || defined (USE_OSD_SLAVE)
2048     case MSP_SET_OSD_CONFIG:
2049         {
2050             const uint8_t addr = sbufReadU8(src);
2051             // set all the other settings
2052             if ((int8_t)addr == -1) {
2053 #ifdef USE_MAX7456
2054                 vcdProfileMutable()->video_system = sbufReadU8(src);
2055 #else
2056                 sbufReadU8(src); // Skip video system
2057 #endif
2058 #if defined(OSD)
2059                 osdConfigMutable()->units = sbufReadU8(src);
2060                 osdConfigMutable()->rssi_alarm = sbufReadU8(src);
2061                 osdConfigMutable()->cap_alarm = sbufReadU16(src);
2062                 osdConfigMutable()->time_alarm = sbufReadU16(src);
2063                 osdConfigMutable()->alt_alarm = sbufReadU16(src);
2064 #endif
2065             } else {
2066 #if defined(OSD)
2067                 // set a position setting
2068                 const uint16_t pos  = sbufReadU16(src);
2069                 if (addr < OSD_ITEM_COUNT) {
2070                     osdConfigMutable()->item_pos[addr] = pos;
2071                 }
2072 #else
2073                 return MSP_RESULT_ERROR;
2074 #endif
2075             }
2076         }
2077         break;
2078
2079     case MSP_OSD_CHAR_WRITE:
2080 #ifdef USE_MAX7456
2081         {
2082             uint8_t font_data[64];
2083             const uint8_t addr = sbufReadU8(src);
2084             for (int i = 0; i < 54; i++) {
2085                 font_data[i] = sbufReadU8(src);
2086             }
2087             // !!TODO - replace this with a device independent implementation
2088             max7456WriteNvm(addr, font_data);
2089         }
2090 #else
2091         return MSP_RESULT_ERROR;
2092 /*
2093         // just discard the data
2094         sbufReadU8(src);
2095         for (int i = 0; i < 54; i++) {
2096             sbufReadU8(src);
2097         }
2098 */
2099 #endif
2100         break;
2101 #endif // OSD || USE_OSD_SLAVE
2102
2103     default:
2104 #ifdef USE_OSD_SLAVE
2105         return mspOsdSlaveProcessInCommand(cmdMSP, src);
2106 #else
2107         return mspFcProcessInCommand(cmdMSP, src);
2108 #endif
2109     }
2110     return MSP_RESULT_ERROR;
2111 }
2112
2113 /*
2114  * Returns MSP_RESULT_ACK, MSP_RESULT_ERROR or MSP_RESULT_NO_REPLY
2115  */
2116 mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
2117 {
2118     int ret = MSP_RESULT_ACK;
2119     sbuf_t *dst = &reply->buf;
2120     sbuf_t *src = &cmd->buf;
2121     const uint8_t cmdMSP = cmd->cmd;
2122     // initialize reply by default
2123     reply->cmd = cmd->cmd;
2124
2125     if (mspCommonProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
2126         ret = MSP_RESULT_ACK;
2127 #ifdef USE_FC
2128     } else if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
2129         ret = MSP_RESULT_ACK;
2130 #endif
2131 #ifdef USE_OSD_SLAVE
2132     } else if (mspOsdSlaveProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
2133         ret = MSP_RESULT_ACK;
2134 #endif
2135 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
2136     } else if (cmdMSP == MSP_SET_4WAY_IF) {
2137         mspFc4waySerialCommand(dst, src, mspPostProcessFn);
2138         ret = MSP_RESULT_ACK;
2139 #endif
2140 #ifdef GPS
2141     } else if (cmdMSP == MSP_WP) {
2142         mspFcWpCommand(dst, src);
2143         ret = MSP_RESULT_ACK;
2144 #endif
2145 #ifdef USE_FLASHFS
2146     } else if (cmdMSP == MSP_DATAFLASH_READ) {
2147         mspFcDataFlashReadCommand(dst, src);
2148         ret = MSP_RESULT_ACK;
2149 #endif
2150     } else {
2151         ret = mspCommonProcessInCommand(cmdMSP, src);
2152     }
2153     reply->result = ret;
2154     return ret;
2155 }
2156
2157 void mspFcProcessReply(mspPacket_t *reply)
2158 {
2159     sbuf_t *src = &reply->buf;
2160     UNUSED(src); // potentially unused depending on compile options.
2161
2162     switch (reply->cmd) {
2163         case MSP_DISPLAYPORT: {
2164 #ifdef USE_OSD_SLAVE
2165             int subCmd = sbufReadU8(src);
2166
2167             switch (subCmd) {
2168                 case 0: // HEARTBEAT
2169                     //debug[0]++;
2170                     osdSlaveHeartbeat();
2171                     break;
2172                 case 1: // RELEASE
2173                     //debug[1]++;
2174                     break;
2175                 case 2: // CLEAR
2176                     //debug[2]++;
2177                     osdSlaveClearScreen();
2178                     break;
2179                 case 3: {
2180                     //debug[3]++;
2181
2182 #define MSP_OSD_MAX_STRING_LENGTH 30 // FIXME move this
2183                     const uint8_t y = sbufReadU8(src); // row
2184                     const uint8_t x = sbufReadU8(src); // column
2185                     const uint8_t reserved = sbufReadU8(src);
2186                     UNUSED(reserved);
2187
2188                     char buf[MSP_OSD_MAX_STRING_LENGTH + 1];
2189                     const int len = MIN(sbufBytesRemaining(src), MSP_OSD_MAX_STRING_LENGTH);
2190                     sbufReadData(src, &buf, len);
2191
2192                     buf[len] = 0;
2193
2194                     osdSlaveWrite(x, y, buf);
2195
2196                     break;
2197                 }
2198                 case 4: {
2199                     osdSlaveDrawScreen();
2200                 }
2201             }
2202 #endif
2203             break;
2204         }
2205     }
2206 }
2207
2208 /*
2209  * Return a pointer to the process command function
2210  */
2211 #ifdef USE_FC
2212 void mspFcInit(void)
2213 {
2214     initActiveBoxIds();
2215 }
2216 #endif
2217
2218 #ifdef USE_OSD_SLAVE
2219 void mspOsdSlaveInit(void)
2220 {
2221 }
2222 #endif