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