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