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