src/main/interface/msp.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24 #include <math.h>
  25 #include <stdlib.h>
  26
  27 #include "platform.h"
  28
  29 #include "blackbox/blackbox.h"
  30
  31 #include "build/build_config.h"
  32 #include "build/debug.h"
  33 #include "build/version.h"
  34
  35 #include "common/axis.h"
  36 #include "common/bitarray.h"
  37 #include "common/color.h"
  38 #include "common/maths.h"
  39 #include "common/streambuf.h"
  40 #include "common/huffman.h"
  41
  42 #include "config/config_eeprom.h"
  43 #include "config/feature.h"
  44 #include "pg/pg.h"
  45 #include "pg/pg_ids.h"
  46 #include "pg/beeper.h"
  47 #include "pg/rx.h"
  48 #include "pg/rx_spi.h"
  49
  50 #include "drivers/accgyro/accgyro.h"
  51 #include "drivers/bus_i2c.h"
  52 #include "drivers/compass/compass.h"
  53 #include "drivers/flash.h"
  54 #include "drivers/io.h"
  55 #include "drivers/max7456.h"
  56 #include "drivers/pwm_output.h"
  57 #include "drivers/sdcard.h"
  58 #include "drivers/serial.h"
  59 #include "drivers/serial_escserial.h"
  60 #include "drivers/system.h"
  61 #include "drivers/vtx_common.h"
  62 #include "drivers/transponder_ir.h"
  63 #include "drivers/camera_control.h"
  64
  65 #include "fc/board_info.h"
  66 #include "fc/config.h"
  67 #include "fc/controlrate_profile.h"
  68 #include "fc/fc_core.h"
  69 #include "fc/fc_rc.h"
  70 #include "fc/rc_adjustments.h"
  71 #include "fc/rc_controls.h"
  72 #include "fc/rc_modes.h"
  73 #include "fc/runtime_config.h"
  74
  75 #include "flight/position.h"
  76 #include "flight/failsafe.h"
  77 #include "flight/imu.h"
  78 #include "flight/mixer.h"
  79 #include "flight/pid.h"
  80 #include "flight/servos.h"
  81
  82 #include "interface/msp.h"
  83 #include "interface/msp_box.h"
  84 #include "interface/msp_protocol.h"
  85
  86 #include "io/asyncfatfs/asyncfatfs.h"
  87 #include "io/beeper.h"
  88 #include "io/flashfs.h"
  89 #include "io/gimbal.h"
  90 #include "io/gps.h"
  91 #include "io/ledstrip.h"
  92 #include "io/motors.h"
  93 #include "io/osd.h"
  94 #include "io/osd_slave.h"
  95 #include "io/serial.h"
  96 #include "io/serial_4way.h"
  97 #include "io/servos.h"
  98 #include "io/transponder_ir.h"
  99 #include "io/vtx_control.h"
 100 #include "io/vtx.h"
 101 #include "io/vtx_string.h"
 102
 103 #include "msp/msp_serial.h"
 104
 105 #include "pg/board.h"
 106 #include "pg/vcd.h"
 107
 108 #include "rx/rx.h"
 109 #include "rx/msp.h"
 110
 111 #include "scheduler/scheduler.h"
 112
 113 #include "sensors/battery.h"
 114
 115 #include "sensors/acceleration.h"
 116 #include "sensors/barometer.h"
 117 #include "sensors/boardalignment.h"
 118 #include "sensors/esc_sensor.h"
 119 #include "sensors/compass.h"
 120 #include "sensors/gyro.h"
 121 #include "sensors/rangefinder.h"
 122 #include "sensors/sensors.h"
 123
 124 #include "telemetry/telemetry.h"
 125
 126 #ifdef USE_HARDWARE_REVISION_DETECTION
 127 #include "hardware_revision.h"
 128 #endif
 129
 130 static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
 131
 132 #ifndef USE_OSD_SLAVE
 133
 134 static const char pidnames[] =
 135     "ROLL;"
 136     "PITCH;"
 137     "YAW;"
 138     "ALT;"
 139     "Pos;"
 140     "PosR;"
 141     "NavR;"
 142     "LEVEL;"
 143     "MAG;"
 144     "VEL;";
 145
 146 typedef enum {
 147     MSP_SDCARD_STATE_NOT_PRESENT = 0,
 148     MSP_SDCARD_STATE_FATAL       = 1,
 149     MSP_SDCARD_STATE_CARD_INIT   = 2,
 150     MSP_SDCARD_STATE_FS_INIT     = 3,
 151     MSP_SDCARD_STATE_READY       = 4
 152 } mspSDCardState_e;
 153
 154 typedef enum {
 155     MSP_SDCARD_FLAG_SUPPORTTED   = 1
 156 } mspSDCardFlags_e;
 157
 158 typedef enum {
 159     MSP_FLASHFS_FLAG_READY       = 1,
 160     MSP_FLASHFS_FLAG_SUPPORTED  = 2
 161 } mspFlashFsFlags_e;
 162
 163 #define RATEPROFILE_MASK (1 << 7)
 164 #endif //USE_OSD_SLAVE
 165
 166 #define RTC_NOT_SUPPORTED 0xff
 167
 168 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
 169 #define ESC_4WAY 0xff
 170
 171 uint8_t escMode;
 172 uint8_t escPortIndex;
 173
 174 #ifdef USE_ESCSERIAL
 175 static void mspEscPassthroughFn(serialPort_t *serialPort)
 176 {
 177     escEnablePassthrough(serialPort, escPortIndex, escMode);
 178 }
 179 #endif
 180
 181 static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
 182 {
 183     const unsigned int dataSize = sbufBytesRemaining(src);
 184     if (dataSize == 0) {
 185         // Legacy format
 186
 187         escMode = ESC_4WAY;
 188     } else {
 189         escMode = sbufReadU8(src);
 190         escPortIndex = sbufReadU8(src);
 191     }
 192
 193     switch (escMode) {
 194     case ESC_4WAY:
 195         // get channel number
 196         // switch all motor lines HI
 197         // reply with the count of ESC found
 198         sbufWriteU8(dst, esc4wayInit());
 199
 200         if (mspPostProcessFn) {
 201             *mspPostProcessFn = esc4wayProcess;
 202         }
 203         break;
 204
 205 #ifdef USE_ESCSERIAL
 206     case PROTOCOL_SIMONK:
 207     case PROTOCOL_BLHELI:
 208     case PROTOCOL_KISS:
 209     case PROTOCOL_KISSALL:
 210     case PROTOCOL_CASTLE:
 211         if (escPortIndex < getMotorCount() || (escMode == PROTOCOL_KISS && escPortIndex == ALL_MOTORS)) {
 212             sbufWriteU8(dst, 1);
 213
 214             if (mspPostProcessFn) {
 215                 *mspPostProcessFn = mspEscPassthroughFn;
 216             }
 217
 218             break;
 219         }
 220         FALLTHROUGH;
 221 #endif
 222     default:
 223         sbufWriteU8(dst, 0);
 224     }
 225 }
 226 #endif //USE_SERIAL_4WAY_BLHELI_INTERFACE
 227
 228 static void mspRebootFn(serialPort_t *serialPort)
 229 {
 230     UNUSED(serialPort);
 231
 232 #ifndef USE_OSD_SLAVE
 233     stopPwmAllMotors();
 234 #endif
 235     systemReset();
 236
 237     // control should never return here.
 238     while (true) ;
 239 }
 240
 241 #ifndef USE_OSD_SLAVE
 242 static void serializeSDCardSummaryReply(sbuf_t *dst)
 243 {
 244 #ifdef USE_SDCARD
 245     uint8_t flags = MSP_SDCARD_FLAG_SUPPORTTED;
 246     uint8_t state = 0;
 247
 248     sbufWriteU8(dst, flags);
 249
 250     // Merge the card and filesystem states together
 251     if (!sdcard_isInserted()) {
 252         state = MSP_SDCARD_STATE_NOT_PRESENT;
 253     } else if (!sdcard_isFunctional()) {
 254         state = MSP_SDCARD_STATE_FATAL;
 255     } else {
 256         switch (afatfs_getFilesystemState()) {
 257         case AFATFS_FILESYSTEM_STATE_READY:
 258             state = MSP_SDCARD_STATE_READY;
 259             break;
 260
 261         case AFATFS_FILESYSTEM_STATE_INITIALIZATION:
 262             if (sdcard_isInitialized()) {
 263                 state = MSP_SDCARD_STATE_FS_INIT;
 264             } else {
 265                 state = MSP_SDCARD_STATE_CARD_INIT;
 266             }
 267             break;
 268
 269         case AFATFS_FILESYSTEM_STATE_FATAL:
 270         case AFATFS_FILESYSTEM_STATE_UNKNOWN:
 271         default:
 272             state = MSP_SDCARD_STATE_FATAL;
 273             break;
 274         }
 275     }
 276
 277     sbufWriteU8(dst, state);
 278     sbufWriteU8(dst, afatfs_getLastError());
 279     // Write free space and total space in kilobytes
 280     sbufWriteU32(dst, afatfs_getContiguousFreeSpace() / 1024);
 281     sbufWriteU32(dst, sdcard_getMetadata()->numBlocks / 2); // Block size is half a kilobyte
 282 #else
 283     sbufWriteU8(dst, 0);
 284     sbufWriteU8(dst, 0);
 285     sbufWriteU8(dst, 0);
 286     sbufWriteU32(dst, 0);
 287     sbufWriteU32(dst, 0);
 288 #endif
 289 }
 290
 291 static void serializeDataflashSummaryReply(sbuf_t *dst)
 292 {
 293 #ifdef USE_FLASHFS
 294     if (flashfsIsSupported()) {
 295         uint8_t flags = MSP_FLASHFS_FLAG_SUPPORTED;
 296         flags |= (flashfsIsReady() ? MSP_FLASHFS_FLAG_READY : 0);
 297         const flashGeometry_t *geometry = flashfsGetGeometry();
 298         sbufWriteU8(dst, flags);
 299         sbufWriteU32(dst, geometry->sectors);
 300         sbufWriteU32(dst, geometry->totalSize);
 301         sbufWriteU32(dst, flashfsGetOffset()); // Effectively the current number of bytes stored on the volume
 302     } else
 303 #endif
 304
 305     // FlashFS is not configured or valid device is not detected
 306     {
 307         sbufWriteU8(dst, 0);
 308         sbufWriteU32(dst, 0);
 309         sbufWriteU32(dst, 0);
 310         sbufWriteU32(dst, 0);
 311     }
 312 }
 313
 314 #ifdef USE_FLASHFS
 315 enum compressionType_e {
 316     NO_COMPRESSION,
 317     HUFFMAN
 318 };
 319
 320 static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uint16_t size, bool useLegacyFormat, bool allowCompression)
 321 {
 322     BUILD_BUG_ON(MSP_PORT_DATAFLASH_INFO_SIZE < 16);
 323
 324     uint16_t readLen = size;
 325     const int bytesRemainingInBuf = sbufBytesRemaining(dst) - MSP_PORT_DATAFLASH_INFO_SIZE;
 326     if (readLen > bytesRemainingInBuf) {
 327         readLen = bytesRemainingInBuf;
 328     }
 329     // size will be lower than that requested if we reach end of volume
 330     const uint32_t flashfsSize = flashfsGetSize();
 331     if (readLen > flashfsSize - address) {
 332         // truncate the request
 333         readLen = flashfsSize - address;
 334     }
 335     sbufWriteU32(dst, address);
 336
 337     // legacy format does not support compression
 338 #ifdef USE_HUFFMAN
 339     const uint8_t compressionMethod = (!allowCompression || useLegacyFormat) ? NO_COMPRESSION : HUFFMAN;
 340 #else
 341     const uint8_t compressionMethod = NO_COMPRESSION;
 342     UNUSED(allowCompression);
 343 #endif
 344
 345     if (compressionMethod == NO_COMPRESSION) {
 346         if (!useLegacyFormat) {
 347             // new format supports variable read lengths
 348             sbufWriteU16(dst, readLen);
 349             sbufWriteU8(dst, 0); // placeholder for compression format
 350         }
 351
 352         const int bytesRead = flashfsReadAbs(address, sbufPtr(dst), readLen);
 353
 354         sbufAdvance(dst, bytesRead);
 355
 356         if (useLegacyFormat) {
 357             // pad the buffer with zeros
 358             for (int i = bytesRead; i < size; i++) {
 359                 sbufWriteU8(dst, 0);
 360             }
 361         }
 362     } else {
 363 #ifdef USE_HUFFMAN
 364         // compress in 256-byte chunks
 365         const uint16_t READ_BUFFER_SIZE = 256;
 366         uint8_t readBuffer[READ_BUFFER_SIZE];
 367
 368         huffmanState_t state = {
 369             .bytesWritten = 0,
 370             .outByte = sbufPtr(dst) + sizeof(uint16_t) + sizeof(uint8_t) + HUFFMAN_INFO_SIZE,
 371             .outBufLen = readLen,
 372             .outBit = 0x80,
 373         };
 374         *state.outByte = 0;
 375
 376         uint16_t bytesReadTotal = 0;
 377         // read until output buffer overflows or flash is exhausted
 378         while (state.bytesWritten < state.outBufLen && address + bytesReadTotal < flashfsSize) {
 379             const int bytesRead = flashfsReadAbs(address + bytesReadTotal, readBuffer,
 380                 MIN(sizeof(readBuffer), flashfsSize - address - bytesReadTotal));
 381
 382             const int status = huffmanEncodeBufStreaming(&state, readBuffer, bytesRead, huffmanTable);
 383             if (status == -1) {
 384                 // overflow
 385                 break;
 386             }
 387
 388             bytesReadTotal += bytesRead;
 389         }
 390
 391         if (state.outBit != 0x80) {
 392             ++state.bytesWritten;
 393         }
 394
 395         // header
 396         sbufWriteU16(dst, HUFFMAN_INFO_SIZE + state.bytesWritten);
 397         sbufWriteU8(dst, compressionMethod);
 398         // payload
 399         sbufWriteU16(dst, bytesReadTotal);
 400         sbufAdvance(dst, state.bytesWritten);
 401 #endif
 402     }
 403 }
 404 #endif // USE_FLASHFS
 405 #endif // USE_OSD_SLAVE
 406
 407 /*
 408  * Returns true if the command was processd, false otherwise.
 409  * May set mspPostProcessFunc to a function to be called once the command has been processed
 410  */
 411 static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
 412 {
 413     switch (cmdMSP) {
 414     case MSP_API_VERSION:
 415         sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
 416         sbufWriteU8(dst, API_VERSION_MAJOR);
 417         sbufWriteU8(dst, API_VERSION_MINOR);
 418         break;
 419
 420     case MSP_FC_VARIANT:
 421         sbufWriteData(dst, flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
 422         break;
 423
 424     case MSP_FC_VERSION:
 425         sbufWriteU8(dst, FC_VERSION_MAJOR);
 426         sbufWriteU8(dst, FC_VERSION_MINOR);
 427         sbufWriteU8(dst, FC_VERSION_PATCH_LEVEL);
 428         break;
 429
 430     case MSP_BOARD_INFO:
 431     {
 432         sbufWriteData(dst, systemConfig()->boardIdentifier, BOARD_IDENTIFIER_LENGTH);
 433 #ifdef USE_HARDWARE_REVISION_DETECTION
 434         sbufWriteU16(dst, hardwareRevision);
 435 #else
 436         sbufWriteU16(dst, 0); // No other build targets currently have hardware revision detection.
 437 #endif
 438 #ifdef USE_OSD_SLAVE
 439         sbufWriteU8(dst, 1);  // 1 == OSD
 440 #else
 441 #if defined(USE_OSD) && defined(USE_MAX7456)
 442         sbufWriteU8(dst, 2);  // 2 == FC with OSD
 443 #else
 444         sbufWriteU8(dst, 0);  // 0 == FC
 445 #endif
 446 #endif
 447         // Board communication capabilities (uint8)
 448         // Bit 0: 1 iff the board has VCP
 449         // Bit 1: 1 iff the board supports software serial
 450         uint8_t commCapabilities = 0;
 451 #ifdef USE_VCP
 452         commCapabilities |= 1 << 0;
 453 #endif
 454 #if defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)
 455         commCapabilities |= 1 << 1;
 456 #endif
 457         sbufWriteU8(dst, commCapabilities);
 458
 459         // Target name with explicit length
 460         sbufWriteU8(dst, strlen(targetName));
 461         sbufWriteData(dst, targetName, strlen(targetName));
 462
 463 #if defined(USE_BOARD_INFO)
 464         // Board name with explicit length
 465         char *value = getBoardName();
 466         sbufWriteU8(dst, strlen(value));
 467         sbufWriteData(dst, value, strlen(value));
 468
 469         // Manufacturer id with explicit length
 470         value = getManufacturerId();
 471         sbufWriteU8(dst, strlen(value));
 472         sbufWriteData(dst, value, strlen(value));
 473
 474 #if defined(USE_SIGNATURE)
 475         // Signature
 476         sbufWriteData(dst, getSignature(), SIGNATURE_LENGTH);
 477 #endif
 478 #endif // USE_BOARD_INFO
 479
 480         break;
 481     }
 482
 483     case MSP_BUILD_INFO:
 484         sbufWriteData(dst, buildDate, BUILD_DATE_LENGTH);
 485         sbufWriteData(dst, buildTime, BUILD_TIME_LENGTH);
 486         sbufWriteData(dst, shortGitRevision, GIT_SHORT_REVISION_LENGTH);
 487         break;
 488
 489     case MSP_REBOOT:
 490         if (mspPostProcessFn) {
 491             *mspPostProcessFn = mspRebootFn;
 492         }
 493         break;
 494
 495     case MSP_ANALOG:
 496         sbufWriteU8(dst, (uint8_t)constrain(getBatteryVoltage(), 0, 255));
 497         sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
 498 #ifdef USE_OSD_SLAVE
 499         sbufWriteU16(dst, 0); // rssi
 500 #else
 501         sbufWriteU16(dst, getRssi());
 502 #endif
 503         sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send current in 0.01 A steps, range is -320A to 320A
 504         break;
 505
 506     case MSP_DEBUG:
 507         // output some useful QA statistics
 508         // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000);         // XX0YY [crystal clock : core clock]
 509
 510         for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) {
 511             sbufWriteU16(dst, debug[i]);      // 4 variables are here for general monitoring purpose
 512         }
 513         break;
 514
 515     case MSP_UID:
 516         sbufWriteU32(dst, U_ID_0);
 517         sbufWriteU32(dst, U_ID_1);
 518         sbufWriteU32(dst, U_ID_2);
 519         break;
 520
 521     case MSP_FEATURE_CONFIG:
 522         sbufWriteU32(dst, featureMask());
 523         break;
 524
 525 #ifdef USE_BEEPER
 526     case MSP_BEEPER_CONFIG:
 527         sbufWriteU32(dst, getBeeperOffMask());
 528         sbufWriteU8(dst, beeperConfig()->dshotBeaconTone);
 529         break;
 530 #endif
 531
 532     case MSP_BATTERY_STATE: {
 533         // battery characteristics
 534         sbufWriteU8(dst, (uint8_t)constrain(getBatteryCellCount(), 0, 255)); // 0 indicates battery not detected.
 535         sbufWriteU16(dst, batteryConfig()->batteryCapacity); // in mAh
 536
 537         // battery state
 538         sbufWriteU8(dst, (uint8_t)constrain(getBatteryVoltage(), 0, 255)); // in 0.1V steps
 539         sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
 540         sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send current in 0.01 A steps, range is -320A to 320A
 541
 542         // battery alerts
 543         sbufWriteU8(dst, (uint8_t)getBatteryState());
 544         break;
 545     }
 546
 547     case MSP_VOLTAGE_METERS: {
 548         // write out id and voltage meter values, once for each meter we support
 549         uint8_t count = supportedVoltageMeterCount;
 550 #ifdef USE_ESC_SENSOR
 551         count -= VOLTAGE_METER_ID_ESC_COUNT - getMotorCount();
 552 #endif
 553
 554         for (int i = 0; i < count; i++) {
 555
 556             voltageMeter_t meter;
 557             uint8_t id = (uint8_t)voltageMeterIds[i];
 558             voltageMeterRead(id, &meter);
 559
 560             sbufWriteU8(dst, id);
 561             sbufWriteU8(dst, (uint8_t)constrain(meter.filtered, 0, 255));
 562         }
 563         break;
 564     }
 565
 566     case MSP_CURRENT_METERS: {
 567         // write out id and current meter values, once for each meter we support
 568         uint8_t count = supportedCurrentMeterCount;
 569 #ifdef USE_ESC_SENSOR
 570         count -= VOLTAGE_METER_ID_ESC_COUNT - getMotorCount();
 571 #endif
 572         for (int i = 0; i < count; i++) {
 573
 574             currentMeter_t meter;
 575             uint8_t id = (uint8_t)currentMeterIds[i];
 576             currentMeterRead(id, &meter);
 577
 578             sbufWriteU8(dst, id);
 579             sbufWriteU16(dst, (uint16_t)constrain(meter.mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
 580             sbufWriteU16(dst, (uint16_t)constrain(meter.amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps (mA). Negative range is truncated to zero
 581         }
 582         break;
 583     }
 584
 585     case MSP_VOLTAGE_METER_CONFIG:
 586         // by using a sensor type and a sub-frame length it's possible to configure any type of voltage meter,
 587         // e.g. an i2c/spi/can sensor or any sensor not built directly into the FC such as ESC/RX/SPort/SBus that has
 588         // different configuration requirements.
 589         BUILD_BUG_ON(VOLTAGE_SENSOR_ADC_VBAT != 0); // VOLTAGE_SENSOR_ADC_VBAT should be the first index,
 590         sbufWriteU8(dst, MAX_VOLTAGE_SENSOR_ADC); // voltage meters in payload
 591         for (int i = VOLTAGE_SENSOR_ADC_VBAT; i < MAX_VOLTAGE_SENSOR_ADC; i++) {
 592             const uint8_t adcSensorSubframeLength = 1 + 1 + 1 + 1 + 1; // length of id, type, vbatscale, vbatresdivval, vbatresdivmultipler, in bytes
 593             sbufWriteU8(dst, adcSensorSubframeLength); // ADC sensor sub-frame length
 594
 595             sbufWriteU8(dst, voltageMeterADCtoIDMap[i]); // id of the sensor
 596             sbufWriteU8(dst, VOLTAGE_SENSOR_TYPE_ADC_RESISTOR_DIVIDER); // indicate the type of sensor that the next part of the payload is for
 597
 598             sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatscale);
 599             sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivval);
 600             sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivmultiplier);
 601         }
 602         // if we had any other voltage sensors, this is where we would output any needed configuration
 603         break;
 604
 605     case MSP_CURRENT_METER_CONFIG: {
 606         // the ADC and VIRTUAL sensors have the same configuration requirements, however this API reflects
 607         // that this situation may change and allows us to support configuration of any current sensor with
 608         // specialist configuration requirements.
 609
 610         int currentMeterCount = 1;
 611
 612 #ifdef USE_VIRTUAL_CURRENT_METER
 613         currentMeterCount++;
 614 #endif
 615         sbufWriteU8(dst, currentMeterCount);
 616
 617         const uint8_t adcSensorSubframeLength = 1 + 1 + 2 + 2; // length of id, type, scale, offset, in bytes
 618         sbufWriteU8(dst, adcSensorSubframeLength);
 619         sbufWriteU8(dst, CURRENT_METER_ID_BATTERY_1); // the id of the meter
 620         sbufWriteU8(dst, CURRENT_SENSOR_ADC); // indicate the type of sensor that the next part of the payload is for
 621         sbufWriteU16(dst, currentSensorADCConfig()->scale);
 622         sbufWriteU16(dst, currentSensorADCConfig()->offset);
 623
 624 #ifdef USE_VIRTUAL_CURRENT_METER
 625         const int8_t virtualSensorSubframeLength = 1 + 1 + 2 + 2; // length of id, type, scale, offset, in bytes
 626         sbufWriteU8(dst, virtualSensorSubframeLength);
 627         sbufWriteU8(dst, CURRENT_METER_ID_VIRTUAL_1); // the id of the meter
 628         sbufWriteU8(dst, CURRENT_SENSOR_VIRTUAL); // indicate the type of sensor that the next part of the payload is for
 629         sbufWriteU16(dst, currentSensorVirtualConfig()->scale);
 630         sbufWriteU16(dst, currentSensorVirtualConfig()->offset);
 631 #endif
 632
 633         // if we had any other current sensors, this is where we would output any needed configuration
 634         break;
 635     }
 636
 637     case MSP_BATTERY_CONFIG:
 638         sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage);
 639         sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage);
 640         sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage);
 641         sbufWriteU16(dst, batteryConfig()->batteryCapacity);
 642         sbufWriteU8(dst, batteryConfig()->voltageMeterSource);
 643         sbufWriteU8(dst, batteryConfig()->currentMeterSource);
 644         break;
 645
 646     case MSP_TRANSPONDER_CONFIG: {
 647 #ifdef USE_TRANSPONDER
 648         // Backward compatibility to BFC 3.1.1 is lost for this message type
 649         sbufWriteU8(dst, TRANSPONDER_PROVIDER_COUNT);
 650         for (unsigned int i = 0; i < TRANSPONDER_PROVIDER_COUNT; i++) {
 651             sbufWriteU8(dst, transponderRequirements[i].provider);
 652             sbufWriteU8(dst, transponderRequirements[i].dataLength);
 653         }
 654
 655         uint8_t provider = transponderConfig()->provider;
 656         sbufWriteU8(dst, provider);
 657
 658         if (provider) {
 659             uint8_t requirementIndex = provider - 1;
 660             uint8_t providerDataLength = transponderRequirements[requirementIndex].dataLength;
 661
 662             for (unsigned int i = 0; i < providerDataLength; i++) {
 663                 sbufWriteU8(dst, transponderConfig()->data[i]);
 664             }
 665         }
 666 #else
 667         sbufWriteU8(dst, 0); // no providers
 668 #endif
 669         break;
 670     }
 671
 672     case MSP_OSD_CONFIG: {
 673 #define OSD_FLAGS_OSD_FEATURE           (1 << 0)
 674 #define OSD_FLAGS_OSD_SLAVE             (1 << 1)
 675 #define OSD_FLAGS_RESERVED_1            (1 << 2)
 676 #define OSD_FLAGS_RESERVED_2            (1 << 3)
 677 #define OSD_FLAGS_OSD_HARDWARE_MAX_7456 (1 << 4)
 678
 679         uint8_t osdFlags = 0;
 680 #if defined(USE_OSD)
 681         osdFlags |= OSD_FLAGS_OSD_FEATURE;
 682 #endif
 683 #if defined(USE_OSD_SLAVE)
 684         osdFlags |= OSD_FLAGS_OSD_SLAVE;
 685 #endif
 686 #ifdef USE_MAX7456
 687         osdFlags |= OSD_FLAGS_OSD_HARDWARE_MAX_7456;
 688 #endif
 689
 690         sbufWriteU8(dst, osdFlags);
 691
 692 #ifdef USE_MAX7456
 693         // send video system (AUTO/PAL/NTSC)
 694         sbufWriteU8(dst, vcdProfile()->video_system);
 695 #else
 696         sbufWriteU8(dst, 0);
 697 #endif
 698
 699 #ifdef USE_OSD
 700         // OSD specific, not applicable to OSD slaves.
 701
 702         // Configuration
 703         sbufWriteU8(dst, osdConfig()->units);
 704
 705         // Alarms
 706         sbufWriteU8(dst, osdConfig()->rssi_alarm);
 707         sbufWriteU16(dst, osdConfig()->cap_alarm);
 708
 709         // Reuse old timer alarm (U16) as OSD_ITEM_COUNT
 710         sbufWriteU8(dst, 0);
 711         sbufWriteU8(dst, OSD_ITEM_COUNT);
 712
 713         sbufWriteU16(dst, osdConfig()->alt_alarm);
 714
 715         // Element position and visibility
 716         for (int i = 0; i < OSD_ITEM_COUNT; i++) {
 717             sbufWriteU16(dst, osdConfig()->item_pos[i]);
 718         }
 719
 720         // Post flight statistics
 721         sbufWriteU8(dst, OSD_STAT_COUNT);
 722         for (int i = 0; i < OSD_STAT_COUNT; i++ ) {
 723             sbufWriteU8(dst, osdStatGetState(i));
 724         }
 725
 726         // Timers
 727         sbufWriteU8(dst, OSD_TIMER_COUNT);
 728         for (int i = 0; i < OSD_TIMER_COUNT; i++) {
 729             sbufWriteU16(dst, osdConfig()->timers[i]);
 730         }
 731
 732         // Enabled warnings
 733         sbufWriteU16(dst, osdConfig()->enabledWarnings);
 734 #endif
 735         break;
 736     }
 737
 738     default:
 739         return false;
 740     }
 741     return true;
 742 }
 743
 744 #ifdef USE_OSD_SLAVE
 745 static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
 746 {
 747     switch (cmdMSP) {
 748     case MSP_STATUS_EX:
 749     case MSP_STATUS:
 750         sbufWriteU16(dst, getTaskDeltaTime(TASK_SERIAL));
 751 #ifdef USE_I2C
 752         sbufWriteU16(dst, i2cGetErrorCounter());
 753 #else
 754         sbufWriteU16(dst, 0);
 755 #endif
 756         sbufWriteU16(dst, 0); // sensors
 757         sbufWriteU32(dst, 0); // flight modes
 758         sbufWriteU8(dst, 0); // profile
 759         sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
 760         if (cmdMSP == MSP_STATUS_EX) {
 761             sbufWriteU8(dst, 1); // max profiles
 762             sbufWriteU8(dst, 0); // control rate profile
 763         } else {
 764             sbufWriteU16(dst, 0); // gyro cycle time
 765         }
 766         break;
 767
 768     default:
 769         return false;
 770     }
 771     return true;
 772 }
 773
 774 #else
 775
 776 static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
 777 {
 778     bool unsupportedCommand = false;
 779
 780     switch (cmdMSP) {
 781     case MSP_STATUS_EX:
 782     case MSP_STATUS:
 783         {
 784             boxBitmask_t flightModeFlags;
 785             const int flagBits = packFlightModeFlags(&flightModeFlags);
 786
 787             sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID));
 788 #ifdef USE_I2C
 789             sbufWriteU16(dst, i2cGetErrorCounter());
 790 #else
 791             sbufWriteU16(dst, 0);
 792 #endif
 793             sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_RANGEFINDER) << 4 | sensors(SENSOR_GYRO) << 5);
 794             sbufWriteData(dst, &flightModeFlags, 4);        // unconditional part of flags, first 32 bits
 795             sbufWriteU8(dst, getCurrentPidProfileIndex());
 796             sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
 797             if (cmdMSP == MSP_STATUS_EX) {
 798                 sbufWriteU8(dst, MAX_PROFILE_COUNT);
 799                 sbufWriteU8(dst, getCurrentControlRateProfileIndex());
 800             } else {  // MSP_STATUS
 801                 sbufWriteU16(dst, 0); // gyro cycle time
 802             }
 803
 804             // write flightModeFlags header. Lowest 4 bits contain number of bytes that follow
 805             // header is emited even when all bits fit into 32 bits to allow future extension
 806             int byteCount = (flagBits - 32 + 7) / 8;        // 32 already stored, round up
 807             byteCount = constrain(byteCount, 0, 15);        // limit to 16 bytes (128 bits)
 808             sbufWriteU8(dst, byteCount);
 809             sbufWriteData(dst, ((uint8_t*)&flightModeFlags) + 4, byteCount);
 810
 811             // Write arming disable flags
 812             // 1 byte, flag count
 813             sbufWriteU8(dst, ARMING_DISABLE_FLAGS_COUNT);
 814             // 4 bytes, flags
 815             const uint32_t armingDisableFlags = getArmingDisableFlags();
 816             sbufWriteU32(dst, armingDisableFlags);
 817         }
 818         break;
 819
 820     case MSP_RAW_IMU:
 821         {
 822             // Hack scale due to choice of units for sensor data in multiwii
 823
 824             uint8_t scale;
 825
 826             if (acc.dev.acc_1G > 512*4) {
 827                 scale = 8;
 828             } else if (acc.dev.acc_1G > 512*2) {
 829                 scale = 4;
 830             } else if (acc.dev.acc_1G >= 512) {
 831                 scale = 2;
 832             } else {
 833                 scale = 1;
 834             }
 835
 836             for (int i = 0; i < 3; i++) {
 837                 sbufWriteU16(dst, lrintf(acc.accADC[i] / scale));
 838             }
 839             for (int i = 0; i < 3; i++) {
 840                 sbufWriteU16(dst, gyroRateDps(i));
 841             }
 842             for (int i = 0; i < 3; i++) {
 843                 sbufWriteU16(dst, lrintf(mag.magADC[i]));
 844             }
 845         }
 846         break;
 847
 848     case MSP_NAME:
 849         {
 850             const int nameLen = strlen(pilotConfig()->name);
 851             for (int i = 0; i < nameLen; i++) {
 852                 sbufWriteU8(dst, pilotConfig()->name[i]);
 853             }
 854         }
 855         break;
 856
 857 #ifdef USE_SERVOS
 858     case MSP_SERVO:
 859         sbufWriteData(dst, &servo, MAX_SUPPORTED_SERVOS * 2);
 860         break;
 861     case MSP_SERVO_CONFIGURATIONS:
 862         for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
 863             sbufWriteU16(dst, servoParams(i)->min);
 864             sbufWriteU16(dst, servoParams(i)->max);
 865             sbufWriteU16(dst, servoParams(i)->middle);
 866             sbufWriteU8(dst, servoParams(i)->rate);
 867             sbufWriteU8(dst, servoParams(i)->forwardFromChannel);
 868             sbufWriteU32(dst, servoParams(i)->reversedSources);
 869         }
 870         break;
 871
 872     case MSP_SERVO_MIX_RULES:
 873         for (int i = 0; i < MAX_SERVO_RULES; i++) {
 874             sbufWriteU8(dst, customServoMixers(i)->targetChannel);
 875             sbufWriteU8(dst, customServoMixers(i)->inputSource);
 876             sbufWriteU8(dst, customServoMixers(i)->rate);
 877             sbufWriteU8(dst, customServoMixers(i)->speed);
 878             sbufWriteU8(dst, customServoMixers(i)->min);
 879             sbufWriteU8(dst, customServoMixers(i)->max);
 880             sbufWriteU8(dst, customServoMixers(i)->box);
 881         }
 882         break;
 883 #endif
 884
 885     case MSP_MOTOR:
 886         for (unsigned i = 0; i < 8; i++) {
 887             if (i >= MAX_SUPPORTED_MOTORS || !pwmGetMotors()[i].enabled) {
 888                 sbufWriteU16(dst, 0);
 889                 continue;
 890             }
 891
 892             sbufWriteU16(dst, convertMotorToExternal(motor[i]));
 893         }
 894         break;
 895
 896     case MSP_RC:
 897         for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
 898             sbufWriteU16(dst, rcData[i]);
 899         }
 900         break;
 901
 902     case MSP_ATTITUDE:
 903         sbufWriteU16(dst, attitude.values.roll);
 904         sbufWriteU16(dst, attitude.values.pitch);
 905         sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
 906         break;
 907
 908     case MSP_ALTITUDE:
 909 #if defined(USE_BARO) || defined(USE_RANGEFINDER)
 910         sbufWriteU32(dst, getEstimatedAltitude());
 911 #else
 912         sbufWriteU32(dst, 0);
 913 #endif
 914         sbufWriteU16(dst, getEstimatedVario());
 915         break;
 916
 917     case MSP_SONAR_ALTITUDE:
 918 #if defined(USE_RANGEFINDER)
 919         sbufWriteU32(dst, rangefinderGetLatestAltitude());
 920 #else
 921         sbufWriteU32(dst, 0);
 922 #endif
 923         break;
 924
 925     case MSP_BOARD_ALIGNMENT_CONFIG:
 926         sbufWriteU16(dst, boardAlignment()->rollDegrees);
 927         sbufWriteU16(dst, boardAlignment()->pitchDegrees);
 928         sbufWriteU16(dst, boardAlignment()->yawDegrees);
 929         break;
 930
 931     case MSP_ARMING_CONFIG:
 932         sbufWriteU8(dst, armingConfig()->auto_disarm_delay);
 933         sbufWriteU8(dst, 0);
 934         sbufWriteU8(dst, imuConfig()->small_angle);
 935         break;
 936
 937     case MSP_RC_TUNING:
 938         sbufWriteU8(dst, currentControlRateProfile->rcRates[FD_ROLL]);
 939         sbufWriteU8(dst, currentControlRateProfile->rcExpo[FD_ROLL]);
 940         for (int i = 0 ; i < 3; i++) {
 941             sbufWriteU8(dst, currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
 942         }
 943         sbufWriteU8(dst, currentControlRateProfile->dynThrPID);
 944         sbufWriteU8(dst, currentControlRateProfile->thrMid8);
 945         sbufWriteU8(dst, currentControlRateProfile->thrExpo8);
 946         sbufWriteU16(dst, currentControlRateProfile->tpa_breakpoint);
 947         sbufWriteU8(dst, currentControlRateProfile->rcExpo[FD_YAW]);
 948         sbufWriteU8(dst, currentControlRateProfile->rcRates[FD_YAW]);
 949         sbufWriteU8(dst, currentControlRateProfile->rcRates[FD_PITCH]);
 950         sbufWriteU8(dst, currentControlRateProfile->rcExpo[FD_PITCH]);
 951         break;
 952
 953     case MSP_PID:
 954         for (int i = 0; i < PID_ITEM_COUNT; i++) {
 955             sbufWriteU8(dst, currentPidProfile->pid[i].P);
 956             sbufWriteU8(dst, currentPidProfile->pid[i].I);
 957             sbufWriteU8(dst, currentPidProfile->pid[i].D);
 958         }
 959         break;
 960
 961     case MSP_PIDNAMES:
 962         for (const char *c = pidnames; *c; c++) {
 963             sbufWriteU8(dst, *c);
 964         }
 965         break;
 966
 967     case MSP_PID_CONTROLLER:
 968         sbufWriteU8(dst, PID_CONTROLLER_BETAFLIGHT);
 969         break;
 970
 971     case MSP_MODE_RANGES:
 972         for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
 973             const modeActivationCondition_t *mac = modeActivationConditions(i);
 974             const box_t *box = findBoxByBoxId(mac->modeId);
 975             sbufWriteU8(dst, box->permanentId);
 976             sbufWriteU8(dst, mac->auxChannelIndex);
 977             sbufWriteU8(dst, mac->range.startStep);
 978             sbufWriteU8(dst, mac->range.endStep);
 979         }
 980         break;
 981
 982     case MSP_ADJUSTMENT_RANGES:
 983         for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
 984             const adjustmentRange_t *adjRange = adjustmentRanges(i);
 985             sbufWriteU8(dst, adjRange->adjustmentIndex);
 986             sbufWriteU8(dst, adjRange->auxChannelIndex);
 987             sbufWriteU8(dst, adjRange->range.startStep);
 988             sbufWriteU8(dst, adjRange->range.endStep);
 989             sbufWriteU8(dst, adjRange->adjustmentFunction);
 990             sbufWriteU8(dst, adjRange->auxSwitchChannelIndex);
 991         }
 992         break;
 993
 994     case MSP_MOTOR_CONFIG:
 995         sbufWriteU16(dst, motorConfig()->minthrottle);
 996         sbufWriteU16(dst, motorConfig()->maxthrottle);
 997         sbufWriteU16(dst, motorConfig()->mincommand);
 998         break;
 999
1000 #ifdef USE_MAG
1001     case MSP_COMPASS_CONFIG:
1002         sbufWriteU16(dst, compassConfig()->mag_declination / 10);
1003         break;
1004 #endif
1005
1006 #if defined(USE_ESC_SENSOR)
1007     case MSP_ESC_SENSOR_DATA:
1008         if (feature(FEATURE_ESC_SENSOR)) {
1009             sbufWriteU8(dst, getMotorCount());
1010             for (int i = 0; i < getMotorCount(); i++) {
1011                 const escSensorData_t *escData = getEscSensorData(i);
1012                 sbufWriteU8(dst, escData->temperature);
1013                 sbufWriteU16(dst, escData->rpm);
1014             }
1015         } else {
1016             unsupportedCommand = true;
1017         }
1018
1019         break;
1020 #endif
1021
1022 #ifdef USE_GPS
1023     case MSP_GPS_CONFIG:
1024         sbufWriteU8(dst, gpsConfig()->provider);
1025         sbufWriteU8(dst, gpsConfig()->sbasMode);
1026         sbufWriteU8(dst, gpsConfig()->autoConfig);
1027         sbufWriteU8(dst, gpsConfig()->autoBaud);
1028         break;
1029
1030     case MSP_RAW_GPS:
1031         sbufWriteU8(dst, STATE(GPS_FIX));
1032         sbufWriteU8(dst, gpsSol.numSat);
1033         sbufWriteU32(dst, gpsSol.llh.lat);
1034         sbufWriteU32(dst, gpsSol.llh.lon);
1035         sbufWriteU16(dst, gpsSol.llh.alt);
1036         sbufWriteU16(dst, gpsSol.groundSpeed);
1037         sbufWriteU16(dst, gpsSol.groundCourse);
1038         break;
1039
1040     case MSP_COMP_GPS:
1041         sbufWriteU16(dst, GPS_distanceToHome);
1042         sbufWriteU16(dst, GPS_directionToHome);
1043         sbufWriteU8(dst, GPS_update & 1);
1044         break;
1045
1046     case MSP_GPSSVINFO:
1047         sbufWriteU8(dst, GPS_numCh);
1048        for (int i = 0; i < GPS_numCh; i++) {
1049            sbufWriteU8(dst, GPS_svinfo_chn[i]);
1050            sbufWriteU8(dst, GPS_svinfo_svid[i]);
1051            sbufWriteU8(dst, GPS_svinfo_quality[i]);
1052            sbufWriteU8(dst, GPS_svinfo_cno[i]);
1053        }
1054         break;
1055 #endif
1056
1057     case MSP_ACC_TRIM:
1058         sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.pitch);
1059         sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.roll);
1060         break;
1061
1062     case MSP_MIXER_CONFIG:
1063         sbufWriteU8(dst, mixerConfig()->mixerMode);
1064         sbufWriteU8(dst, mixerConfig()->yaw_motors_reversed);
1065         break;
1066
1067     case MSP_RX_CONFIG:
1068         sbufWriteU8(dst, rxConfig()->serialrx_provider);
1069         sbufWriteU16(dst, rxConfig()->maxcheck);
1070         sbufWriteU16(dst, rxConfig()->midrc);
1071         sbufWriteU16(dst, rxConfig()->mincheck);
1072         sbufWriteU8(dst, rxConfig()->spektrum_sat_bind);
1073         sbufWriteU16(dst, rxConfig()->rx_min_usec);
1074         sbufWriteU16(dst, rxConfig()->rx_max_usec);
1075         sbufWriteU8(dst, rxConfig()->rcInterpolation);
1076         sbufWriteU8(dst, rxConfig()->rcInterpolationInterval);
1077         sbufWriteU16(dst, rxConfig()->airModeActivateThreshold * 10 + 1000);
1078 #ifdef USE_RX_SPI
1079         sbufWriteU8(dst, rxSpiConfig()->rx_spi_protocol);
1080         sbufWriteU32(dst, rxSpiConfig()->rx_spi_id);
1081         sbufWriteU8(dst, rxSpiConfig()->rx_spi_rf_channel_count);
1082 #else
1083         sbufWriteU8(dst, 0);
1084         sbufWriteU32(dst, 0);
1085         sbufWriteU8(dst, 0);
1086 #endif
1087         sbufWriteU8(dst, rxConfig()->fpvCamAngleDegrees);
1088         break;
1089
1090     case MSP_FAILSAFE_CONFIG:
1091         sbufWriteU8(dst, failsafeConfig()->failsafe_delay);
1092         sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay);
1093         sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
1094         sbufWriteU8(dst, failsafeConfig()->failsafe_switch_mode);
1095         sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay);
1096         sbufWriteU8(dst, failsafeConfig()->failsafe_procedure);
1097         break;
1098
1099     case MSP_RXFAIL_CONFIG:
1100         for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
1101             sbufWriteU8(dst, rxFailsafeChannelConfigs(i)->mode);
1102             sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxFailsafeChannelConfigs(i)->step));
1103         }
1104         break;
1105
1106     case MSP_RSSI_CONFIG:
1107         sbufWriteU8(dst, rxConfig()->rssi_channel);
1108         break;
1109
1110     case MSP_RX_MAP:
1111         sbufWriteData(dst, rxConfig()->rcmap, RX_MAPPABLE_CHANNEL_COUNT);
1112         break;
1113
1114     case MSP_BF_CONFIG:
1115         sbufWriteU8(dst, mixerConfig()->mixerMode);
1116
1117         sbufWriteU32(dst, featureMask());
1118
1119         sbufWriteU8(dst, rxConfig()->serialrx_provider);
1120
1121         sbufWriteU16(dst, boardAlignment()->rollDegrees);
1122         sbufWriteU16(dst, boardAlignment()->pitchDegrees);
1123         sbufWriteU16(dst, boardAlignment()->yawDegrees);
1124
1125         sbufWriteU16(dst, 0); // was currentMeterScale, see MSP_CURRENT_METER_CONFIG
1126         sbufWriteU16(dst, 0); //was currentMeterOffset, see MSP_CURRENT_METER_CONFIG
1127         break;
1128
1129     case MSP_CF_SERIAL_CONFIG:
1130         for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
1131             if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
1132                 continue;
1133             };
1134             sbufWriteU8(dst, serialConfig()->portConfigs[i].identifier);
1135             sbufWriteU16(dst, serialConfig()->portConfigs[i].functionMask);
1136             sbufWriteU8(dst, serialConfig()->portConfigs[i].msp_baudrateIndex);
1137             sbufWriteU8(dst, serialConfig()->portConfigs[i].gps_baudrateIndex);
1138             sbufWriteU8(dst, serialConfig()->portConfigs[i].telemetry_baudrateIndex);
1139             sbufWriteU8(dst, serialConfig()->portConfigs[i].blackbox_baudrateIndex);
1140         }
1141         break;
1142
1143 #ifdef USE_LED_STRIP
1144     case MSP_LED_COLORS:
1145         for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
1146             const hsvColor_t *color = &ledStripConfig()->colors[i];
1147             sbufWriteU16(dst, color->h);
1148             sbufWriteU8(dst, color->s);
1149             sbufWriteU8(dst, color->v);
1150         }
1151         break;
1152
1153     case MSP_LED_STRIP_CONFIG:
1154         for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
1155             const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
1156             sbufWriteU32(dst, *ledConfig);
1157         }
1158         break;
1159
1160     case MSP_LED_STRIP_MODECOLOR:
1161         for (int i = 0; i < LED_MODE_COUNT; i++) {
1162             for (int j = 0; j < LED_DIRECTION_COUNT; j++) {
1163                 sbufWriteU8(dst, i);
1164                 sbufWriteU8(dst, j);
1165                 sbufWriteU8(dst, ledStripConfig()->modeColors[i].color[j]);
1166             }
1167         }
1168
1169         for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) {
1170             sbufWriteU8(dst, LED_MODE_COUNT);
1171             sbufWriteU8(dst, j);
1172             sbufWriteU8(dst, ledStripConfig()->specialColors.color[j]);
1173         }
1174
1175         sbufWriteU8(dst, LED_AUX_CHANNEL);
1176         sbufWriteU8(dst, 0);
1177         sbufWriteU8(dst, ledStripConfig()->ledstrip_aux_channel);
1178         break;
1179 #endif
1180
1181     case MSP_DATAFLASH_SUMMARY:
1182         serializeDataflashSummaryReply(dst);
1183         break;
1184
1185     case MSP_BLACKBOX_CONFIG:
1186 #ifdef USE_BLACKBOX
1187         sbufWriteU8(dst, 1); //Blackbox supported
1188         sbufWriteU8(dst, blackboxConfig()->device);
1189         sbufWriteU8(dst, 1); // Rate numerator, not used anymore
1190         sbufWriteU8(dst, blackboxGetRateDenom());
1191         sbufWriteU16(dst, blackboxConfig()->p_ratio);
1192 #else
1193         sbufWriteU8(dst, 0); // Blackbox not supported
1194         sbufWriteU8(dst, 0);
1195         sbufWriteU8(dst, 0);
1196         sbufWriteU8(dst, 0);
1197         sbufWriteU16(dst, 0);
1198 #endif
1199         break;
1200
1201     case MSP_SDCARD_SUMMARY:
1202         serializeSDCardSummaryReply(dst);
1203         break;
1204
1205     case MSP_MOTOR_3D_CONFIG:
1206         sbufWriteU16(dst, flight3DConfig()->deadband3d_low);
1207         sbufWriteU16(dst, flight3DConfig()->deadband3d_high);
1208         sbufWriteU16(dst, flight3DConfig()->neutral3d);
1209         break;
1210
1211     case MSP_RC_DEADBAND:
1212         sbufWriteU8(dst, rcControlsConfig()->deadband);
1213         sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
1214         sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband);
1215         sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle);
1216         break;
1217
1218     case MSP_SENSOR_ALIGNMENT:
1219         sbufWriteU8(dst, gyroConfig()->gyro_align);
1220         sbufWriteU8(dst, accelerometerConfig()->acc_align);
1221         sbufWriteU8(dst, compassConfig()->mag_align);
1222         break;
1223
1224     case MSP_ADVANCED_CONFIG:
1225         sbufWriteU8(dst, gyroConfig()->gyro_sync_denom);
1226         sbufWriteU8(dst, pidConfig()->pid_process_denom);
1227         sbufWriteU8(dst, motorConfig()->dev.useUnsyncedPwm);
1228         sbufWriteU8(dst, motorConfig()->dev.motorPwmProtocol);
1229         sbufWriteU16(dst, motorConfig()->dev.motorPwmRate);
1230         sbufWriteU16(dst, motorConfig()->digitalIdleOffsetValue);
1231         sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
1232         sbufWriteU8(dst, motorConfig()->dev.motorPwmInversion);
1233         break;
1234
1235     case MSP_FILTER_CONFIG :
1236         sbufWriteU8(dst, gyroConfig()->gyro_lowpass_hz);
1237         sbufWriteU16(dst, currentPidProfile->dterm_lowpass_hz);
1238         sbufWriteU16(dst, currentPidProfile->yaw_lowpass_hz);
1239         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1);
1240         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1);
1241         sbufWriteU16(dst, currentPidProfile->dterm_notch_hz);
1242         sbufWriteU16(dst, currentPidProfile->dterm_notch_cutoff);
1243         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2);
1244         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2);
1245         sbufWriteU8(dst, currentPidProfile->dterm_filter_type);
1246         break;
1247
1248     case MSP_PID_ADVANCED:
1249         sbufWriteU16(dst, 0);
1250         sbufWriteU16(dst, 0);
1251         sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit
1252         sbufWriteU8(dst, 0); // reserved
1253         sbufWriteU8(dst, currentPidProfile->vbatPidCompensation);
1254         sbufWriteU8(dst, currentPidProfile->setpointRelaxRatio);
1255         sbufWriteU8(dst, MIN(currentPidProfile->dtermSetpointWeight, 255));
1256         sbufWriteU8(dst, 0); // reserved
1257         sbufWriteU8(dst, 0); // reserved
1258         sbufWriteU8(dst, 0); // reserved
1259         sbufWriteU16(dst, currentPidProfile->rateAccelLimit);
1260         sbufWriteU16(dst, currentPidProfile->yawRateAccelLimit);
1261         sbufWriteU8(dst, currentPidProfile->levelAngleLimit);
1262         sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity
1263         sbufWriteU16(dst, currentPidProfile->itermThrottleThreshold);
1264         sbufWriteU16(dst, currentPidProfile->itermAcceleratorGain);
1265         sbufWriteU16(dst, currentPidProfile->dtermSetpointWeight);
1266         break;
1267
1268     case MSP_SENSOR_CONFIG:
1269         sbufWriteU8(dst, accelerometerConfig()->acc_hardware);
1270         sbufWriteU8(dst, barometerConfig()->baro_hardware);
1271         sbufWriteU8(dst, compassConfig()->mag_hardware);
1272         break;
1273
1274 #if defined(USE_VTX_COMMON)
1275     case MSP_VTX_CONFIG:
1276         {
1277             const vtxDevice_t *vtxDevice = vtxCommonDevice();
1278             if (vtxDevice) {
1279                 uint8_t pitmode=0;
1280                 vtxCommonGetPitMode(vtxDevice, &pitmode);
1281                 sbufWriteU8(dst, vtxCommonGetDeviceType(vtxDevice));
1282                 sbufWriteU8(dst, vtxSettingsConfig()->band);
1283                 sbufWriteU8(dst, vtxSettingsConfig()->channel);
1284                 sbufWriteU8(dst, vtxSettingsConfig()->power);
1285                 sbufWriteU8(dst, pitmode);
1286                 sbufWriteU16(dst, vtxSettingsConfig()->freq);
1287                 // future extensions here...
1288             } else {
1289                 sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX detected
1290             }
1291         }
1292
1293         break;
1294 #endif
1295
1296     case MSP_TX_INFO:
1297         sbufWriteU8(dst, rssiSource);
1298         uint8_t rtcDateTimeIsSet = 0;
1299 #ifdef USE_RTC_TIME
1300         dateTime_t dt;
1301         if (rtcGetDateTime(&dt)) {
1302             rtcDateTimeIsSet = 1;
1303         }
1304 #else
1305         rtcDateTimeIsSet = RTC_NOT_SUPPORTED;
1306 #endif
1307         sbufWriteU8(dst, rtcDateTimeIsSet);
1308
1309         break;
1310 #ifdef USE_RTC_TIME
1311     case MSP_RTC:
1312         {
1313             dateTime_t dt;
1314             if (rtcGetDateTime(&dt)) {
1315                 sbufWriteU16(dst, dt.year);
1316                 sbufWriteU8(dst, dt.month);
1317                 sbufWriteU8(dst, dt.day);
1318                 sbufWriteU8(dst, dt.hours);
1319                 sbufWriteU8(dst, dt.minutes);
1320                 sbufWriteU8(dst, dt.seconds);
1321                 sbufWriteU16(dst, dt.millis);
1322             }
1323         }
1324
1325         break;
1326 #endif
1327     default:
1328         unsupportedCommand = true;
1329     }
1330     return !unsupportedCommand;
1331 }
1332
1333 static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *arg, sbuf_t *dst)
1334 {
1335     switch (cmdMSP) {
1336     case MSP_BOXNAMES:
1337         {
1338             const int page = sbufBytesRemaining(arg) ? sbufReadU8(arg) : 0;
1339             serializeBoxReply(dst, page, &serializeBoxNameFn);
1340         }
1341         break;
1342     case MSP_BOXIDS:
1343         {
1344             const int page = sbufBytesRemaining(arg) ? sbufReadU8(arg) : 0;
1345             serializeBoxReply(dst, page, &serializeBoxPermanentIdFn);
1346         }
1347         break;
1348     default:
1349         return MSP_RESULT_CMD_UNKNOWN;
1350     }
1351     return MSP_RESULT_ACK;
1352 }
1353 #endif // USE_OSD_SLAVE
1354
1355 #ifdef USE_FLASHFS
1356 static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
1357 {
1358     const unsigned int dataSize = sbufBytesRemaining(src);
1359     const uint32_t readAddress = sbufReadU32(src);
1360     uint16_t readLength;
1361     bool allowCompression = false;
1362     bool useLegacyFormat;
1363     if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
1364         readLength = sbufReadU16(src);
1365         if (sbufBytesRemaining(src)) {
1366             allowCompression = sbufReadU8(src);
1367         }
1368         useLegacyFormat = false;
1369     } else {
1370         readLength = 128;
1371         useLegacyFormat = true;
1372     }
1373
1374     serializeDataflashReadReply(dst, readAddress, readLength, useLegacyFormat, allowCompression);
1375 }
1376 #endif
1377
1378 #ifdef USE_OSD_SLAVE
1379 static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
1380 {
1381     UNUSED(cmdMSP);
1382     UNUSED(src);
1383
1384     switch(cmdMSP) {
1385     case MSP_RESET_CONF:
1386         resetEEPROM();
1387         readEEPROM();
1388         break;
1389     case MSP_EEPROM_WRITE:
1390         writeEEPROM();
1391         readEEPROM();
1392         break;
1393     default:
1394         // we do not know how to handle the (valid) message, indicate error MSP $M!
1395         return MSP_RESULT_ERROR;
1396     }
1397     return MSP_RESULT_ACK;
1398 }
1399
1400 #else
1401
1402 static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
1403 {
1404     uint32_t i;
1405     uint8_t value;
1406     const unsigned int dataSize = sbufBytesRemaining(src);
1407     switch (cmdMSP) {
1408     case MSP_SELECT_SETTING:
1409         value = sbufReadU8(src);
1410         if ((value & RATEPROFILE_MASK) == 0) {
1411             if (!ARMING_FLAG(ARMED)) {
1412                 if (value >= MAX_PROFILE_COUNT) {
1413                     value = 0;
1414                 }
1415                 changePidProfile(value);
1416             }
1417         } else {
1418             value = value & ~RATEPROFILE_MASK;
1419
1420             if (value >= CONTROL_RATE_PROFILE_COUNT) {
1421                 value = 0;
1422             }
1423             changeControlRateProfile(value);
1424         }
1425         break;
1426
1427     case MSP_COPY_PROFILE:
1428         value = sbufReadU8(src);        // 0 = pid profile, 1 = control rate profile
1429         uint8_t dstProfileIndex = sbufReadU8(src);
1430         uint8_t srcProfileIndex = sbufReadU8(src);
1431         if (value == 0) {
1432             pidCopyProfile(dstProfileIndex, srcProfileIndex);
1433         }
1434         else if (value == 1) {
1435             copyControlRateProfile(dstProfileIndex, srcProfileIndex);
1436         }
1437         break;
1438
1439 #if defined(USE_GPS) || defined(USE_MAG)
1440     case MSP_SET_HEADING:
1441         magHold = sbufReadU16(src);
1442         break;
1443 #endif
1444
1445     case MSP_SET_RAW_RC:
1446 #ifdef USE_RX_MSP
1447         {
1448             uint8_t channelCount = dataSize / sizeof(uint16_t);
1449             if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) {
1450                 return MSP_RESULT_ERROR;
1451             } else {
1452                 uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
1453                 for (int i = 0; i < channelCount; i++) {
1454                     frame[i] = sbufReadU16(src);
1455                 }
1456                 rxMspFrameReceive(frame, channelCount);
1457             }
1458         }
1459 #endif
1460         break;
1461     case MSP_SET_ACC_TRIM:
1462         accelerometerConfigMutable()->accelerometerTrims.values.pitch = sbufReadU16(src);
1463         accelerometerConfigMutable()->accelerometerTrims.values.roll  = sbufReadU16(src);
1464         break;
1465     case MSP_SET_ARMING_CONFIG:
1466         armingConfigMutable()->auto_disarm_delay = sbufReadU8(src);
1467         sbufReadU8(src); // reserved
1468         if (sbufBytesRemaining(src)) {
1469           imuConfigMutable()->small_angle = sbufReadU8(src);
1470         }
1471         break;
1472
1473     case MSP_SET_PID_CONTROLLER:
1474         break;
1475
1476     case MSP_SET_PID:
1477         for (int i = 0; i < PID_ITEM_COUNT; i++) {
1478             currentPidProfile->pid[i].P = sbufReadU8(src);
1479             currentPidProfile->pid[i].I = sbufReadU8(src);
1480             currentPidProfile->pid[i].D = sbufReadU8(src);
1481         }
1482         pidInitConfig(currentPidProfile);
1483         break;
1484
1485     case MSP_SET_MODE_RANGE:
1486         i = sbufReadU8(src);
1487         if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
1488             modeActivationCondition_t *mac = modeActivationConditionsMutable(i);
1489             i = sbufReadU8(src);
1490             const box_t *box = findBoxByPermanentId(i);
1491             if (box) {
1492                 mac->modeId = box->boxId;
1493                 mac->auxChannelIndex = sbufReadU8(src);
1494                 mac->range.startStep = sbufReadU8(src);
1495                 mac->range.endStep = sbufReadU8(src);
1496
1497                 useRcControlsConfig(currentPidProfile);
1498             } else {
1499                 return MSP_RESULT_ERROR;
1500             }
1501         } else {
1502             return MSP_RESULT_ERROR;
1503         }
1504         break;
1505
1506     case MSP_SET_ADJUSTMENT_RANGE:
1507         i = sbufReadU8(src);
1508         if (i < MAX_ADJUSTMENT_RANGE_COUNT) {
1509             adjustmentRange_t *adjRange = adjustmentRangesMutable(i);
1510             i = sbufReadU8(src);
1511             if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) {
1512                 adjRange->adjustmentIndex = i;
1513                 adjRange->auxChannelIndex = sbufReadU8(src);
1514                 adjRange->range.startStep = sbufReadU8(src);
1515                 adjRange->range.endStep = sbufReadU8(src);
1516                 adjRange->adjustmentFunction = sbufReadU8(src);
1517                 adjRange->auxSwitchChannelIndex = sbufReadU8(src);
1518             } else {
1519                 return MSP_RESULT_ERROR;
1520             }
1521         } else {
1522             return MSP_RESULT_ERROR;
1523         }
1524         break;
1525
1526     case MSP_SET_RC_TUNING:
1527         if (sbufBytesRemaining(src) >= 10) {
1528             value = sbufReadU8(src);
1529             if (currentControlRateProfile->rcRates[FD_PITCH] == currentControlRateProfile->rcRates[FD_ROLL]) {
1530                 currentControlRateProfile->rcRates[FD_PITCH] = value;
1531             }
1532             currentControlRateProfile->rcRates[FD_ROLL] = value;
1533
1534             value = sbufReadU8(src);
1535             if (currentControlRateProfile->rcExpo[FD_PITCH] == currentControlRateProfile->rcExpo[FD_ROLL]) {
1536                 currentControlRateProfile->rcExpo[FD_PITCH] = value;
1537             }
1538             currentControlRateProfile->rcExpo[FD_ROLL] = value;
1539
1540             for (int i = 0; i < 3; i++) {
1541                 currentControlRateProfile->rates[i] = sbufReadU8(src);
1542             }
1543
1544             value = sbufReadU8(src);
1545             currentControlRateProfile->dynThrPID = MIN(value, CONTROL_RATE_CONFIG_TPA_MAX);
1546             currentControlRateProfile->thrMid8 = sbufReadU8(src);
1547             currentControlRateProfile->thrExpo8 = sbufReadU8(src);
1548             currentControlRateProfile->tpa_breakpoint = sbufReadU16(src);
1549
1550             if (sbufBytesRemaining(src) >= 1) {
1551                 currentControlRateProfile->rcExpo[FD_YAW] = sbufReadU8(src);
1552             }
1553
1554             if (sbufBytesRemaining(src) >= 1) {
1555                 currentControlRateProfile->rcRates[FD_YAW] = sbufReadU8(src);
1556             }
1557
1558             if (sbufBytesRemaining(src) >= 1) {
1559                 currentControlRateProfile->rcRates[FD_PITCH] = sbufReadU8(src);
1560             }
1561
1562             if (sbufBytesRemaining(src) >= 1) {
1563                 currentControlRateProfile->rcExpo[FD_PITCH] = sbufReadU8(src);
1564             }
1565
1566             initRcProcessing();
1567         } else {
1568             return MSP_RESULT_ERROR;
1569         }
1570         break;
1571
1572     case MSP_SET_MOTOR_CONFIG:
1573         motorConfigMutable()->minthrottle = sbufReadU16(src);
1574         motorConfigMutable()->maxthrottle = sbufReadU16(src);
1575         motorConfigMutable()->mincommand = sbufReadU16(src);
1576         break;
1577
1578 #ifdef USE_GPS
1579     case MSP_SET_GPS_CONFIG:
1580         gpsConfigMutable()->provider = sbufReadU8(src);
1581         gpsConfigMutable()->sbasMode = sbufReadU8(src);
1582         gpsConfigMutable()->autoConfig = sbufReadU8(src);
1583         gpsConfigMutable()->autoBaud = sbufReadU8(src);
1584         break;
1585 #endif
1586
1587 #ifdef USE_MAG
1588     case MSP_SET_COMPASS_CONFIG:
1589         compassConfigMutable()->mag_declination = sbufReadU16(src) * 10;
1590         break;
1591 #endif
1592
1593     case MSP_SET_MOTOR:
1594         for (int i = 0; i < getMotorCount(); i++) {
1595             motor_disarmed[i] = convertExternalToMotor(sbufReadU16(src));
1596         }
1597         break;
1598
1599     case MSP_SET_SERVO_CONFIGURATION:
1600 #ifdef USE_SERVOS
1601         if (dataSize != 1 + 12) {
1602             return MSP_RESULT_ERROR;
1603         }
1604         i = sbufReadU8(src);
1605         if (i >= MAX_SUPPORTED_SERVOS) {
1606             return MSP_RESULT_ERROR;
1607         } else {
1608             servoParamsMutable(i)->min = sbufReadU16(src);
1609             servoParamsMutable(i)->max = sbufReadU16(src);
1610             servoParamsMutable(i)->middle = sbufReadU16(src);
1611             servoParamsMutable(i)->rate = sbufReadU8(src);
1612             servoParamsMutable(i)->forwardFromChannel = sbufReadU8(src);
1613             servoParamsMutable(i)->reversedSources = sbufReadU32(src);
1614         }
1615 #endif
1616         break;
1617
1618     case MSP_SET_SERVO_MIX_RULE:
1619 #ifdef USE_SERVOS
1620         i = sbufReadU8(src);
1621         if (i >= MAX_SERVO_RULES) {
1622             return MSP_RESULT_ERROR;
1623         } else {
1624             customServoMixersMutable(i)->targetChannel = sbufReadU8(src);
1625             customServoMixersMutable(i)->inputSource = sbufReadU8(src);
1626             customServoMixersMutable(i)->rate = sbufReadU8(src);
1627             customServoMixersMutable(i)->speed = sbufReadU8(src);
1628             customServoMixersMutable(i)->min = sbufReadU8(src);
1629             customServoMixersMutable(i)->max = sbufReadU8(src);
1630             customServoMixersMutable(i)->box = sbufReadU8(src);
1631             loadCustomServoMixer();
1632         }
1633 #endif
1634         break;
1635
1636     case MSP_SET_MOTOR_3D_CONFIG:
1637         flight3DConfigMutable()->deadband3d_low = sbufReadU16(src);
1638         flight3DConfigMutable()->deadband3d_high = sbufReadU16(src);
1639         flight3DConfigMutable()->neutral3d = sbufReadU16(src);
1640         break;
1641
1642     case MSP_SET_RC_DEADBAND:
1643         rcControlsConfigMutable()->deadband = sbufReadU8(src);
1644         rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
1645         rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src);
1646         flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src);
1647         break;
1648
1649     case MSP_SET_RESET_CURR_PID:
1650         resetPidProfile(currentPidProfile);
1651         break;
1652     case MSP_SET_SENSOR_ALIGNMENT:
1653         gyroConfigMutable()->gyro_align = sbufReadU8(src);
1654         accelerometerConfigMutable()->acc_align = sbufReadU8(src);
1655         compassConfigMutable()->mag_align = sbufReadU8(src);
1656         break;
1657
1658     case MSP_SET_ADVANCED_CONFIG:
1659         gyroConfigMutable()->gyro_sync_denom = sbufReadU8(src);
1660         pidConfigMutable()->pid_process_denom = sbufReadU8(src);
1661         motorConfigMutable()->dev.useUnsyncedPwm = sbufReadU8(src);
1662 #ifdef USE_DSHOT
1663         motorConfigMutable()->dev.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
1664 #else
1665         motorConfigMutable()->dev.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
1666 #endif
1667         motorConfigMutable()->dev.motorPwmRate = sbufReadU16(src);
1668         if (sbufBytesRemaining(src) >= 2) {
1669             motorConfigMutable()->digitalIdleOffsetValue = sbufReadU16(src);
1670         }
1671         if (sbufBytesRemaining(src)) {
1672             gyroConfigMutable()->gyro_use_32khz = sbufReadU8(src);
1673         }
1674         validateAndFixGyroConfig();
1675
1676         if (sbufBytesRemaining(src)) {
1677             motorConfigMutable()->dev.motorPwmInversion = sbufReadU8(src);
1678         }
1679         break;
1680
1681     case MSP_SET_FILTER_CONFIG:
1682         gyroConfigMutable()->gyro_lowpass_hz = sbufReadU8(src);
1683         currentPidProfile->dterm_lowpass_hz = sbufReadU16(src);
1684         currentPidProfile->yaw_lowpass_hz = sbufReadU16(src);
1685         if (sbufBytesRemaining(src) >= 8) {
1686             gyroConfigMutable()->gyro_soft_notch_hz_1 = sbufReadU16(src);
1687             gyroConfigMutable()->gyro_soft_notch_cutoff_1 = sbufReadU16(src);
1688             currentPidProfile->dterm_notch_hz = sbufReadU16(src);
1689             currentPidProfile->dterm_notch_cutoff = sbufReadU16(src);
1690         }
1691         if (sbufBytesRemaining(src) >= 4) {
1692             gyroConfigMutable()->gyro_soft_notch_hz_2 = sbufReadU16(src);
1693             gyroConfigMutable()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
1694         }
1695         if (sbufBytesRemaining(src) >= 1) {
1696             currentPidProfile->dterm_filter_type = sbufReadU8(src);
1697         }
1698         // reinitialize the gyro filters with the new values
1699         validateAndFixGyroConfig();
1700         gyroInitFilters();
1701         // reinitialize the PID filters with the new values
1702         pidInitFilters(currentPidProfile);
1703         break;
1704
1705     case MSP_SET_PID_ADVANCED:
1706         sbufReadU16(src);
1707         sbufReadU16(src);
1708         sbufReadU16(src); // was pidProfile.yaw_p_limit
1709         sbufReadU8(src); // reserved
1710         currentPidProfile->vbatPidCompensation = sbufReadU8(src);
1711         currentPidProfile->setpointRelaxRatio = sbufReadU8(src);
1712         currentPidProfile->dtermSetpointWeight = sbufReadU8(src);
1713         sbufReadU8(src); // reserved
1714         sbufReadU8(src); // reserved
1715         sbufReadU8(src); // reserved
1716         currentPidProfile->rateAccelLimit = sbufReadU16(src);
1717         currentPidProfile->yawRateAccelLimit = sbufReadU16(src);
1718         if (sbufBytesRemaining(src) >= 2) {
1719             currentPidProfile->levelAngleLimit = sbufReadU8(src);
1720             sbufReadU8(src); // was pidProfile.levelSensitivity
1721         }
1722         if (sbufBytesRemaining(src) >= 4) {
1723             currentPidProfile->itermThrottleThreshold = sbufReadU16(src);
1724             currentPidProfile->itermAcceleratorGain = sbufReadU16(src);
1725         }
1726         if (sbufBytesRemaining(src) >= 2) {
1727             currentPidProfile->dtermSetpointWeight = sbufReadU16(src);
1728         }
1729         pidInitConfig(currentPidProfile);
1730         break;
1731
1732     case MSP_SET_SENSOR_CONFIG:
1733         accelerometerConfigMutable()->acc_hardware = sbufReadU8(src);
1734         barometerConfigMutable()->baro_hardware = sbufReadU8(src);
1735         compassConfigMutable()->mag_hardware = sbufReadU8(src);
1736         break;
1737
1738     case MSP_RESET_CONF:
1739         if (!ARMING_FLAG(ARMED)) {
1740             resetEEPROM();
1741             readEEPROM();
1742         }
1743         break;
1744
1745     case MSP_ACC_CALIBRATION:
1746         if (!ARMING_FLAG(ARMED))
1747             accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
1748         break;
1749
1750     case MSP_MAG_CALIBRATION:
1751         if (!ARMING_FLAG(ARMED))
1752             ENABLE_STATE(CALIBRATE_MAG);
1753         break;
1754
1755     case MSP_EEPROM_WRITE:
1756         if (ARMING_FLAG(ARMED)) {
1757             return MSP_RESULT_ERROR;
1758         }
1759         writeEEPROM();
1760         readEEPROM();
1761         break;
1762
1763 #ifdef USE_BLACKBOX
1764     case MSP_SET_BLACKBOX_CONFIG:
1765         // Don't allow config to be updated while Blackbox is logging
1766         if (blackboxMayEditConfig()) {
1767             blackboxConfigMutable()->device = sbufReadU8(src);
1768             const int rateNum = sbufReadU8(src); // was rate_num
1769             const int rateDenom = sbufReadU8(src); // was rate_denom
1770             if (sbufBytesRemaining(src) >= 2) {
1771                 // p_ratio specified, so use it directly
1772                 blackboxConfigMutable()->p_ratio = sbufReadU16(src);
1773             } else {
1774                 // p_ratio not specified in MSP, so calculate it from old rateNum and rateDenom
1775                 blackboxConfigMutable()->p_ratio = blackboxCalculatePDenom(rateNum, rateDenom);
1776             }
1777         }
1778         break;
1779 #endif
1780
1781 #ifdef USE_VTX_COMMON
1782     case MSP_SET_VTX_CONFIG:
1783         {
1784             vtxDevice_t *vtxDevice = vtxCommonDevice();
1785             if (vtxDevice) {
1786                 if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) {
1787                     uint16_t newFrequency = sbufReadU16(src);
1788                     if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) {  //value is band and channel
1789                         const uint8_t newBand = (newFrequency / 8) + 1;
1790                         const uint8_t newChannel = (newFrequency % 8) + 1;
1791                         vtxSettingsConfigMutable()->band = newBand;
1792                         vtxSettingsConfigMutable()->channel = newChannel;
1793                         vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(newBand, newChannel);
1794                     } else {  //value is frequency in MHz
1795                         vtxSettingsConfigMutable()->band = 0;
1796                         vtxSettingsConfigMutable()->freq = newFrequency;
1797                     }
1798
1799                     if (sbufBytesRemaining(src) > 1) {
1800                         vtxSettingsConfigMutable()->power = sbufReadU8(src);
1801                         // Delegate pitmode to vtx directly
1802                         const uint8_t newPitmode = sbufReadU8(src);
1803                         uint8_t currentPitmode = 0;
1804                         vtxCommonGetPitMode(vtxDevice, &currentPitmode);
1805                         if (currentPitmode != newPitmode) {
1806                             vtxCommonSetPitMode(vtxDevice, newPitmode);
1807                         }
1808                     }
1809                 }
1810             }
1811         }
1812         break;
1813 #endif
1814
1815 #ifdef USE_CAMERA_CONTROL
1816     case MSP_CAMERA_CONTROL:
1817         {
1818             if (ARMING_FLAG(ARMED)) {
1819                 return MSP_RESULT_ERROR;
1820             }
1821
1822             const uint8_t key = sbufReadU8(src);
1823             cameraControlKeyPress(key, 0);
1824         }
1825         break;
1826 #endif
1827
1828     case MSP_SET_ARMING_DISABLED:
1829         {
1830             const uint8_t command = sbufReadU8(src);
1831             uint8_t disableRunawayTakeoff = 0;
1832 #ifndef USE_RUNAWAY_TAKEOFF
1833             UNUSED(disableRunawayTakeoff);
1834 #endif
1835             if (sbufBytesRemaining(src)) {
1836                 disableRunawayTakeoff = sbufReadU8(src);
1837             }
1838             if (command) {
1839                 setArmingDisabled(ARMING_DISABLED_MSP);
1840                 if (ARMING_FLAG(ARMED)) {
1841                     disarm();
1842                 }
1843 #ifdef USE_RUNAWAY_TAKEOFF
1844                 runawayTakeoffTemporaryDisable(false);
1845 #endif
1846             } else {
1847                 unsetArmingDisabled(ARMING_DISABLED_MSP);
1848 #ifdef USE_RUNAWAY_TAKEOFF
1849                 runawayTakeoffTemporaryDisable(disableRunawayTakeoff);
1850 #endif
1851             }
1852         }
1853         break;
1854
1855 #ifdef USE_FLASHFS
1856     case MSP_DATAFLASH_ERASE:
1857         flashfsEraseCompletely();
1858         break;
1859 #endif
1860
1861 #ifdef USE_GPS
1862     case MSP_SET_RAW_GPS:
1863         if (sbufReadU8(src)) {
1864             ENABLE_STATE(GPS_FIX);
1865         } else {
1866             DISABLE_STATE(GPS_FIX);
1867         }
1868         gpsSol.numSat = sbufReadU8(src);
1869         gpsSol.llh.lat = sbufReadU32(src);
1870         gpsSol.llh.lon = sbufReadU32(src);
1871         gpsSol.llh.alt = sbufReadU16(src);
1872         gpsSol.groundSpeed = sbufReadU16(src);
1873         GPS_update |= 2;        // New data signalisation to GPS functions // FIXME Magic Numbers
1874         break;
1875 #endif // USE_GPS
1876     case MSP_SET_FEATURE_CONFIG:
1877         featureClearAll();
1878         featureSet(sbufReadU32(src)); // features bitmap
1879         break;
1880
1881 #ifdef USE_BEEPER
1882     case MSP_SET_BEEPER_CONFIG:
1883         beeperOffClearAll();
1884         setBeeperOffMask(sbufReadU32(src));
1885         if (sbufBytesRemaining(src) >= 1) {
1886             beeperConfigMutable()->dshotBeaconTone = sbufReadU8(src);
1887         }
1888         break;
1889 #endif
1890
1891     case MSP_SET_BOARD_ALIGNMENT_CONFIG:
1892         boardAlignmentMutable()->rollDegrees = sbufReadU16(src);
1893         boardAlignmentMutable()->pitchDegrees = sbufReadU16(src);
1894         boardAlignmentMutable()->yawDegrees = sbufReadU16(src);
1895         break;
1896
1897     case MSP_SET_MIXER_CONFIG:
1898 #ifndef USE_QUAD_MIXER_ONLY
1899         mixerConfigMutable()->mixerMode = sbufReadU8(src);
1900 #else
1901         sbufReadU8(src);
1902 #endif
1903         if (sbufBytesRemaining(src) >= 1) {
1904             mixerConfigMutable()->yaw_motors_reversed = sbufReadU8(src);
1905         }
1906         break;
1907
1908     case MSP_SET_RX_CONFIG:
1909         rxConfigMutable()->serialrx_provider = sbufReadU8(src);
1910         rxConfigMutable()->maxcheck = sbufReadU16(src);
1911         rxConfigMutable()->midrc = sbufReadU16(src);
1912         rxConfigMutable()->mincheck = sbufReadU16(src);
1913         rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src);
1914         if (sbufBytesRemaining(src) >= 4) {
1915             rxConfigMutable()->rx_min_usec = sbufReadU16(src);
1916             rxConfigMutable()->rx_max_usec = sbufReadU16(src);
1917         }
1918         if (sbufBytesRemaining(src) >= 4) {
1919             rxConfigMutable()->rcInterpolation = sbufReadU8(src);
1920             rxConfigMutable()->rcInterpolationInterval = sbufReadU8(src);
1921             rxConfigMutable()->airModeActivateThreshold = (sbufReadU16(src) - 1000) / 10;
1922         }
1923         if (sbufBytesRemaining(src) >= 6) {
1924 #ifdef USE_RX_SPI
1925             rxSpiConfigMutable()->rx_spi_protocol = sbufReadU8(src);
1926             rxSpiConfigMutable()->rx_spi_id = sbufReadU32(src);
1927             rxSpiConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(src);
1928 #else
1929             sbufReadU8(src);
1930             sbufReadU32(src);
1931             sbufReadU8(src);
1932 #endif
1933         }
1934         if (sbufBytesRemaining(src) >= 1) {
1935             rxConfigMutable()->fpvCamAngleDegrees = sbufReadU8(src);
1936         }
1937         break;
1938
1939     case MSP_SET_FAILSAFE_CONFIG:
1940         failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
1941         failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src);
1942         failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src);
1943         failsafeConfigMutable()->failsafe_switch_mode = sbufReadU8(src);
1944         failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src);
1945         failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src);
1946         break;
1947
1948     case MSP_SET_RXFAIL_CONFIG:
1949         i = sbufReadU8(src);
1950         if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) {
1951             rxFailsafeChannelConfigsMutable(i)->mode = sbufReadU8(src);
1952             rxFailsafeChannelConfigsMutable(i)->step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
1953         } else {
1954             return MSP_RESULT_ERROR;
1955         }
1956         break;
1957
1958     case MSP_SET_RSSI_CONFIG:
1959         rxConfigMutable()->rssi_channel = sbufReadU8(src);
1960         break;
1961
1962     case MSP_SET_RX_MAP:
1963         for (int i = 0; i < RX_MAPPABLE_CHANNEL_COUNT; i++) {
1964             rxConfigMutable()->rcmap[i] = sbufReadU8(src);
1965         }
1966         break;
1967
1968     case MSP_SET_BF_CONFIG:
1969 #ifdef USE_QUAD_MIXER_ONLY
1970         sbufReadU8(src); // mixerMode ignored
1971 #else
1972         mixerConfigMutable()->mixerMode = sbufReadU8(src); // mixerMode
1973 #endif
1974
1975         featureClearAll();
1976         featureSet(sbufReadU32(src)); // features bitmap
1977
1978         rxConfigMutable()->serialrx_provider = sbufReadU8(src); // serialrx_type
1979
1980         boardAlignmentMutable()->rollDegrees = sbufReadU16(src); // board_align_roll
1981         boardAlignmentMutable()->pitchDegrees = sbufReadU16(src); // board_align_pitch
1982         boardAlignmentMutable()->yawDegrees = sbufReadU16(src); // board_align_
1983         sbufReadU16(src); // was currentMeterScale, see MSP_SET_CURRENT_METER_CONFIG
1984         sbufReadU16(src); // was currentMeterOffset see MSP_SET_CURRENT_METER_CONFIG
1985         break;
1986
1987     case MSP_SET_CF_SERIAL_CONFIG:
1988         {
1989             uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4);
1990
1991             if (dataSize % portConfigSize != 0) {
1992                 return MSP_RESULT_ERROR;
1993             }
1994
1995             uint8_t remainingPortsInPacket = dataSize / portConfigSize;
1996
1997             while (remainingPortsInPacket--) {
1998                 uint8_t identifier = sbufReadU8(src);
1999
2000                 serialPortConfig_t *portConfig = serialFindPortConfiguration(identifier);
2001                 if (!portConfig) {
2002                     return MSP_RESULT_ERROR;
2003                 }
2004
2005                 portConfig->identifier = identifier;
2006                 portConfig->functionMask = sbufReadU16(src);
2007                 portConfig->msp_baudrateIndex = sbufReadU8(src);
2008                 portConfig->gps_baudrateIndex = sbufReadU8(src);
2009                 portConfig->telemetry_baudrateIndex = sbufReadU8(src);
2010                 portConfig->blackbox_baudrateIndex = sbufReadU8(src);
2011             }
2012         }
2013         break;
2014
2015 #ifdef USE_LED_STRIP
2016     case MSP_SET_LED_COLORS:
2017         for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
2018             hsvColor_t *color = &ledStripConfigMutable()->colors[i];
2019             color->h = sbufReadU16(src);
2020             color->s = sbufReadU8(src);
2021             color->v = sbufReadU8(src);
2022         }
2023         break;
2024
2025     case MSP_SET_LED_STRIP_CONFIG:
2026         {
2027             i = sbufReadU8(src);
2028             if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) {
2029                 return MSP_RESULT_ERROR;
2030             }
2031             ledConfig_t *ledConfig = &ledStripConfigMutable()->ledConfigs[i];
2032             *ledConfig = sbufReadU32(src);
2033             reevaluateLedConfig();
2034         }
2035         break;
2036
2037     case MSP_SET_LED_STRIP_MODECOLOR:
2038         {
2039             ledModeIndex_e modeIdx = sbufReadU8(src);
2040             int funIdx = sbufReadU8(src);
2041             int color = sbufReadU8(src);
2042
2043             if (!setModeColor(modeIdx, funIdx, color))
2044                 return MSP_RESULT_ERROR;
2045         }
2046         break;
2047 #endif
2048
2049     case MSP_SET_NAME:
2050         memset(pilotConfigMutable()->name, 0, ARRAYLEN(pilotConfig()->name));
2051         for (unsigned int i = 0; i < MIN(MAX_NAME_LENGTH, dataSize); i++) {
2052             pilotConfigMutable()->name[i] = sbufReadU8(src);
2053         }
2054         break;
2055
2056 #ifdef USE_RTC_TIME
2057     case MSP_SET_RTC:
2058         {
2059             dateTime_t dt;
2060             dt.year = sbufReadU16(src);
2061             dt.month = sbufReadU8(src);
2062             dt.day = sbufReadU8(src);
2063             dt.hours = sbufReadU8(src);
2064             dt.minutes = sbufReadU8(src);
2065             dt.seconds = sbufReadU8(src);
2066             dt.millis = 0;
2067             rtcSetDateTime(&dt);
2068         }
2069
2070         break;
2071 #endif
2072
2073     case MSP_SET_TX_INFO:
2074         setRssiMsp(sbufReadU8(src));
2075
2076         break;
2077
2078 #if defined(USE_BOARD_INFO)
2079     case MSP_SET_BOARD_INFO:
2080         if (!boardInformationIsSet()) {
2081             char boardName[MAX_BOARD_NAME_LENGTH + 1] = {0};
2082             char manufacturerId[MAX_MANUFACTURER_ID_LENGTH + 1] = {0};
2083             uint8_t length = sbufReadU8(src);
2084             for (unsigned int i = 0; i < length; i++) {
2085                 boardName[i] = sbufReadU8(src);
2086             }
2087             length = sbufReadU8(src);
2088             for (unsigned int i = 0; i < length; i++) {
2089                 manufacturerId[i] = sbufReadU8(src);
2090             }
2091
2092             setBoardName(boardName);
2093             setManufacturerId(manufacturerId);
2094             persistBoardInformation();
2095         } else {
2096             return MSP_RESULT_ERROR;
2097         }
2098
2099         break;
2100 #if defined(USE_SIGNATURE)
2101     case MSP_SET_SIGNATURE:
2102         if (!signatureIsSet()) {
2103             uint8_t signature[SIGNATURE_LENGTH];
2104             sbufReadData(src, signature, SIGNATURE_LENGTH);
2105             setSignature(signature);
2106             persistSignature();
2107         } else {
2108             return MSP_RESULT_ERROR;
2109         }
2110
2111         break;
2112 #endif
2113 #endif // USE_BOARD_INFO
2114     default:
2115         // we do not know how to handle the (valid) message, indicate error MSP $M!
2116         return MSP_RESULT_ERROR;
2117     }
2118     return MSP_RESULT_ACK;
2119 }
2120 #endif // USE_OSD_SLAVE
2121
2122 static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
2123 {
2124     const unsigned int dataSize = sbufBytesRemaining(src);
2125     UNUSED(dataSize); // maybe unused due to compiler options
2126
2127     switch (cmdMSP) {
2128 #ifdef USE_TRANSPONDER
2129     case MSP_SET_TRANSPONDER_CONFIG: {
2130         // Backward compatibility to BFC 3.1.1 is lost for this message type
2131
2132         uint8_t provider = sbufReadU8(src);
2133         uint8_t bytesRemaining = dataSize - 1;
2134
2135         if (provider > TRANSPONDER_PROVIDER_COUNT) {
2136             return MSP_RESULT_ERROR;
2137         }
2138
2139         const uint8_t requirementIndex = provider - 1;
2140         const uint8_t transponderDataSize = transponderRequirements[requirementIndex].dataLength;
2141
2142         transponderConfigMutable()->provider = provider;
2143
2144         if (provider == TRANSPONDER_NONE) {
2145             break;
2146         }
2147
2148         if (bytesRemaining != transponderDataSize) {
2149             return MSP_RESULT_ERROR;
2150         }
2151
2152         if (provider != transponderConfig()->provider) {
2153             transponderStopRepeating();
2154         }
2155
2156         memset(transponderConfigMutable()->data, 0, sizeof(transponderConfig()->data));
2157
2158         for (unsigned int i = 0; i < transponderDataSize; i++) {
2159             transponderConfigMutable()->data[i] = sbufReadU8(src);
2160         }
2161         transponderUpdateData();
2162         break;
2163     }
2164 #endif
2165
2166     case MSP_SET_VOLTAGE_METER_CONFIG: {
2167         int8_t id = sbufReadU8(src);
2168
2169         //
2170         // find and configure an ADC voltage sensor
2171         //
2172         int8_t voltageSensorADCIndex;
2173         for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) {
2174             if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) {
2175                 break;
2176             }
2177         }
2178
2179         if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) {
2180             voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src);
2181             voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src);
2182             voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src);
2183         } else {
2184             // if we had any other types of voltage sensor to configure, this is where we'd do it.
2185             sbufReadU8(src);
2186             sbufReadU8(src);
2187             sbufReadU8(src);
2188         }
2189         break;
2190     }
2191
2192     case MSP_SET_CURRENT_METER_CONFIG: {
2193         int id = sbufReadU8(src);
2194
2195         switch (id) {
2196             case CURRENT_METER_ID_BATTERY_1:
2197                 currentSensorADCConfigMutable()->scale = sbufReadU16(src);
2198                 currentSensorADCConfigMutable()->offset = sbufReadU16(src);
2199                 break;
2200 #ifdef USE_VIRTUAL_CURRENT_METER
2201             case CURRENT_METER_ID_VIRTUAL_1:
2202                 currentSensorVirtualConfigMutable()->scale = sbufReadU16(src);
2203                 currentSensorVirtualConfigMutable()->offset = sbufReadU16(src);
2204                 break;
2205 #endif
2206             default:
2207                 sbufReadU16(src);
2208                 sbufReadU16(src);
2209                 break;
2210         }
2211         break;
2212     }
2213
2214     case MSP_SET_BATTERY_CONFIG:
2215         batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src);      // vbatlevel_warn1 in MWC2.3 GUI
2216         batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src);      // vbatlevel_warn2 in MWC2.3 GUI
2217         batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src);  // vbatlevel when buzzer starts to alert
2218         batteryConfigMutable()->batteryCapacity = sbufReadU16(src);
2219         batteryConfigMutable()->voltageMeterSource = sbufReadU8(src);
2220         batteryConfigMutable()->currentMeterSource = sbufReadU8(src);
2221         break;
2222
2223 #if defined(USE_OSD) || defined (USE_OSD_SLAVE)
2224     case MSP_SET_OSD_CONFIG:
2225         {
2226             const uint8_t addr = sbufReadU8(src);
2227
2228             if ((int8_t)addr == -1) {
2229                 /* Set general OSD settings */
2230 #ifdef USE_MAX7456
2231                 vcdProfileMutable()->video_system = sbufReadU8(src);
2232 #else
2233                 sbufReadU8(src); // Skip video system
2234 #endif
2235 #if defined(USE_OSD)
2236                 osdConfigMutable()->units = sbufReadU8(src);
2237
2238                 // Alarms
2239                 osdConfigMutable()->rssi_alarm = sbufReadU8(src);
2240                 osdConfigMutable()->cap_alarm = sbufReadU16(src);
2241                 sbufReadU16(src); // Skip unused (previously fly timer)
2242                 osdConfigMutable()->alt_alarm = sbufReadU16(src);
2243
2244                 if (sbufBytesRemaining(src) >= 2) {
2245                     /* Enabled warnings */
2246                     osdConfigMutable()->enabledWarnings = sbufReadU16(src);
2247                 }
2248 #endif
2249             } else if ((int8_t)addr == -2) {
2250 #if defined(USE_OSD)
2251                 // Timers
2252                 uint8_t index = sbufReadU8(src);
2253                 if (index > OSD_TIMER_COUNT) {
2254                   return MSP_RESULT_ERROR;
2255                 }
2256                 osdConfigMutable()->timers[index] = sbufReadU16(src);
2257 #endif
2258                 return MSP_RESULT_ERROR;
2259             } else {
2260 #if defined(USE_OSD)
2261                 const uint16_t value = sbufReadU16(src);
2262
2263                 /* Get screen index, 0 is post flight statistics, 1 and above are in flight OSD screens */
2264                 const uint8_t screen = (sbufBytesRemaining(src) >= 1) ? sbufReadU8(src) : 1;
2265
2266                 if (screen == 0 && addr < OSD_STAT_COUNT) {
2267                     /* Set statistic item enable */
2268                     osdStatSetState(addr, (value != 0));
2269                 } else if (addr < OSD_ITEM_COUNT) {
2270                     /* Set element positions */
2271                     osdConfigMutable()->item_pos[addr] = value;
2272                 } else {
2273                   return MSP_RESULT_ERROR;
2274                 }
2275 #else
2276                 return MSP_RESULT_ERROR;
2277 #endif
2278             }
2279         }
2280         break;
2281
2282     case MSP_OSD_CHAR_WRITE:
2283 #ifdef USE_MAX7456
2284         {
2285             uint8_t font_data[64];
2286             const uint8_t addr = sbufReadU8(src);
2287             for (int i = 0; i < 54; i++) {
2288                 font_data[i] = sbufReadU8(src);
2289             }
2290             // !!TODO - replace this with a device independent implementation
2291             max7456WriteNvm(addr, font_data);
2292         }
2293         break;
2294 #else
2295         return MSP_RESULT_ERROR;
2296 #endif
2297 #endif // OSD || USE_OSD_SLAVE
2298
2299     default:
2300         return mspProcessInCommand(cmdMSP, src);
2301     }
2302     return MSP_RESULT_ACK;
2303 }
2304
2305 /*
2306  * Returns MSP_RESULT_ACK, MSP_RESULT_ERROR or MSP_RESULT_NO_REPLY
2307  */
2308 mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
2309 {
2310     int ret = MSP_RESULT_ACK;
2311     sbuf_t *dst = &reply->buf;
2312     sbuf_t *src = &cmd->buf;
2313     const uint8_t cmdMSP = cmd->cmd;
2314     // initialize reply by default
2315     reply->cmd = cmd->cmd;
2316
2317     if (mspCommonProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
2318         ret = MSP_RESULT_ACK;
2319     } else if (mspProcessOutCommand(cmdMSP, dst)) {
2320         ret = MSP_RESULT_ACK;
2321 #ifndef USE_OSD_SLAVE
2322     } else if ((ret = mspFcProcessOutCommandWithArg(cmdMSP, src, dst)) != MSP_RESULT_CMD_UNKNOWN) {
2323         /* ret */;
2324 #endif
2325 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
2326     } else if (cmdMSP == MSP_SET_4WAY_IF) {
2327         mspFc4waySerialCommand(dst, src, mspPostProcessFn);
2328         ret = MSP_RESULT_ACK;
2329 #endif
2330 #ifdef USE_FLASHFS
2331     } else if (cmdMSP == MSP_DATAFLASH_READ) {
2332         mspFcDataFlashReadCommand(dst, src);
2333         ret = MSP_RESULT_ACK;
2334 #endif
2335     } else {
2336         ret = mspCommonProcessInCommand(cmdMSP, src);
2337     }
2338     reply->result = ret;
2339     return ret;
2340 }
2341
2342 void mspFcProcessReply(mspPacket_t *reply)
2343 {
2344     sbuf_t *src = &reply->buf;
2345     UNUSED(src); // potentially unused depending on compile options.
2346
2347     switch (reply->cmd) {
2348 #ifndef OSD_SLAVE
2349     case MSP_ANALOG:
2350         {
2351             uint8_t batteryVoltage = sbufReadU8(src);
2352             uint16_t mAhDrawn = sbufReadU16(src);
2353             uint16_t rssi = sbufReadU16(src);
2354             uint16_t amperage = sbufReadU16(src);
2355
2356             UNUSED(rssi);
2357             UNUSED(batteryVoltage);
2358             UNUSED(amperage);
2359             UNUSED(mAhDrawn);
2360
2361 #ifdef USE_MSP_CURRENT_METER
2362             currentMeterMSPSet(amperage, mAhDrawn);
2363 #endif
2364         }
2365         break;
2366 #endif
2367
2368 #ifdef USE_OSD_SLAVE
2369     case MSP_DISPLAYPORT:
2370         {
2371             osdSlaveIsLocked = true; // lock it as soon as a MSP_DISPLAYPORT message is received to prevent accidental CLI/DFU mode.
2372
2373             const int subCmd = sbufReadU8(src);
2374
2375             switch (subCmd) {
2376             case 0: // HEARTBEAT
2377                 //debug[0]++;
2378                 osdSlaveHeartbeat();
2379                 break;
2380             case 1: // RELEASE
2381                 //debug[1]++;
2382                 break;
2383             case 2: // CLEAR
2384                 //debug[2]++;
2385                 osdSlaveClearScreen();
2386                 break;
2387             case 3:
2388                 {
2389                     //debug[3]++;
2390 #define MSP_OSD_MAX_STRING_LENGTH 30 // FIXME move this
2391                     const uint8_t y = sbufReadU8(src); // row
2392                     const uint8_t x = sbufReadU8(src); // column
2393                     sbufReadU8(src); // reserved
2394                     char buf[MSP_OSD_MAX_STRING_LENGTH + 1];
2395                     const int len = MIN(sbufBytesRemaining(src), MSP_OSD_MAX_STRING_LENGTH);
2396                     sbufReadData(src, &buf, len);
2397                     buf[len] = 0;
2398                     osdSlaveWrite(x, y, buf);
2399                 }
2400                 break;
2401             case 4:
2402                 osdSlaveDrawScreen();
2403                 break;
2404             }
2405         }
2406         break;
2407 #endif
2408     }
2409 }
2410
2411 void mspInit(void)
2412 {
2413 #ifndef USE_OSD_SLAVE
2414     initActiveBoxIds();
2415 #endif
2416 }