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