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