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