src/main/io/serial_msp.c

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