src/main/io/serial_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 <stdlib.h>
  21 #include <string.h>
  22 #include <math.h>
  23
  24 #include "build_config.h"
  25 #include "debug.h"
  26 #include "platform.h"
  27
  28 #include "common/axis.h"
  29 #include "common/color.h"
  30 #include "common/maths.h"
  31
  32 #include "drivers/system.h"
  33
  34 #include "drivers/sensor.h"
  35 #include "drivers/accgyro.h"
  36 #include "drivers/compass.h"
  37
  38 #include "drivers/serial.h"
  39 #include "drivers/bus_i2c.h"
  40 #include "drivers/gpio.h"
  41 #include "drivers/timer.h"
  42 #include "drivers/pwm_rx.h"
  43 #include "drivers/sdcard.h"
  44 #include "drivers/buf_writer.h"
  45 #include "drivers/max7456.h"
  46 #include "drivers/vtx_soft_spi_rtc6705.h"
  47 #include "rx/rx.h"
  48 #include "rx/msp.h"
  49
  50 #include "io/beeper.h"
  51 #include "io/escservo.h"
  52 #include "io/rc_controls.h"
  53 #include "io/gps.h"
  54 #include "io/gimbal.h"
  55 #include "io/serial.h"
  56 #include "io/ledstrip.h"
  57 #include "io/flashfs.h"
  58 #include "io/transponder_ir.h"
  59 #include "io/asyncfatfs/asyncfatfs.h"
  60 #include "io/osd.h"
  61 #include "io/vtx.h"
  62
  63 #include "telemetry/telemetry.h"
  64
  65 #include "scheduler/scheduler.h"
  66
  67 #include "sensors/boardalignment.h"
  68 #include "sensors/sensors.h"
  69 #include "sensors/battery.h"
  70 #include "sensors/sonar.h"
  71 #include "sensors/acceleration.h"
  72 #include "sensors/barometer.h"
  73 #include "sensors/compass.h"
  74 #include "sensors/gyro.h"
  75
  76 #include "flight/mixer.h"
  77 #include "flight/pid.h"
  78 #include "flight/imu.h"
  79 #include "flight/failsafe.h"
  80 #include "flight/navigation.h"
  81 #include "flight/altitudehold.h"
  82
  83 #include "blackbox/blackbox.h"
  84
  85 #include "mw.h"
  86
  87 #include "config/runtime_config.h"
  88 #include "config/config.h"
  89 #include "config/config_profile.h"
  90 #include "config/config_master.h"
  91
  92 #include "version.h"
  93 #ifdef USE_HARDWARE_REVISION_DETECTION
  94 #include "hardware_revision.h"
  95 #endif
  96
  97 #include "serial_msp.h"
  98
  99 #include "io/serial_4way.h"
 100
 101 static serialPort_t *mspSerialPort;
 102
 103 extern uint16_t cycleTime; // FIXME dependency on mw.c
 104 extern uint16_t rssi; // FIXME dependency on mw.c
 105 extern void resetPidProfile(pidProfile_t *pidProfile);
 106
 107 void setGyroSamplingSpeed(uint16_t looptime) {
 108     uint16_t gyroSampleRate = 1000;
 109     uint8_t maxDivider = 1;
 110
 111     if (looptime != gyro.targetLooptime || looptime == 0) {
 112         if (looptime == 0) looptime = gyro.targetLooptime; // needed for pid controller changes
 113 #ifdef STM32F303xC
 114         if (looptime < 1000) {
 115             masterConfig.gyro_lpf = 0;
 116             gyroSampleRate = 125;
 117             maxDivider = 8;
 118             masterConfig.pid_process_denom = 1;
 119             masterConfig.acc_hardware = ACC_DEFAULT;
 120             masterConfig.baro_hardware = BARO_DEFAULT;
 121             masterConfig.mag_hardware = MAG_DEFAULT;
 122             if (looptime < 250) {
 123                 masterConfig.acc_hardware = ACC_NONE;
 124                 masterConfig.baro_hardware = BARO_NONE;
 125                 masterConfig.mag_hardware = MAG_NONE;
 126                 masterConfig.pid_process_denom = 2;
 127             } else if (looptime < 375) {
 128                 masterConfig.acc_hardware = CONFIG_FASTLOOP_PREFERRED_ACC;
 129                 masterConfig.baro_hardware = BARO_NONE;
 130                 masterConfig.mag_hardware = MAG_NONE;
 131                 masterConfig.pid_process_denom = 2;
 132             }
 133             masterConfig.gyro_sync_denom = constrain(looptime / gyroSampleRate, 1, maxDivider);
 134         } else {
 135             masterConfig.gyro_lpf = 0;
 136             masterConfig.gyro_sync_denom = 8;
 137             masterConfig.acc_hardware = ACC_DEFAULT;
 138             masterConfig.baro_hardware = BARO_DEFAULT;
 139             masterConfig.mag_hardware = MAG_DEFAULT;
 140         }
 141 #else
 142         if (looptime < 1000) {
 143             masterConfig.gyro_lpf = 0;
 144             masterConfig.acc_hardware = ACC_NONE;
 145             masterConfig.baro_hardware = BARO_NONE;
 146             masterConfig.mag_hardware = MAG_NONE;
 147             gyroSampleRate = 125;
 148             maxDivider = 8;
 149             masterConfig.pid_process_denom = 1;
 150             if (currentProfile->pidProfile.pidController == PID_CONTROLLER_FLOAT) {
 151                 masterConfig.pid_process_denom = 2;
 152             }
 153             if (looptime < 250) {
 154                 masterConfig.pid_process_denom = 4;
 155             } else if (looptime < 375) {
 156                 if (currentProfile->pidProfile.pidController == PID_CONTROLLER_FLOAT) {
 157                     masterConfig.pid_process_denom = 3;
 158                 } else {
 159                     masterConfig.pid_process_denom = 2;
 160                 }
 161             }
 162             masterConfig.gyro_sync_denom = constrain(looptime / gyroSampleRate, 1, maxDivider);
 163         } else {
 164             masterConfig.gyro_lpf = 0;
 165             masterConfig.gyro_sync_denom = 8;
 166             masterConfig.acc_hardware = ACC_DEFAULT;
 167             masterConfig.baro_hardware = BARO_DEFAULT;
 168             masterConfig.mag_hardware = MAG_DEFAULT;
 169             masterConfig.pid_process_denom = 1;
 170         }
 171 #endif
 172     }
 173 }
 174
 175 void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse);
 176
 177 const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
 178
 179 typedef struct box_e {
 180     const uint8_t boxId;         // see boxId_e
 181     const char *boxName;            // GUI-readable box name
 182     const uint8_t permanentId;      //
 183 } box_t;
 184
 185 // FIXME remove ;'s
 186 static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
 187     { BOXARM, "ARM;", 0 },
 188     { BOXANGLE, "ANGLE;", 1 },
 189     { BOXHORIZON, "HORIZON;", 2 },
 190     { BOXBARO, "BARO;", 3 },
 191     //{ BOXVARIO, "VARIO;", 4 },
 192     { BOXMAG, "MAG;", 5 },
 193     { BOXHEADFREE, "HEADFREE;", 6 },
 194     { BOXHEADADJ, "HEADADJ;", 7 },
 195     { BOXCAMSTAB, "CAMSTAB;", 8 },
 196     { BOXCAMTRIG, "CAMTRIG;", 9 },
 197     { BOXGPSHOME, "GPS HOME;", 10 },
 198     { BOXGPSHOLD, "GPS HOLD;", 11 },
 199     { BOXPASSTHRU, "PASSTHRU;", 12 },
 200     { BOXBEEPERON, "BEEPER;", 13 },
 201     { BOXLEDMAX, "LEDMAX;", 14 },
 202     { BOXLEDLOW, "LEDLOW;", 15 },
 203     { BOXLLIGHTS, "LLIGHTS;", 16 },
 204     { BOXCALIB, "CALIB;", 17 },
 205     { BOXGOV, "GOVERNOR;", 18 },
 206     { BOXOSD, "OSD SW;", 19 },
 207     { BOXTELEMETRY, "TELEMETRY;", 20 },
 208     { BOXGTUNE, "GTUNE;", 21 },
 209     { BOXSONAR, "SONAR;", 22 },
 210     { BOXSERVO1, "SERVO1;", 23 },
 211     { BOXSERVO2, "SERVO2;", 24 },
 212     { BOXSERVO3, "SERVO3;", 25 },
 213     { BOXBLACKBOX, "BLACKBOX;", 26 },
 214     { BOXFAILSAFE, "FAILSAFE;", 27 },
 215     { BOXAIRMODE, "AIR MODE;", 28 },
 216     { BOX3DDISABLESWITCH, "DISABLE 3D SWITCH;", 29},
 217     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 218 };
 219
 220 // this is calculated at startup based on enabled features.
 221 static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT];
 222 // this is the number of filled indexes in above array
 223 static uint8_t activeBoxIdCount = 0;
 224 // from mixer.c
 225 extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 226
 227 // cause reboot after MSP processing complete
 228 static bool isRebootScheduled = false;
 229
 230 static const char pidnames[] =
 231     "ROLL;"
 232     "PITCH;"
 233     "YAW;"
 234     "ALT;"
 235     "Pos;"
 236     "PosR;"
 237     "NavR;"
 238     "LEVEL;"
 239     "MAG;"
 240     "VEL;";
 241
 242 typedef enum {
 243     MSP_SDCARD_STATE_NOT_PRESENT = 0,
 244     MSP_SDCARD_STATE_FATAL       = 1,
 245     MSP_SDCARD_STATE_CARD_INIT   = 2,
 246     MSP_SDCARD_STATE_FS_INIT     = 3,
 247     MSP_SDCARD_STATE_READY       = 4
 248 } mspSDCardState_e;
 249
 250
 251 STATIC_UNIT_TESTED mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
 252
 253 STATIC_UNIT_TESTED mspPort_t *currentPort;
 254 STATIC_UNIT_TESTED bufWriter_t *writer;
 255
 256 static void serialize8(uint8_t a)
 257 {
 258     bufWriterAppend(writer, a);
 259     currentPort->checksum ^= a;
 260 }
 261
 262 static void serialize16(uint16_t a)
 263 {
 264     serialize8((uint8_t)(a >> 0));
 265     serialize8((uint8_t)(a >> 8));
 266 }
 267
 268 static void serialize32(uint32_t a)
 269 {
 270     serialize16((uint16_t)(a >> 0));
 271     serialize16((uint16_t)(a >> 16));
 272 }
 273
 274 static uint8_t read8(void)
 275 {
 276     return currentPort->inBuf[currentPort->indRX++] & 0xff;
 277 }
 278
 279 static uint16_t read16(void)
 280 {
 281     uint16_t t = read8();
 282     t += (uint16_t)read8() << 8;
 283     return t;
 284 }
 285
 286 static uint32_t read32(void)
 287 {
 288     uint32_t t = read16();
 289     t += (uint32_t)read16() << 16;
 290     return t;
 291 }
 292
 293 static void headSerialResponse(uint8_t err, uint8_t responseBodySize)
 294 {
 295     serialBeginWrite(mspSerialPort);
 296
 297     serialize8('$');
 298     serialize8('M');
 299     serialize8(err ? '!' : '>');
 300     currentPort->checksum = 0;               // start calculating a new checksum
 301     serialize8(responseBodySize);
 302     serialize8(currentPort->cmdMSP);
 303 }
 304
 305 static void headSerialReply(uint8_t responseBodySize)
 306 {
 307     headSerialResponse(0, responseBodySize);
 308 }
 309
 310 static void headSerialError(uint8_t responseBodySize)
 311 {
 312     headSerialResponse(1, responseBodySize);
 313 }
 314
 315 static void tailSerialReply(void)
 316 {
 317     serialize8(currentPort->checksum);
 318     serialEndWrite(mspSerialPort);
 319 }
 320
 321 static void s_struct(uint8_t *cb, uint8_t siz)
 322 {
 323     headSerialReply(siz);
 324     while (siz--)
 325         serialize8(*cb++);
 326 }
 327
 328 static void serializeNames(const char *s)
 329 {
 330     const char *c;
 331     for (c = s; *c; c++)
 332         serialize8(*c);
 333 }
 334
 335 static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
 336 {
 337     uint8_t boxIndex;
 338     const box_t *candidate;
 339     for (boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
 340         candidate = &boxes[boxIndex];
 341         if (candidate->boxId == activeBoxId) {
 342             return candidate;
 343         }
 344     }
 345     return NULL;
 346 }
 347
 348 static const box_t *findBoxByPermenantId(uint8_t permenantId)
 349 {
 350     uint8_t boxIndex;
 351     const box_t *candidate;
 352     for (boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
 353         candidate = &boxes[boxIndex];
 354         if (candidate->permanentId == permenantId) {
 355             return candidate;
 356         }
 357     }
 358     return NULL;
 359 }
 360
 361 static void serializeBoxNamesReply(void)
 362 {
 363     int i, activeBoxId, j, flag = 1, count = 0, len;
 364     const box_t *box;
 365
 366 reset:
 367     // in first run of the loop, we grab total size of junk to be sent
 368     // then come back and actually send it
 369     for (i = 0; i < activeBoxIdCount; i++) {
 370         activeBoxId = activeBoxIds[i];
 371
 372         box = findBoxByActiveBoxId(activeBoxId);
 373         if (!box) {
 374             continue;
 375         }
 376
 377         len = strlen(box->boxName);
 378         if (flag) {
 379             count += len;
 380         } else {
 381             for (j = 0; j < len; j++)
 382                 serialize8(box->boxName[j]);
 383         }
 384     }
 385
 386     if (flag) {
 387         headSerialReply(count);
 388         flag = 0;
 389         goto reset;
 390     }
 391 }
 392
 393 static void serializeSDCardSummaryReply(void)
 394 {
 395     headSerialReply(3 + 4 + 4);
 396
 397 #ifdef USE_SDCARD
 398     uint8_t flags = 1 /* SD card supported */ ;
 399     uint8_t state = 0;
 400
 401     serialize8(flags);
 402
 403     // Merge the card and filesystem states together
 404     if (!sdcard_isInserted()) {
 405         state = MSP_SDCARD_STATE_NOT_PRESENT;
 406     } else if (!sdcard_isFunctional()) {
 407         state = MSP_SDCARD_STATE_FATAL;
 408     } else {
 409         switch (afatfs_getFilesystemState()) {
 410             case AFATFS_FILESYSTEM_STATE_READY:
 411                 state = MSP_SDCARD_STATE_READY;
 412             break;
 413             case AFATFS_FILESYSTEM_STATE_INITIALIZATION:
 414                 if (sdcard_isInitialized()) {
 415                     state = MSP_SDCARD_STATE_FS_INIT;
 416                 } else {
 417                     state = MSP_SDCARD_STATE_CARD_INIT;
 418                 }
 419             break;
 420             case AFATFS_FILESYSTEM_STATE_FATAL:
 421             case AFATFS_FILESYSTEM_STATE_UNKNOWN:
 422                 state = MSP_SDCARD_STATE_FATAL;
 423             break;
 424         }
 425     }
 426
 427     serialize8(state);
 428     serialize8(afatfs_getLastError());
 429     // Write free space and total space in kilobytes
 430     serialize32(afatfs_getContiguousFreeSpace() / 1024);
 431     serialize32(sdcard_getMetadata()->numBlocks / 2); // Block size is half a kilobyte
 432 #else
 433     serialize8(0);
 434     serialize8(0);
 435     serialize8(0);
 436     serialize32(0);
 437     serialize32(0);
 438 #endif
 439 }
 440
 441 static void serializeDataflashSummaryReply(void)
 442 {
 443     headSerialReply(1 + 3 * 4);
 444 #ifdef USE_FLASHFS
 445     const flashGeometry_t *geometry = flashfsGetGeometry();
 446     uint8_t flags = (flashfsIsReady() ? 1 : 0) | 2 /* FlashFS is supported */;
 447
 448     serialize8(flags);
 449     serialize32(geometry->sectors);
 450     serialize32(geometry->totalSize);
 451     serialize32(flashfsGetOffset()); // Effectively the current number of bytes stored on the volume
 452 #else
 453     serialize8(0); // FlashFS is neither ready nor supported
 454     serialize32(0);
 455     serialize32(0);
 456     serialize32(0);
 457 #endif
 458 }
 459
 460 #ifdef USE_FLASHFS
 461 static void serializeDataflashReadReply(uint32_t address, uint8_t size)
 462 {
 463     uint8_t buffer[128];
 464     int bytesRead;
 465
 466     if (size > sizeof(buffer)) {
 467         size = sizeof(buffer);
 468     }
 469
 470     headSerialReply(4 + size);
 471
 472     serialize32(address);
 473
 474     // bytesRead will be lower than that requested if we reach end of volume
 475     bytesRead = flashfsReadAbs(address, buffer, size);
 476
 477     for (int i = 0; i < bytesRead; i++) {
 478         serialize8(buffer[i]);
 479     }
 480 }
 481 #endif
 482
 483 static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort)
 484 {
 485     memset(mspPortToReset, 0, sizeof(mspPort_t));
 486
 487     mspPortToReset->port = serialPort;
 488 }
 489
 490 void mspAllocateSerialPorts(serialConfig_t *serialConfig)
 491 {
 492     UNUSED(serialConfig);
 493
 494     serialPort_t *serialPort;
 495
 496     uint8_t portIndex = 0;
 497
 498     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP);
 499
 500     while (portConfig && portIndex < MAX_MSP_PORT_COUNT) {
 501         mspPort_t *mspPort = &mspPorts[portIndex];
 502         if (mspPort->port) {
 503             portIndex++;
 504             continue;
 505         }
 506
 507         serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
 508         if (serialPort) {
 509             resetMspPort(mspPort, serialPort);
 510             portIndex++;
 511         }
 512
 513         portConfig = findNextSerialPortConfig(FUNCTION_MSP);
 514     }
 515 }
 516
 517 void mspReleasePortIfAllocated(serialPort_t *serialPort)
 518 {
 519     uint8_t portIndex;
 520     for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
 521         mspPort_t *candidateMspPort = &mspPorts[portIndex];
 522         if (candidateMspPort->port == serialPort) {
 523             closeSerialPort(serialPort);
 524             memset(candidateMspPort, 0, sizeof(mspPort_t));
 525         }
 526     }
 527 }
 528
 529 void mspInit(serialConfig_t *serialConfig)
 530 {
 531     // calculate used boxes based on features and fill availableBoxes[] array
 532     memset(activeBoxIds, 0xFF, sizeof(activeBoxIds));
 533
 534     activeBoxIdCount = 0;
 535     activeBoxIds[activeBoxIdCount++] = BOXARM;
 536
 537     if (sensors(SENSOR_ACC)) {
 538         activeBoxIds[activeBoxIdCount++] = BOXANGLE;
 539         activeBoxIds[activeBoxIdCount++] = BOXHORIZON;
 540     }
 541
 542     if (!feature(FEATURE_AIRMODE)) activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
 543     activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH;
 544
 545     if (sensors(SENSOR_BARO)) {
 546         activeBoxIds[activeBoxIdCount++] = BOXBARO;
 547     }
 548
 549     if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
 550         activeBoxIds[activeBoxIdCount++] = BOXMAG;
 551         activeBoxIds[activeBoxIdCount++] = BOXHEADFREE;
 552         activeBoxIds[activeBoxIdCount++] = BOXHEADADJ;
 553     }
 554
 555     if (feature(FEATURE_SERVO_TILT))
 556         activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB;
 557
 558 #ifdef GPS
 559     if (feature(FEATURE_GPS)) {
 560         activeBoxIds[activeBoxIdCount++] = BOXGPSHOME;
 561         activeBoxIds[activeBoxIdCount++] = BOXGPSHOLD;
 562     }
 563 #endif
 564
 565     if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE)
 566         activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
 567
 568     activeBoxIds[activeBoxIdCount++] = BOXBEEPERON;
 569
 570 #ifdef LED_STRIP
 571     if (feature(FEATURE_LED_STRIP)) {
 572         activeBoxIds[activeBoxIdCount++] = BOXLEDLOW;
 573     }
 574 #endif
 575
 576     if (feature(FEATURE_INFLIGHT_ACC_CAL))
 577         activeBoxIds[activeBoxIdCount++] = BOXCALIB;
 578
 579     activeBoxIds[activeBoxIdCount++] = BOXOSD;
 580
 581     if (feature(FEATURE_TELEMETRY) && masterConfig.telemetryConfig.telemetry_switch)
 582         activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY;
 583
 584     if (feature(FEATURE_SONAR)){
 585         activeBoxIds[activeBoxIdCount++] = BOXSONAR;
 586     }
 587
 588 #ifdef USE_SERVOS
 589     if (masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) {
 590         activeBoxIds[activeBoxIdCount++] = BOXSERVO1;
 591         activeBoxIds[activeBoxIdCount++] = BOXSERVO2;
 592         activeBoxIds[activeBoxIdCount++] = BOXSERVO3;
 593     }
 594 #endif
 595
 596 #ifdef BLACKBOX
 597     if (feature(FEATURE_BLACKBOX)){
 598         activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX;
 599     }
 600 #endif
 601
 602     if (feature(FEATURE_FAILSAFE)){
 603         activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE;
 604     }
 605
 606 #ifdef GTUNE
 607     activeBoxIds[activeBoxIdCount++] = BOXGTUNE;
 608 #endif
 609
 610     memset(mspPorts, 0x00, sizeof(mspPorts));
 611     mspAllocateSerialPorts(serialConfig);
 612 }
 613
 614 #define IS_ENABLED(mask) (mask == 0 ? 0 : 1)
 615
 616 static uint32_t packFlightModeFlags(void)
 617 {
 618     uint32_t i, junk, tmp;
 619
 620     // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
 621     // Requires new Multiwii protocol version to fix
 622     // It would be preferable to setting the enabled bits based on BOXINDEX.
 623     junk = 0;
 624     tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE |
 625         IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON |
 626         IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << BOXBARO |
 627         IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG |
 628         IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE |
 629         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ |
 630         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB |
 631         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMTRIG)) << BOXCAMTRIG |
 632         IS_ENABLED(FLIGHT_MODE(GPS_HOME_MODE)) << BOXGPSHOME |
 633         IS_ENABLED(FLIGHT_MODE(GPS_HOLD_MODE)) << BOXGPSHOLD |
 634         IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU |
 635         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON |
 636         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX)) << BOXLEDMAX |
 637         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW |
 638         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS |
 639         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB)) << BOXCALIB |
 640         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
 641         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
 642         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
 643         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
 644         IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
 645         IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
 646         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
 647         IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE |
 648         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE;
 649
 650     for (i = 0; i < activeBoxIdCount; i++) {
 651         int flag = (tmp & (1 << activeBoxIds[i]));
 652         if (flag)
 653             junk |= 1 << i;
 654     }
 655
 656     return junk;
 657 }
 658
 659 static bool processOutCommand(uint8_t cmdMSP)
 660 {
 661     uint32_t i;
 662     uint8_t len;
 663 #ifdef GPS
 664     uint8_t wp_no;
 665     int32_t lat = 0, lon = 0;
 666 #endif
 667
 668     switch (cmdMSP) {
 669     case MSP_API_VERSION:
 670         headSerialReply(
 671             1 + // protocol version length
 672             API_VERSION_LENGTH
 673         );
 674         serialize8(MSP_PROTOCOL_VERSION);
 675
 676         serialize8(API_VERSION_MAJOR);
 677         serialize8(API_VERSION_MINOR);
 678         break;
 679
 680     case MSP_FC_VARIANT:
 681         headSerialReply(FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
 682
 683         for (i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) {
 684             serialize8(flightControllerIdentifier[i]);
 685         }
 686         break;
 687
 688     case MSP_FC_VERSION:
 689         headSerialReply(FLIGHT_CONTROLLER_VERSION_LENGTH);
 690
 691         serialize8(FC_VERSION_MAJOR);
 692         serialize8(FC_VERSION_MINOR);
 693         serialize8(FC_VERSION_PATCH_LEVEL);
 694         break;
 695
 696     case MSP_BOARD_INFO:
 697         headSerialReply(
 698             BOARD_IDENTIFIER_LENGTH +
 699             BOARD_HARDWARE_REVISION_LENGTH
 700         );
 701         for (i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) {
 702             serialize8(boardIdentifier[i]);
 703         }
 704 #ifdef USE_HARDWARE_REVISION_DETECTION
 705         serialize16(hardwareRevision);
 706 #else
 707         serialize16(0); // No other build targets currently have hardware revision detection.
 708 #endif
 709         break;
 710
 711     case MSP_BUILD_INFO:
 712         headSerialReply(
 713                 BUILD_DATE_LENGTH +
 714                 BUILD_TIME_LENGTH +
 715                 GIT_SHORT_REVISION_LENGTH
 716         );
 717
 718         for (i = 0; i < BUILD_DATE_LENGTH; i++) {
 719             serialize8(buildDate[i]);
 720         }
 721         for (i = 0; i < BUILD_TIME_LENGTH; i++) {
 722             serialize8(buildTime[i]);
 723         }
 724
 725         for (i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) {
 726             serialize8(shortGitRevision[i]);
 727         }
 728         break;
 729
 730     // DEPRECATED - Use MSP_API_VERSION
 731     case MSP_IDENT:
 732         headSerialReply(7);
 733         serialize8(MW_VERSION);
 734         serialize8(masterConfig.mixerMode);
 735         serialize8(MSP_PROTOCOL_VERSION);
 736         serialize32(CAP_DYNBALANCE); // "capability"
 737         break;
 738
 739     case MSP_STATUS_EX:
 740         headSerialReply(12);
 741         serialize16(cycleTime);
 742 #ifdef USE_I2C
 743         serialize16(i2cGetErrorCounter());
 744 #else
 745         serialize16(0);
 746 #endif
 747         serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
 748         serialize32(packFlightModeFlags());
 749         serialize8(masterConfig.current_profile_index);
 750         serialize16(constrain(averageSystemLoadPercent, 0, 100));
 751         break;
 752
 753     case MSP_NAME:
 754         len = strlen(masterConfig.name);
 755         headSerialReply(len);
 756         for (uint8_t i=0; i<len; i++) {
 757             serialize8(masterConfig.name[i]);
 758         }
 759         break;
 760
 761     case MSP_STATUS:
 762         headSerialReply(11);
 763         serialize16(cycleTime);
 764 #ifdef USE_I2C
 765         serialize16(i2cGetErrorCounter());
 766 #else
 767         serialize16(0);
 768 #endif
 769         serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
 770         serialize32(packFlightModeFlags());
 771         serialize8(masterConfig.current_profile_index);
 772         break;
 773     case MSP_RAW_IMU:
 774         headSerialReply(18);
 775
 776         // Hack scale due to choice of units for sensor data in multiwii
 777         const uint8_t scale = (acc.acc_1G > 512) ? 4 : 1;
 778
 779         for (i = 0; i < 3; i++)
 780             serialize16(accSmooth[i] / scale);
 781         for (i = 0; i < 3; i++)
 782             serialize16(gyroADC[i]);
 783         for (i = 0; i < 3; i++)
 784             serialize16(magADC[i]);
 785         break;
 786 #ifdef USE_SERVOS
 787     case MSP_SERVO:
 788         s_struct((uint8_t *)&servo, MAX_SUPPORTED_SERVOS * 2);
 789         break;
 790     case MSP_SERVO_CONFIGURATIONS:
 791         headSerialReply(MAX_SUPPORTED_SERVOS * sizeof(servoParam_t));
 792         for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
 793             serialize16(masterConfig.servoConf[i].min);
 794             serialize16(masterConfig.servoConf[i].max);
 795             serialize16(masterConfig.servoConf[i].middle);
 796             serialize8(masterConfig.servoConf[i].rate);
 797             serialize8(masterConfig.servoConf[i].angleAtMin);
 798             serialize8(masterConfig.servoConf[i].angleAtMax);
 799             serialize8(masterConfig.servoConf[i].forwardFromChannel);
 800             serialize32(masterConfig.servoConf[i].reversedSources);
 801         }
 802         break;
 803     case MSP_SERVO_MIX_RULES:
 804         headSerialReply(MAX_SERVO_RULES * sizeof(servoMixer_t));
 805         for (i = 0; i < MAX_SERVO_RULES; i++) {
 806             serialize8(masterConfig.customServoMixer[i].targetChannel);
 807             serialize8(masterConfig.customServoMixer[i].inputSource);
 808             serialize8(masterConfig.customServoMixer[i].rate);
 809             serialize8(masterConfig.customServoMixer[i].speed);
 810             serialize8(masterConfig.customServoMixer[i].min);
 811             serialize8(masterConfig.customServoMixer[i].max);
 812             serialize8(masterConfig.customServoMixer[i].box);
 813         }
 814         break;
 815 #endif
 816     case MSP_MOTOR:
 817         s_struct((uint8_t *)motor, 16);
 818         break;
 819     case MSP_RC:
 820         headSerialReply(2 * rxRuntimeConfig.channelCount);
 821         for (i = 0; i < rxRuntimeConfig.channelCount; i++)
 822             serialize16(rcData[i]);
 823         break;
 824     case MSP_ATTITUDE:
 825         headSerialReply(6);
 826         serialize16(attitude.values.roll);
 827         serialize16(attitude.values.pitch);
 828         serialize16(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
 829         break;
 830     case MSP_ALTITUDE:
 831         headSerialReply(6);
 832 #if defined(BARO) || defined(SONAR)
 833         serialize32(altitudeHoldGetEstimatedAltitude());
 834 #else
 835         serialize32(0);
 836 #endif
 837         serialize16(vario);
 838         break;
 839     case MSP_SONAR_ALTITUDE:
 840         headSerialReply(4);
 841 #if defined(SONAR)
 842         serialize32(sonarGetLatestAltitude());
 843 #else
 844         serialize32(0);
 845 #endif
 846         break;
 847     case MSP_ANALOG:
 848         headSerialReply(7);
 849         serialize8((uint8_t)constrain(vbat, 0, 255));
 850         serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
 851         serialize16(rssi);
 852         if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) {
 853             serialize16((uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
 854         } else
 855             serialize16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
 856         break;
 857     case MSP_ARMING_CONFIG:
 858         headSerialReply(2);
 859         serialize8(masterConfig.auto_disarm_delay);
 860         serialize8(masterConfig.disarm_kill_switch);
 861         break;
 862     case MSP_LOOP_TIME:
 863         headSerialReply(2);
 864         serialize16((uint16_t)gyro.targetLooptime);
 865         break;
 866     case MSP_RC_TUNING:
 867         headSerialReply(11);
 868         serialize8(currentControlRateProfile->rcRate8);
 869         serialize8(currentControlRateProfile->rcExpo8);
 870         for (i = 0 ; i < 3; i++) {
 871             serialize8(currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
 872         }
 873         serialize8(currentControlRateProfile->dynThrPID);
 874         serialize8(currentControlRateProfile->thrMid8);
 875         serialize8(currentControlRateProfile->thrExpo8);
 876         serialize16(currentControlRateProfile->tpa_breakpoint);
 877         serialize8(currentControlRateProfile->rcYawExpo8);
 878         break;
 879     case MSP_PID:
 880         headSerialReply(3 * PID_ITEM_COUNT);
 881         for (i = 0; i < PID_ITEM_COUNT; i++) {
 882             serialize8(currentProfile->pidProfile.P8[i]);
 883             serialize8(currentProfile->pidProfile.I8[i]);
 884             serialize8(currentProfile->pidProfile.D8[i]);
 885         }
 886         break;
 887     case MSP_PIDNAMES:
 888         headSerialReply(sizeof(pidnames) - 1);
 889         serializeNames(pidnames);
 890         break;
 891     case MSP_PID_CONTROLLER:
 892         headSerialReply(1);
 893         serialize8(currentProfile->pidProfile.pidController);
 894         break;
 895     case MSP_MODE_RANGES:
 896         headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT);
 897         for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
 898             modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i];
 899             const box_t *box = &boxes[mac->modeId];
 900             serialize8(box->permanentId);
 901             serialize8(mac->auxChannelIndex);
 902             serialize8(mac->range.startStep);
 903             serialize8(mac->range.endStep);
 904         }
 905         break;
 906     case MSP_ADJUSTMENT_RANGES:
 907         headSerialReply(MAX_ADJUSTMENT_RANGE_COUNT * (
 908                 1 + // adjustment index/slot
 909                 1 + // aux channel index
 910                 1 + // start step
 911                 1 + // end step
 912                 1 + // adjustment function
 913                 1   // aux switch channel index
 914         ));
 915         for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
 916             adjustmentRange_t *adjRange = &masterConfig.adjustmentRanges[i];
 917             serialize8(adjRange->adjustmentIndex);
 918             serialize8(adjRange->auxChannelIndex);
 919             serialize8(adjRange->range.startStep);
 920             serialize8(adjRange->range.endStep);
 921             serialize8(adjRange->adjustmentFunction);
 922             serialize8(adjRange->auxSwitchChannelIndex);
 923         }
 924         break;
 925     case MSP_BOXNAMES:
 926         serializeBoxNamesReply();
 927         break;
 928     case MSP_BOXIDS:
 929         headSerialReply(activeBoxIdCount);
 930         for (i = 0; i < activeBoxIdCount; i++) {
 931             const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]);
 932             if (!box) {
 933                 continue;
 934             }
 935             serialize8(box->permanentId);
 936         }
 937         break;
 938     case MSP_MISC:
 939         headSerialReply(2 * 5 + 3 + 3 + 2 + 4);
 940         serialize16(masterConfig.rxConfig.midrc);
 941
 942         serialize16(masterConfig.escAndServoConfig.minthrottle);
 943         serialize16(masterConfig.escAndServoConfig.maxthrottle);
 944         serialize16(masterConfig.escAndServoConfig.mincommand);
 945
 946         serialize16(masterConfig.failsafeConfig.failsafe_throttle);
 947
 948 #ifdef GPS
 949         serialize8(masterConfig.gpsConfig.provider); // gps_type
 950         serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
 951         serialize8(masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas
 952 #else
 953         serialize8(0); // gps_type
 954         serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
 955         serialize8(0); // gps_ubx_sbas
 956 #endif
 957         serialize8(masterConfig.batteryConfig.multiwiiCurrentMeterOutput);
 958         serialize8(masterConfig.rxConfig.rssi_channel);
 959         serialize8(0);
 960
 961         serialize16(masterConfig.mag_declination / 10);
 962
 963         serialize8(masterConfig.batteryConfig.vbatscale);
 964         serialize8(masterConfig.batteryConfig.vbatmincellvoltage);
 965         serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage);
 966         serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage);
 967         break;
 968
 969     case MSP_MOTOR_PINS:
 970         // FIXME This is hardcoded and should not be.
 971         headSerialReply(8);
 972         for (i = 0; i < 8; i++)
 973             serialize8(i + 1);
 974         break;
 975 #ifdef GPS
 976     case MSP_RAW_GPS:
 977         headSerialReply(16);
 978         serialize8(STATE(GPS_FIX));
 979         serialize8(GPS_numSat);
 980         serialize32(GPS_coord[LAT]);
 981         serialize32(GPS_coord[LON]);
 982         serialize16(GPS_altitude);
 983         serialize16(GPS_speed);
 984         serialize16(GPS_ground_course);
 985         break;
 986     case MSP_COMP_GPS:
 987         headSerialReply(5);
 988         serialize16(GPS_distanceToHome);
 989         serialize16(GPS_directionToHome);
 990         serialize8(GPS_update & 1);
 991         break;
 992     case MSP_WP:
 993         wp_no = read8();    // get the wp number
 994         headSerialReply(18);
 995         if (wp_no == 0) {
 996             lat = GPS_home[LAT];
 997             lon = GPS_home[LON];
 998         } else if (wp_no == 16) {
 999             lat = GPS_hold[LAT];
1000             lon = GPS_hold[LON];
1001         }
1002         serialize8(wp_no);
1003         serialize32(lat);
1004         serialize32(lon);
1005         serialize32(AltHold);           // altitude (cm) will come here -- temporary implementation to test feature with apps
1006         serialize16(0);                 // heading  will come here (deg)
1007         serialize16(0);                 // time to stay (ms) will come here
1008         serialize8(0);                  // nav flag will come here
1009         break;
1010     case MSP_GPSSVINFO:
1011         headSerialReply(1 + (GPS_numCh * 4));
1012         serialize8(GPS_numCh);
1013            for (i = 0; i < GPS_numCh; i++){
1014                serialize8(GPS_svinfo_chn[i]);
1015                serialize8(GPS_svinfo_svid[i]);
1016                serialize8(GPS_svinfo_quality[i]);
1017                serialize8(GPS_svinfo_cno[i]);
1018            }
1019         break;
1020 #endif
1021     case MSP_DEBUG:
1022         headSerialReply(DEBUG16_VALUE_COUNT * sizeof(debug[0]));
1023
1024         // output some useful QA statistics
1025         // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000);         // XX0YY [crystal clock : core clock]
1026
1027         for (i = 0; i < DEBUG16_VALUE_COUNT; i++)
1028             serialize16(debug[i]);      // 4 variables are here for general monitoring purpose
1029         break;
1030
1031     // Additional commands that are not compatible with MultiWii
1032     case MSP_ACC_TRIM:
1033         headSerialReply(4);
1034         serialize16(masterConfig.accelerometerTrims.values.pitch);
1035         serialize16(masterConfig.accelerometerTrims.values.roll);
1036         break;
1037
1038     case MSP_UID:
1039         headSerialReply(12);
1040         serialize32(U_ID_0);
1041         serialize32(U_ID_1);
1042         serialize32(U_ID_2);
1043         break;
1044
1045     case MSP_FEATURE:
1046         headSerialReply(4);
1047         serialize32(featureMask());
1048         break;
1049
1050     case MSP_BOARD_ALIGNMENT:
1051         headSerialReply(6);
1052         serialize16(masterConfig.boardAlignment.rollDegrees);
1053         serialize16(masterConfig.boardAlignment.pitchDegrees);
1054         serialize16(masterConfig.boardAlignment.yawDegrees);
1055         break;
1056
1057     case MSP_VOLTAGE_METER_CONFIG:
1058         headSerialReply(4);
1059         serialize8(masterConfig.batteryConfig.vbatscale);
1060         serialize8(masterConfig.batteryConfig.vbatmincellvoltage);
1061         serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage);
1062         serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage);
1063         break;
1064
1065     case MSP_CURRENT_METER_CONFIG:
1066         headSerialReply(7);
1067         serialize16(masterConfig.batteryConfig.currentMeterScale);
1068         serialize16(masterConfig.batteryConfig.currentMeterOffset);
1069         serialize8(masterConfig.batteryConfig.currentMeterType);
1070         serialize16(masterConfig.batteryConfig.batteryCapacity);
1071         break;
1072
1073     case MSP_MIXER:
1074         headSerialReply(1);
1075         serialize8(masterConfig.mixerMode);
1076         break;
1077
1078     case MSP_RX_CONFIG:
1079         headSerialReply(12);
1080         serialize8(masterConfig.rxConfig.serialrx_provider);
1081         serialize16(masterConfig.rxConfig.maxcheck);
1082         serialize16(masterConfig.rxConfig.midrc);
1083         serialize16(masterConfig.rxConfig.mincheck);
1084         serialize8(masterConfig.rxConfig.spektrum_sat_bind);
1085         serialize16(masterConfig.rxConfig.rx_min_usec);
1086         serialize16(masterConfig.rxConfig.rx_max_usec);
1087         break;
1088
1089     case MSP_FAILSAFE_CONFIG:
1090         headSerialReply(8);
1091         serialize8(masterConfig.failsafeConfig.failsafe_delay);
1092         serialize8(masterConfig.failsafeConfig.failsafe_off_delay);
1093         serialize16(masterConfig.failsafeConfig.failsafe_throttle);
1094         serialize8(masterConfig.failsafeConfig.failsafe_kill_switch);
1095         serialize16(masterConfig.failsafeConfig.failsafe_throttle_low_delay);
1096         serialize8(masterConfig.failsafeConfig.failsafe_procedure);
1097         break;
1098
1099     case MSP_RXFAIL_CONFIG:
1100         headSerialReply(3 * (rxRuntimeConfig.channelCount));
1101         for (i = 0; i < rxRuntimeConfig.channelCount; i++) {
1102             serialize8(masterConfig.rxConfig.failsafe_channel_configurations[i].mode);
1103             serialize16(RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.failsafe_channel_configurations[i].step));
1104         }
1105         break;
1106
1107     case MSP_RSSI_CONFIG:
1108         headSerialReply(1);
1109         serialize8(masterConfig.rxConfig.rssi_channel);
1110         break;
1111
1112     case MSP_RX_MAP:
1113         headSerialReply(MAX_MAPPABLE_RX_INPUTS);
1114         for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++)
1115             serialize8(masterConfig.rxConfig.rcmap[i]);
1116         break;
1117
1118     case MSP_BF_CONFIG:
1119         headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 2 + 2);
1120         serialize8(masterConfig.mixerMode);
1121
1122         serialize32(featureMask());
1123
1124         serialize8(masterConfig.rxConfig.serialrx_provider);
1125
1126         serialize16(masterConfig.boardAlignment.rollDegrees);
1127         serialize16(masterConfig.boardAlignment.pitchDegrees);
1128         serialize16(masterConfig.boardAlignment.yawDegrees);
1129
1130         serialize16(masterConfig.batteryConfig.currentMeterScale);
1131         serialize16(masterConfig.batteryConfig.currentMeterOffset);
1132         break;
1133
1134     case MSP_CF_SERIAL_CONFIG:
1135         headSerialReply(
1136             ((sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4)) * serialGetAvailablePortCount())
1137         );
1138         for (i = 0; i < SERIAL_PORT_COUNT; i++) {
1139             if (!serialIsPortAvailable(masterConfig.serialConfig.portConfigs[i].identifier)) {
1140                 continue;
1141             };
1142             serialize8(masterConfig.serialConfig.portConfigs[i].identifier);
1143             serialize16(masterConfig.serialConfig.portConfigs[i].functionMask);
1144             serialize8(masterConfig.serialConfig.portConfigs[i].msp_baudrateIndex);
1145             serialize8(masterConfig.serialConfig.portConfigs[i].gps_baudrateIndex);
1146             serialize8(masterConfig.serialConfig.portConfigs[i].telemetry_baudrateIndex);
1147             serialize8(masterConfig.serialConfig.portConfigs[i].blackbox_baudrateIndex);
1148         }
1149         break;
1150
1151 #ifdef LED_STRIP
1152     case MSP_LED_COLORS:
1153         headSerialReply(CONFIGURABLE_COLOR_COUNT * 4);
1154         for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) {
1155             hsvColor_t *color = &masterConfig.colors[i];
1156             serialize16(color->h);
1157             serialize8(color->s);
1158             serialize8(color->v);
1159         }
1160         break;
1161
1162     case MSP_LED_STRIP_CONFIG:
1163         headSerialReply(MAX_LED_STRIP_LENGTH * 7);
1164         for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) {
1165             ledConfig_t *ledConfig = &masterConfig.ledConfigs[i];
1166             serialize16((ledConfig->flags & LED_DIRECTION_MASK) >> LED_DIRECTION_BIT_OFFSET);
1167             serialize16((ledConfig->flags & LED_FUNCTION_MASK) >> LED_FUNCTION_BIT_OFFSET);
1168             serialize8(GET_LED_X(ledConfig));
1169             serialize8(GET_LED_Y(ledConfig));
1170             serialize8(ledConfig->color);
1171         }
1172         break;
1173 #endif
1174
1175     case MSP_DATAFLASH_SUMMARY:
1176         serializeDataflashSummaryReply();
1177         break;
1178
1179 #ifdef USE_FLASHFS
1180     case MSP_DATAFLASH_READ:
1181         {
1182             uint32_t readAddress = read32();
1183
1184             serializeDataflashReadReply(readAddress, 128);
1185         }
1186         break;
1187 #endif
1188
1189     case MSP_BLACKBOX_CONFIG:
1190         headSerialReply(4);
1191
1192 #ifdef BLACKBOX
1193         serialize8(1); //Blackbox supported
1194         serialize8(masterConfig.blackbox_device);
1195         serialize8(masterConfig.blackbox_rate_num);
1196         serialize8(masterConfig.blackbox_rate_denom);
1197 #else
1198         serialize8(0); // Blackbox not supported
1199         serialize8(0);
1200         serialize8(0);
1201         serialize8(0);
1202 #endif
1203         break;
1204
1205     case MSP_SDCARD_SUMMARY:
1206         serializeSDCardSummaryReply();
1207         break;
1208
1209     case MSP_TRANSPONDER_CONFIG:
1210 #ifdef TRANSPONDER
1211         headSerialReply(1 + sizeof(masterConfig.transponderData));
1212
1213         serialize8(1); //Transponder supported
1214
1215         for (i = 0; i < sizeof(masterConfig.transponderData); i++) {
1216             serialize8(masterConfig.transponderData[i]);
1217         }
1218 #else
1219         headSerialReply(1);
1220         serialize8(0); // Transponder not supported
1221 #endif
1222         break;
1223
1224     case MSP_OSD_CONFIG:
1225 #ifdef OSD
1226         headSerialReply(2 + (OSD_MAX_ITEMS * 2));
1227         serialize8(1); // OSD supported
1228         // send video system (AUTO/PAL/NTSC)
1229         serialize8(masterConfig.osdProfile.video_system);
1230         for (i = 0; i < OSD_MAX_ITEMS; i++) {
1231             serialize16(masterConfig.osdProfile.item_pos[i]);
1232         }
1233 #else
1234         headSerialReply(1);
1235         serialize8(0); // OSD not supported
1236 #endif
1237         break;
1238
1239     case MSP_BF_BUILD_INFO:
1240         headSerialReply(11 + 4 + 4);
1241         for (i = 0; i < 11; i++)
1242         serialize8(buildDate[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
1243         serialize32(0); // future exp
1244         serialize32(0); // future exp
1245         break;
1246
1247     case MSP_3D:
1248         headSerialReply(2 * 4);
1249         serialize16(masterConfig.flight3DConfig.deadband3d_low);
1250         serialize16(masterConfig.flight3DConfig.deadband3d_high);
1251         serialize16(masterConfig.flight3DConfig.neutral3d);
1252         break;
1253
1254     case MSP_RC_DEADBAND:
1255         headSerialReply(3);
1256         serialize8(masterConfig.rcControlsConfig.deadband);
1257         serialize8(masterConfig.rcControlsConfig.yaw_deadband);
1258         serialize8(masterConfig.rcControlsConfig.alt_hold_deadband);
1259         serialize16(masterConfig.flight3DConfig.deadband3d_throttle);
1260         break;
1261     case MSP_SENSOR_ALIGNMENT:
1262         headSerialReply(3);
1263         serialize8(masterConfig.sensorAlignmentConfig.gyro_align);
1264         serialize8(masterConfig.sensorAlignmentConfig.acc_align);
1265         serialize8(masterConfig.sensorAlignmentConfig.mag_align);
1266         break;
1267     case MSP_PID_ADVANCED_CONFIG :
1268         headSerialReply(6);
1269         serialize8(masterConfig.gyro_sync_denom);
1270         serialize8(masterConfig.pid_process_denom);
1271         serialize8(masterConfig.use_unsyncedPwm);
1272         serialize8(masterConfig.motor_pwm_protocol);
1273         serialize16(masterConfig.motor_pwm_rate);
1274         break;
1275     case MSP_FILTER_CONFIG :
1276         headSerialReply(5);
1277         serialize8(masterConfig.gyro_soft_lpf_hz);
1278         serialize16(currentProfile->pidProfile.dterm_lpf_hz);
1279         serialize16(currentProfile->pidProfile.yaw_lpf_hz);
1280         break;
1281     case MSP_ADVANCED_TUNING:
1282         headSerialReply(3 * 2 + 2);
1283         serialize16(currentProfile->pidProfile.rollPitchItermIgnoreRate);
1284         serialize16(currentProfile->pidProfile.yawItermIgnoreRate);
1285         serialize16(currentProfile->pidProfile.yaw_p_limit);
1286         serialize8(currentProfile->pidProfile.deltaMethod);
1287         serialize8(masterConfig.batteryConfig.vbatPidCompensation);
1288         break;
1289     case MSP_SPECIAL_PARAMETERS:
1290         headSerialReply(1 + 2 + 1 + 2);
1291         serialize8(currentControlRateProfile->rcYawRate8);
1292         serialize16(masterConfig.rxConfig.airModeActivateThreshold);
1293         serialize8(masterConfig.rxConfig.rcSmoothInterval);
1294         serialize16(masterConfig.escAndServoConfig.escDesyncProtection);
1295         break;
1296     case MSP_SENSOR_CONFIG:
1297         headSerialReply(3);
1298         serialize8(masterConfig.acc_hardware);
1299         serialize8(masterConfig.baro_hardware);
1300         serialize8(masterConfig.mag_hardware);
1301         break;
1302
1303     default:
1304         return false;
1305     }
1306     return true;
1307 }
1308
1309 static bool processInCommand(void)
1310 {
1311     uint32_t i;
1312     uint16_t tmp;
1313     uint8_t rate;
1314     uint8_t oldPid;
1315 #ifdef GPS
1316     uint8_t wp_no;
1317     int32_t lat = 0, lon = 0, alt = 0;
1318 #endif
1319 #ifdef OSD
1320     uint8_t addr, font_data[64];
1321 #endif
1322     switch (currentPort->cmdMSP) {
1323     case MSP_SELECT_SETTING:
1324         if (!ARMING_FLAG(ARMED)) {
1325             masterConfig.current_profile_index = read8();
1326             if (masterConfig.current_profile_index > 1) {
1327                 masterConfig.current_profile_index = 0;
1328             }
1329             writeEEPROM();
1330             readEEPROM();
1331         }
1332         break;
1333     case MSP_SET_HEAD:
1334         magHold = read16();
1335         break;
1336     case MSP_SET_RAW_RC:
1337         {
1338             uint8_t channelCount = currentPort->dataSize / sizeof(uint16_t);
1339             if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) {
1340                 headSerialError(0);
1341             } else {
1342                 uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
1343
1344                 for (i = 0; i < channelCount; i++) {
1345                     frame[i] = read16();
1346                 }
1347
1348                 rxMspFrameReceive(frame, channelCount);
1349             }
1350         }
1351         break;
1352     case MSP_SET_ACC_TRIM:
1353         masterConfig.accelerometerTrims.values.pitch = read16();
1354         masterConfig.accelerometerTrims.values.roll  = read16();
1355         break;
1356     case MSP_SET_ARMING_CONFIG:
1357         masterConfig.auto_disarm_delay = read8();
1358         masterConfig.disarm_kill_switch = read8();
1359         break;
1360     case MSP_SET_LOOP_TIME:
1361         setGyroSamplingSpeed(read16());
1362         break;
1363     case MSP_SET_PID_CONTROLLER:
1364         oldPid = currentProfile->pidProfile.pidController;
1365         currentProfile->pidProfile.pidController = constrain(read8(), 1, 2);
1366         pidSetController(currentProfile->pidProfile.pidController);
1367         if (oldPid != currentProfile->pidProfile.pidController) setGyroSamplingSpeed(0); // recalculate looptimes for new PID
1368         break;
1369     case MSP_SET_PID:
1370         for (i = 0; i < PID_ITEM_COUNT; i++) {
1371             currentProfile->pidProfile.P8[i] = read8();
1372             currentProfile->pidProfile.I8[i] = read8();
1373             currentProfile->pidProfile.D8[i] = read8();
1374         }
1375         break;
1376     case MSP_SET_MODE_RANGE:
1377         i = read8();
1378         if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
1379             modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i];
1380             i = read8();
1381             const box_t *box = findBoxByPermenantId(i);
1382             if (box) {
1383                 mac->modeId = box->boxId;
1384                 mac->auxChannelIndex = read8();
1385                 mac->range.startStep = read8();
1386                 mac->range.endStep = read8();
1387
1388                 useRcControlsConfig(masterConfig.modeActivationConditions, &masterConfig.escAndServoConfig, &currentProfile->pidProfile);
1389             } else {
1390                 headSerialError(0);
1391             }
1392         } else {
1393             headSerialError(0);
1394         }
1395         break;
1396     case MSP_SET_ADJUSTMENT_RANGE:
1397         i = read8();
1398         if (i < MAX_ADJUSTMENT_RANGE_COUNT) {
1399             adjustmentRange_t *adjRange = &masterConfig.adjustmentRanges[i];
1400             i = read8();
1401             if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) {
1402                 adjRange->adjustmentIndex = i;
1403                 adjRange->auxChannelIndex = read8();
1404                 adjRange->range.startStep = read8();
1405                 adjRange->range.endStep = read8();
1406                 adjRange->adjustmentFunction = read8();
1407                 adjRange->auxSwitchChannelIndex = read8();
1408             } else {
1409                 headSerialError(0);
1410             }
1411         } else {
1412             headSerialError(0);
1413         }
1414         break;
1415
1416     case MSP_SET_RC_TUNING:
1417         if (currentPort->dataSize >= 10) {
1418             currentControlRateProfile->rcRate8 = read8();
1419             currentControlRateProfile->rcExpo8 = read8();
1420             for (i = 0; i < 3; i++) {
1421                 rate = read8();
1422                 currentControlRateProfile->rates[i] = MIN(rate, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
1423             }
1424             rate = read8();
1425             currentControlRateProfile->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX);
1426             currentControlRateProfile->thrMid8 = read8();
1427             currentControlRateProfile->thrExpo8 = read8();
1428             currentControlRateProfile->tpa_breakpoint = read16();
1429             if (currentPort->dataSize >= 11) {
1430                 currentControlRateProfile->rcYawExpo8 = read8();
1431             }
1432         } else {
1433             headSerialError(0);
1434         }
1435         break;
1436     case MSP_SET_MISC:
1437         tmp = read16();
1438         if (tmp < 1600 && tmp > 1400)
1439             masterConfig.rxConfig.midrc = tmp;
1440
1441         masterConfig.escAndServoConfig.minthrottle = read16();
1442         masterConfig.escAndServoConfig.maxthrottle = read16();
1443         masterConfig.escAndServoConfig.mincommand = read16();
1444
1445         masterConfig.failsafeConfig.failsafe_throttle = read16();
1446
1447 #ifdef GPS
1448         masterConfig.gpsConfig.provider = read8(); // gps_type
1449         read8(); // gps_baudrate
1450         masterConfig.gpsConfig.sbasMode = read8(); // gps_ubx_sbas
1451 #else
1452         read8(); // gps_type
1453         read8(); // gps_baudrate
1454         read8(); // gps_ubx_sbas
1455 #endif
1456         masterConfig.batteryConfig.multiwiiCurrentMeterOutput = read8();
1457         masterConfig.rxConfig.rssi_channel = read8();
1458         read8();
1459
1460         masterConfig.mag_declination = read16() * 10;
1461
1462         masterConfig.batteryConfig.vbatscale = read8();           // actual vbatscale as intended
1463         masterConfig.batteryConfig.vbatmincellvoltage = read8();  // vbatlevel_warn1 in MWC2.3 GUI
1464         masterConfig.batteryConfig.vbatmaxcellvoltage = read8();  // vbatlevel_warn2 in MWC2.3 GUI
1465         masterConfig.batteryConfig.vbatwarningcellvoltage = read8();  // vbatlevel when buzzer starts to alert
1466         break;
1467     case MSP_SET_MOTOR:
1468         for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8
1469             motor_disarmed[i] = read16();
1470         break;
1471     case MSP_SET_SERVO_CONFIGURATION:
1472 #ifdef USE_SERVOS
1473         if (currentPort->dataSize != 1 + sizeof(servoParam_t)) {
1474             headSerialError(0);
1475             break;
1476         }
1477         i = read8();
1478         if (i >= MAX_SUPPORTED_SERVOS) {
1479             headSerialError(0);
1480         } else {
1481             masterConfig.servoConf[i].min = read16();
1482             masterConfig.servoConf[i].max = read16();
1483             masterConfig.servoConf[i].middle = read16();
1484             masterConfig.servoConf[i].rate = read8();
1485             masterConfig.servoConf[i].angleAtMin = read8();
1486             masterConfig.servoConf[i].angleAtMax = read8();
1487             masterConfig.servoConf[i].forwardFromChannel = read8();
1488             masterConfig.servoConf[i].reversedSources = read32();
1489         }
1490 #endif
1491         break;
1492
1493     case MSP_SET_SERVO_MIX_RULE:
1494 #ifdef USE_SERVOS
1495         i = read8();
1496         if (i >= MAX_SERVO_RULES) {
1497             headSerialError(0);
1498         } else {
1499             masterConfig.customServoMixer[i].targetChannel = read8();
1500             masterConfig.customServoMixer[i].inputSource = read8();
1501             masterConfig.customServoMixer[i].rate = read8();
1502             masterConfig.customServoMixer[i].speed = read8();
1503             masterConfig.customServoMixer[i].min = read8();
1504             masterConfig.customServoMixer[i].max = read8();
1505             masterConfig.customServoMixer[i].box = read8();
1506             loadCustomServoMixer();
1507         }
1508 #endif
1509         break;
1510
1511     case MSP_SET_3D:
1512         masterConfig.flight3DConfig.deadband3d_low = read16();
1513         masterConfig.flight3DConfig.deadband3d_high = read16();
1514         masterConfig.flight3DConfig.neutral3d = read16();
1515         break;
1516
1517     case MSP_SET_RC_DEADBAND:
1518         masterConfig.rcControlsConfig.deadband = read8();
1519         masterConfig.rcControlsConfig.yaw_deadband = read8();
1520         masterConfig.rcControlsConfig.alt_hold_deadband = read8();
1521         masterConfig.flight3DConfig.deadband3d_throttle = read16();
1522         break;
1523
1524     case MSP_SET_RESET_CURR_PID:
1525         resetPidProfile(&currentProfile->pidProfile);
1526         break;
1527
1528     case MSP_SET_SENSOR_ALIGNMENT:
1529         masterConfig.sensorAlignmentConfig.gyro_align = read8();
1530         masterConfig.sensorAlignmentConfig.acc_align = read8();
1531         masterConfig.sensorAlignmentConfig.mag_align = read8();
1532         break;
1533
1534     case MSP_RESET_CONF:
1535         if (!ARMING_FLAG(ARMED)) {
1536             resetEEPROM();
1537             readEEPROM();
1538         }
1539         break;
1540     case MSP_ACC_CALIBRATION:
1541         if (!ARMING_FLAG(ARMED))
1542             accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
1543         break;
1544     case MSP_MAG_CALIBRATION:
1545         if (!ARMING_FLAG(ARMED))
1546             ENABLE_STATE(CALIBRATE_MAG);
1547         break;
1548     case MSP_EEPROM_WRITE:
1549         if (ARMING_FLAG(ARMED)) {
1550             headSerialError(0);
1551             return true;
1552         }
1553         writeEEPROM();
1554         readEEPROM();
1555         break;
1556
1557 #ifdef BLACKBOX
1558     case MSP_SET_BLACKBOX_CONFIG:
1559         // Don't allow config to be updated while Blackbox is logging
1560         if (blackboxMayEditConfig()) {
1561             masterConfig.blackbox_device = read8();
1562             masterConfig.blackbox_rate_num = read8();
1563             masterConfig.blackbox_rate_denom = read8();
1564         }
1565         break;
1566 #endif
1567
1568 #ifdef TRANSPONDER
1569     case MSP_SET_TRANSPONDER_CONFIG:
1570         if (currentPort->dataSize != sizeof(masterConfig.transponderData)) {
1571             headSerialError(0);
1572             break;
1573         }
1574
1575         for (i = 0; i < sizeof(masterConfig.transponderData); i++) {
1576             masterConfig.transponderData[i] = read8();
1577         }
1578
1579         transponderUpdateData(masterConfig.transponderData);
1580         break;
1581 #endif
1582 #ifdef OSD
1583     case MSP_SET_OSD_CONFIG:
1584         addr = read8();
1585         // set all the other settings
1586         if ((int8_t)addr == -1) {
1587             masterConfig.osdProfile.video_system = read8();
1588         }
1589         // set a position setting
1590         else {
1591             masterConfig.osdProfile.item_pos[addr] = read16();
1592         }
1593         break;
1594     case MSP_OSD_CHAR_WRITE:
1595         addr = read8();
1596         for (i = 0; i < 54; i++) {
1597             font_data[i] = read8();
1598         }
1599         max7456_write_nvm(addr, font_data);
1600         break;
1601 #endif
1602
1603 #ifdef USE_RTC6705
1604     case MSP_SET_VTX_CONFIG:
1605         tmp = read16();
1606         if  (tmp < 40)
1607             masterConfig.vtx_channel = tmp;
1608         if (current_vtx_channel != masterConfig.vtx_channel) {
1609             current_vtx_channel = masterConfig.vtx_channel;
1610             rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
1611         }
1612         break;
1613 #endif
1614
1615 #ifdef USE_FLASHFS
1616     case MSP_DATAFLASH_ERASE:
1617         flashfsEraseCompletely();
1618         break;
1619 #endif
1620
1621 #ifdef GPS
1622     case MSP_SET_RAW_GPS:
1623         if (read8()) {
1624             ENABLE_STATE(GPS_FIX);
1625         } else {
1626             DISABLE_STATE(GPS_FIX);
1627         }
1628         GPS_numSat = read8();
1629         GPS_coord[LAT] = read32();
1630         GPS_coord[LON] = read32();
1631         GPS_altitude = read16();
1632         GPS_speed = read16();
1633         GPS_update |= 2;        // New data signalisation to GPS functions // FIXME Magic Numbers
1634         break;
1635     case MSP_SET_WP:
1636         wp_no = read8();    //get the wp number
1637         lat = read32();
1638         lon = read32();
1639         alt = read32();     // to set altitude (cm)
1640         read16();           // future: to set heading (deg)
1641         read16();           // future: to set time to stay (ms)
1642         read8();            // future: to set nav flag
1643         if (wp_no == 0) {
1644             GPS_home[LAT] = lat;
1645             GPS_home[LON] = lon;
1646             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
1647             ENABLE_STATE(GPS_FIX_HOME);
1648             if (alt != 0)
1649                 AltHold = alt;          // temporary implementation to test feature with apps
1650         } 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
1651             GPS_hold[LAT] = lat;
1652             GPS_hold[LON] = lon;
1653             if (alt != 0)
1654                 AltHold = alt;          // temporary implementation to test feature with apps
1655             nav_mode = NAV_MODE_WP;
1656             GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
1657         }
1658         break;
1659 #endif
1660     case MSP_SET_FEATURE:
1661         featureClearAll();
1662         featureSet(read32()); // features bitmap
1663         break;
1664
1665     case MSP_SET_BOARD_ALIGNMENT:
1666         masterConfig.boardAlignment.rollDegrees = read16();
1667         masterConfig.boardAlignment.pitchDegrees = read16();
1668         masterConfig.boardAlignment.yawDegrees = read16();
1669         break;
1670
1671     case MSP_SET_VOLTAGE_METER_CONFIG:
1672         masterConfig.batteryConfig.vbatscale = read8();           // actual vbatscale as intended
1673         masterConfig.batteryConfig.vbatmincellvoltage = read8();  // vbatlevel_warn1 in MWC2.3 GUI
1674         masterConfig.batteryConfig.vbatmaxcellvoltage = read8();  // vbatlevel_warn2 in MWC2.3 GUI
1675         masterConfig.batteryConfig.vbatwarningcellvoltage = read8();  // vbatlevel when buzzer starts to alert
1676         break;
1677
1678     case MSP_SET_CURRENT_METER_CONFIG:
1679         masterConfig.batteryConfig.currentMeterScale = read16();
1680         masterConfig.batteryConfig.currentMeterOffset = read16();
1681         masterConfig.batteryConfig.currentMeterType = read8();
1682         masterConfig.batteryConfig.batteryCapacity = read16();
1683         break;
1684
1685 #ifndef USE_QUAD_MIXER_ONLY
1686     case MSP_SET_MIXER:
1687         masterConfig.mixerMode = read8();
1688         break;
1689 #endif
1690
1691     case MSP_SET_RX_CONFIG:
1692         masterConfig.rxConfig.serialrx_provider = read8();
1693         masterConfig.rxConfig.maxcheck = read16();
1694         masterConfig.rxConfig.midrc = read16();
1695         masterConfig.rxConfig.mincheck = read16();
1696         masterConfig.rxConfig.spektrum_sat_bind = read8();
1697         if (currentPort->dataSize > 8) {
1698             masterConfig.rxConfig.rx_min_usec = read16();
1699             masterConfig.rxConfig.rx_max_usec = read16();
1700         }
1701         break;
1702
1703     case MSP_SET_FAILSAFE_CONFIG:
1704         masterConfig.failsafeConfig.failsafe_delay = read8();
1705         masterConfig.failsafeConfig.failsafe_off_delay = read8();
1706         masterConfig.failsafeConfig.failsafe_throttle = read16();
1707         masterConfig.failsafeConfig.failsafe_kill_switch = read8();
1708         masterConfig.failsafeConfig.failsafe_throttle_low_delay = read16();
1709         masterConfig.failsafeConfig.failsafe_procedure = read8();
1710         break;
1711
1712     case MSP_SET_RXFAIL_CONFIG:
1713         i = read8();
1714         if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) {
1715             masterConfig.rxConfig.failsafe_channel_configurations[i].mode = read8();
1716             masterConfig.rxConfig.failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(read16());
1717         } else {
1718             headSerialError(0);
1719         }
1720         break;
1721
1722     case MSP_SET_RSSI_CONFIG:
1723         masterConfig.rxConfig.rssi_channel = read8();
1724         break;
1725
1726     case MSP_SET_RX_MAP:
1727         for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) {
1728             masterConfig.rxConfig.rcmap[i] = read8();
1729         }
1730         break;
1731
1732     case MSP_SET_BF_CONFIG:
1733
1734 #ifdef USE_QUAD_MIXER_ONLY
1735         read8(); // mixerMode ignored
1736 #else
1737         masterConfig.mixerMode = read8(); // mixerMode
1738 #endif
1739
1740         featureClearAll();
1741         featureSet(read32()); // features bitmap
1742
1743         masterConfig.rxConfig.serialrx_provider = read8(); // serialrx_type
1744
1745         masterConfig.boardAlignment.rollDegrees = read16(); // board_align_roll
1746         masterConfig.boardAlignment.pitchDegrees = read16(); // board_align_pitch
1747         masterConfig.boardAlignment.yawDegrees = read16(); // board_align_yaw
1748
1749         masterConfig.batteryConfig.currentMeterScale = read16();
1750         masterConfig.batteryConfig.currentMeterOffset = read16();
1751         break;
1752
1753     case MSP_SET_CF_SERIAL_CONFIG:
1754         {
1755             uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4);
1756
1757             if (currentPort->dataSize % portConfigSize != 0) {
1758                 headSerialError(0);
1759                 break;
1760             }
1761
1762             uint8_t remainingPortsInPacket = currentPort->dataSize / portConfigSize;
1763
1764             while (remainingPortsInPacket--) {
1765                 uint8_t identifier = read8();
1766
1767                 serialPortConfig_t *portConfig = serialFindPortConfiguration(identifier);
1768                 if (!portConfig) {
1769                     headSerialError(0);
1770                     break;
1771                 }
1772
1773                 portConfig->identifier = identifier;
1774                 portConfig->functionMask = read16();
1775                 portConfig->msp_baudrateIndex = read8();
1776                 portConfig->gps_baudrateIndex = read8();
1777                 portConfig->telemetry_baudrateIndex = read8();
1778                 portConfig->blackbox_baudrateIndex = read8();
1779             }
1780         }
1781         break;
1782
1783 #ifdef LED_STRIP
1784     case MSP_SET_LED_COLORS:
1785         for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) {
1786             hsvColor_t *color = &masterConfig.colors[i];
1787             color->h = read16();
1788             color->s = read8();
1789             color->v = read8();
1790         }
1791         break;
1792
1793     case MSP_SET_LED_STRIP_CONFIG:
1794         {
1795             i = read8();
1796             if (i >= MAX_LED_STRIP_LENGTH || currentPort->dataSize != (1 + 7)) {
1797                 headSerialError(0);
1798                 break;
1799             }
1800             ledConfig_t *ledConfig = &masterConfig.ledConfigs[i];
1801             uint16_t mask;
1802             // currently we're storing directions and functions in a uint16 (flags)
1803             // the msp uses 2 x uint16_t to cater for future expansion
1804             mask = read16();
1805             ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK;
1806
1807             mask = read16();
1808             ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK;
1809
1810             mask = read8();
1811             ledConfig->xy = CALCULATE_LED_X(mask);
1812
1813             mask = read8();
1814             ledConfig->xy |= CALCULATE_LED_Y(mask);
1815
1816             ledConfig->color = read8();
1817
1818             reevaluateLedConfig();
1819         }
1820         break;
1821 #endif
1822     case MSP_REBOOT:
1823         isRebootScheduled = true;
1824         break;
1825
1826 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
1827     case MSP_SET_4WAY_IF:
1828         // get channel number
1829         // switch all motor lines HI
1830         // reply the count of ESC found
1831         headSerialReply(1);
1832         serialize8(esc4wayInit());
1833         // because we do not come back after calling Process4WayInterface
1834         // proceed with a success reply first
1835         tailSerialReply();
1836         // flush the transmit buffer
1837         bufWriterFlush(writer);
1838         // wait for all data to send
1839         waitForSerialPortToFinishTransmitting(currentPort->port);
1840         // rem: App: Wait at least appx. 500 ms for BLHeli to jump into
1841         // bootloader mode before try to connect any ESC
1842         // Start to activate here
1843         esc4wayProcess(currentPort->port);
1844         // former used MSP uart is still active
1845         // proceed as usual with MSP commands
1846         break;
1847 #endif
1848
1849     case MSP_SET_PID_ADVANCED_CONFIG :
1850         masterConfig.gyro_sync_denom = read8();
1851         masterConfig.pid_process_denom = read8();
1852         masterConfig.use_unsyncedPwm = read8();
1853         masterConfig.motor_pwm_protocol = read8();
1854         masterConfig.motor_pwm_rate = read16();
1855         break;
1856     case MSP_SET_FILTER_CONFIG :
1857         masterConfig.gyro_soft_lpf_hz = read8();
1858         currentProfile->pidProfile.dterm_lpf_hz = read16();
1859         currentProfile->pidProfile.yaw_lpf_hz = read16();
1860         break;
1861     case MSP_SET_ADVANCED_TUNING:
1862         currentProfile->pidProfile.rollPitchItermIgnoreRate = read16();
1863         currentProfile->pidProfile.yawItermIgnoreRate = read16();
1864         currentProfile->pidProfile.yaw_p_limit = read16();
1865         currentProfile->pidProfile.deltaMethod = read8();
1866         masterConfig.batteryConfig.vbatPidCompensation = read8();
1867         break;
1868     case MSP_SET_SPECIAL_PARAMETERS:
1869         currentControlRateProfile->rcYawRate8 = read8();
1870         masterConfig.rxConfig.airModeActivateThreshold = read16();
1871         masterConfig.rxConfig.rcSmoothInterval = read8();
1872         masterConfig.escAndServoConfig.escDesyncProtection = read16();
1873         break;
1874     case MSP_SET_SENSOR_CONFIG:
1875         masterConfig.acc_hardware = read8();
1876         masterConfig.baro_hardware = read8();
1877         masterConfig.mag_hardware = read8();
1878         break;
1879
1880     case MSP_SET_NAME:
1881         memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name));
1882         for (i = 0; i < MIN(MAX_NAME_LENGTH, currentPort->dataSize); i++) {
1883             masterConfig.name[i] = read8();
1884         }
1885         break;
1886     default:
1887         // we do not know how to handle the (valid) message, indicate error MSP $M!
1888         return false;
1889     }
1890     headSerialReply(0);
1891     return true;
1892 }
1893
1894 STATIC_UNIT_TESTED void mspProcessReceivedCommand() {
1895     if (!(processOutCommand(currentPort->cmdMSP) || processInCommand())) {
1896         headSerialError(0);
1897     }
1898     tailSerialReply();
1899     currentPort->c_state = IDLE;
1900 }
1901
1902 static bool mspProcessReceivedData(uint8_t c)
1903 {
1904     if (currentPort->c_state == IDLE) {
1905         if (c == '$') {
1906             currentPort->c_state = HEADER_START;
1907         } else {
1908             return false;
1909         }
1910     } else if (currentPort->c_state == HEADER_START) {
1911         currentPort->c_state = (c == 'M') ? HEADER_M : IDLE;
1912     } else if (currentPort->c_state == HEADER_M) {
1913         currentPort->c_state = (c == '<') ? HEADER_ARROW : IDLE;
1914     } else if (currentPort->c_state == HEADER_ARROW) {
1915         if (c > MSP_PORT_INBUF_SIZE) {
1916             currentPort->c_state = IDLE;
1917
1918         } else {
1919             currentPort->dataSize = c;
1920             currentPort->offset = 0;
1921             currentPort->checksum = 0;
1922             currentPort->indRX = 0;
1923             currentPort->checksum ^= c;
1924             currentPort->c_state = HEADER_SIZE;
1925         }
1926     } else if (currentPort->c_state == HEADER_SIZE) {
1927         currentPort->cmdMSP = c;
1928         currentPort->checksum ^= c;
1929         currentPort->c_state = HEADER_CMD;
1930     } else if (currentPort->c_state == HEADER_CMD && currentPort->offset < currentPort->dataSize) {
1931         currentPort->checksum ^= c;
1932         currentPort->inBuf[currentPort->offset++] = c;
1933     } else if (currentPort->c_state == HEADER_CMD && currentPort->offset >= currentPort->dataSize) {
1934         if (currentPort->checksum == c) {
1935             currentPort->c_state = COMMAND_RECEIVED;
1936         } else {
1937             currentPort->c_state = IDLE;
1938         }
1939     }
1940     return true;
1941 }
1942
1943 STATIC_UNIT_TESTED void setCurrentPort(mspPort_t *port)
1944 {
1945     currentPort = port;
1946     mspSerialPort = currentPort->port;
1947 }
1948
1949 void mspProcess(void)
1950 {
1951     uint8_t portIndex;
1952     mspPort_t *candidatePort;
1953
1954     for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
1955         candidatePort = &mspPorts[portIndex];
1956         if (!candidatePort->port) {
1957             continue;
1958         }
1959
1960         setCurrentPort(candidatePort);
1961         // Big enough to fit a MSP_STATUS in one write.
1962         uint8_t buf[sizeof(bufWriter_t) + 20];
1963         writer = bufWriterInit(buf, sizeof(buf),
1964                                (bufWrite_t)serialWriteBufShim, currentPort->port);
1965
1966         while (serialRxBytesWaiting(mspSerialPort)) {
1967
1968             uint8_t c = serialRead(mspSerialPort);
1969             bool consumed = mspProcessReceivedData(c);
1970
1971             if (!consumed && !ARMING_FLAG(ARMED)) {
1972                 evaluateOtherData(mspSerialPort, c);
1973             }
1974
1975             if (currentPort->c_state == COMMAND_RECEIVED) {
1976                 mspProcessReceivedCommand();
1977                 break; // process one command at a time so as not to block.
1978             }
1979         }
1980
1981         bufWriterFlush(writer);
1982
1983         if (isRebootScheduled) {
1984             waitForSerialPortToFinishTransmitting(candidatePort->port);
1985             stopPwmAllMotors();
1986             // On real flight controllers, systemReset() will do a soft reset of the device,
1987             // reloading the program.  But to support offline testing this flag needs to be
1988             // cleared so that the software doesn't continuously attempt to reboot itself.
1989             isRebootScheduled = false;
1990             systemReset();
1991         }
1992     }
1993 }