src/main/fc/fc_msp.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <ctype.h>
  19 #include <stdbool.h>
  20 #include <stdint.h>
  21 #include <string.h>
  22 #include <math.h>
  23
  24 #include "common/log.h" //for MSP_SIMULATOR
  25 #include "platform.h"
  26
  27 #include "blackbox/blackbox.h"
  28
  29 #include "build/debug.h"
  30 #include "build/version.h"
  31
  32 #include "common/axis.h"
  33 #include "common/color.h"
  34 #include "common/maths.h"
  35 #include "common/streambuf.h"
  36 #include "common/bitarray.h"
  37 #include "common/time.h"
  38 #include "common/utils.h"
  39 #include "programming/global_variables.h"
  40 #include "programming/pid.h"
  41
  42 #include "config/parameter_group_ids.h"
  43
  44 #include "drivers/accgyro/accgyro.h"
  45 #include "drivers/compass/compass.h"
  46 #include "drivers/compass/compass_msp.h"
  47 #include "drivers/barometer/barometer_msp.h"
  48 #include "drivers/pitotmeter/pitotmeter_msp.h"
  49 #include "sensors/battery_sensor_fake.h"
  50 #include "drivers/bus_i2c.h"
  51 #include "drivers/display.h"
  52 #include "drivers/flash.h"
  53 #include "drivers/osd.h"
  54 #include "drivers/osd_symbols.h"
  55 #include "drivers/pwm_mapping.h"
  56 #include "drivers/sdcard/sdcard.h"
  57 #include "drivers/serial.h"
  58 #include "drivers/system.h"
  59 #include "drivers/time.h"
  60 #include "drivers/timer.h"
  61 #include "drivers/vtx_common.h"
  62
  63 #include "fc/fc_core.h"
  64 #include "fc/config.h"
  65 #include "fc/controlrate_profile.h"
  66 #include "fc/fc_msp.h"
  67 #include "fc/fc_msp_box.h"
  68 #include "fc/firmware_update.h"
  69 #include "fc/rc_adjustments.h"
  70 #include "fc/rc_controls.h"
  71 #include "fc/rc_modes.h"
  72 #include "fc/runtime_config.h"
  73 #include "fc/settings.h"
  74
  75 #include "flight/failsafe.h"
  76 #include "flight/imu.h"
  77 #include "flight/mixer_profile.h"
  78 #include "flight/mixer.h"
  79 #include "flight/pid.h"
  80 #include "flight/servos.h"
  81 #include "flight/ez_tune.h"
  82
  83 #include "config/config_eeprom.h"
  84 #include "config/feature.h"
  85
  86 #include "io/asyncfatfs/asyncfatfs.h"
  87 #include "io/flashfs.h"
  88 #include "io/gps.h"
  89 #include "io/opflow.h"
  90 #include "io/rangefinder.h"
  91 #include "io/ledstrip.h"
  92 #include "io/osd.h"
  93 #include "io/serial.h"
  94 #include "io/serial_4way.h"
  95 #include "io/vtx.h"
  96 #include "io/vtx_string.h"
  97 #include "io/gps_private.h"  //for MSP_SIMULATOR
  98
  99 #include "msp/msp.h"
 100 #include "msp/msp_protocol.h"
 101 #include "msp/msp_serial.h"
 102
 103 #include "navigation/navigation.h"
 104 #include "navigation/navigation_private.h" //for MSP_SIMULATOR
 105 #include "navigation/navigation_pos_estimator_private.h" //for MSP_SIMULATOR
 106
 107 #include "rx/rx.h"
 108 #include "rx/msp.h"
 109
 110 #include "scheduler/scheduler.h"
 111
 112 #include "sensors/boardalignment.h"
 113 #include "sensors/sensors.h"
 114 #include "sensors/diagnostics.h"
 115 #include "sensors/battery.h"
 116 #include "sensors/rangefinder.h"
 117 #include "sensors/acceleration.h"
 118 #include "sensors/barometer.h"
 119 #include "sensors/pitotmeter.h"
 120 #include "sensors/compass.h"
 121 #include "sensors/gyro.h"
 122 #include "sensors/opflow.h"
 123 #include "sensors/temperature.h"
 124 #include "sensors/esc_sensor.h"
 125
 126 #include "telemetry/telemetry.h"
 127
 128 #ifdef USE_HARDWARE_REVISION_DETECTION
 129 #include "hardware_revision.h"
 130 #endif
 131
 132 extern timeDelta_t cycleTime; // FIXME dependency on mw.c
 133
 134 static const char * const flightControllerIdentifier = INAV_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
 135 static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
 136
 137 // from mixer.c
 138 extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 139
 140 static const char pidnames[] =
 141     "ROLL;"
 142     "PITCH;"
 143     "YAW;"
 144     "ALT;"
 145     "Pos;"
 146     "PosR;"
 147     "NavR;"
 148     "LEVEL;"
 149     "MAG;"
 150     "VEL;";
 151
 152 typedef enum {
 153     MSP_SDCARD_STATE_NOT_PRESENT = 0,
 154     MSP_SDCARD_STATE_FATAL       = 1,
 155     MSP_SDCARD_STATE_CARD_INIT   = 2,
 156     MSP_SDCARD_STATE_FS_INIT     = 3,
 157     MSP_SDCARD_STATE_READY       = 4
 158 } mspSDCardState_e;
 159
 160 typedef enum {
 161     MSP_SDCARD_FLAG_SUPPORTTED   = 1
 162 } mspSDCardFlags_e;
 163
 164 typedef enum {
 165     MSP_FLASHFS_BIT_READY        = 1,
 166     MSP_FLASHFS_BIT_SUPPORTED    = 2
 167 } mspFlashfsFlags_e;
 168
 169 typedef enum {
 170     MSP_PASSTHROUGH_SERIAL_ID          = 0xFD,
 171     MSP_PASSTHROUGH_SERIAL_FUNCTION_ID = 0xFE,
 172     MSP_PASSTHROUGH_ESC_4WAY           = 0xFF,
 173  } mspPassthroughType_e;
 174
 175 static uint8_t mspPassthroughMode;
 176 static uint8_t mspPassthroughArgument;
 177
 178 static serialPort_t *mspFindPassthroughSerialPort(void)
 179  {
 180     serialPortUsage_t *portUsage = NULL;
 181
 182     switch (mspPassthroughMode) {
 183     case MSP_PASSTHROUGH_SERIAL_ID:
 184     {
 185         portUsage = findSerialPortUsageByIdentifier(mspPassthroughArgument);
 186         break;
 187     }
 188     case MSP_PASSTHROUGH_SERIAL_FUNCTION_ID:
 189     {
 190         const serialPortConfig_t *portConfig = findSerialPortConfig(1 << mspPassthroughArgument);
 191         if (portConfig) {
 192             portUsage = findSerialPortUsageByIdentifier(portConfig->identifier);
 193         }
 194         break;
 195     }
 196     }
 197     return portUsage ? portUsage->serialPort : NULL;
 198 }
 199
 200 static void mspSerialPassthroughFn(serialPort_t *serialPort)
 201 {
 202     serialPort_t *passthroughPort = mspFindPassthroughSerialPort();
 203     if (passthroughPort && serialPort) {
 204         serialPassthrough(passthroughPort, serialPort, NULL, NULL);
 205     }
 206 }
 207
 208 static void mspFcSetPassthroughCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
 209 {
 210     const unsigned int dataSize = sbufBytesRemaining(src);
 211
 212     if (dataSize == 0) {
 213         // Legacy format
 214         mspPassthroughMode = MSP_PASSTHROUGH_ESC_4WAY;
 215     } else {
 216         mspPassthroughMode = sbufReadU8(src);
 217         if (!sbufReadU8Safe(&mspPassthroughArgument, src)) {
 218             mspPassthroughArgument = 0;
 219         }
 220     }
 221
 222     switch (mspPassthroughMode) {
 223     case MSP_PASSTHROUGH_SERIAL_ID:
 224     case MSP_PASSTHROUGH_SERIAL_FUNCTION_ID:
 225          if (mspFindPassthroughSerialPort()) {
 226              if (mspPostProcessFn) {
 227                  *mspPostProcessFn = mspSerialPassthroughFn;
 228              }
 229              sbufWriteU8(dst, 1);
 230          } else {
 231              sbufWriteU8(dst, 0);
 232          }
 233          break;
 234 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
 235     case MSP_PASSTHROUGH_ESC_4WAY:
 236         // get channel number
 237         // switch all motor lines HI
 238         // reply with the count of ESC found
 239         sbufWriteU8(dst, esc4wayInit());
 240
 241         if (mspPostProcessFn) {
 242             *mspPostProcessFn = esc4wayProcess;
 243         }
 244         break;
 245 #endif
 246     default:
 247         sbufWriteU8(dst, 0);
 248     }
 249 }
 250
 251 static void mspRebootFn(serialPort_t *serialPort)
 252 {
 253     UNUSED(serialPort);
 254     fcReboot(false);
 255 }
 256
 257 static void serializeSDCardSummaryReply(sbuf_t *dst)
 258 {
 259 #ifdef USE_SDCARD
 260     uint8_t flags = MSP_SDCARD_FLAG_SUPPORTTED;
 261     uint8_t state;
 262
 263     sbufWriteU8(dst, flags);
 264
 265     // Merge the card and filesystem states together
 266     if (!sdcard_isInserted()) {
 267         state = MSP_SDCARD_STATE_NOT_PRESENT;
 268     } else if (!sdcard_isFunctional()) {
 269         state = MSP_SDCARD_STATE_FATAL;
 270     } else {
 271         switch (afatfs_getFilesystemState()) {
 272             case AFATFS_FILESYSTEM_STATE_READY:
 273                 state = MSP_SDCARD_STATE_READY;
 274                 break;
 275             case AFATFS_FILESYSTEM_STATE_INITIALIZATION:
 276                 if (sdcard_isInitialized()) {
 277                     state = MSP_SDCARD_STATE_FS_INIT;
 278                 } else {
 279                     state = MSP_SDCARD_STATE_CARD_INIT;
 280                 }
 281                 break;
 282             case AFATFS_FILESYSTEM_STATE_FATAL:
 283             case AFATFS_FILESYSTEM_STATE_UNKNOWN:
 284             default:
 285                 state = MSP_SDCARD_STATE_FATAL;
 286                 break;
 287         }
 288     }
 289
 290     sbufWriteU8(dst, state);
 291     sbufWriteU8(dst, afatfs_getLastError());
 292     // Write free space and total space in kilobytes
 293     sbufWriteU32(dst, afatfs_getContiguousFreeSpace() / 1024);
 294     sbufWriteU32(dst, sdcard_getMetadata()->numBlocks / 2); // Block size is half a kilobyte
 295 #else
 296     sbufWriteU8(dst, 0);
 297     sbufWriteU8(dst, 0);
 298     sbufWriteU8(dst, 0);
 299     sbufWriteU32(dst, 0);
 300     sbufWriteU32(dst, 0);
 301 #endif
 302 }
 303
 304 static void serializeDataflashSummaryReply(sbuf_t *dst)
 305 {
 306 #ifdef USE_FLASHFS
 307     const flashGeometry_t *geometry = flashGetGeometry();
 308     sbufWriteU8(dst, flashIsReady() ? 1 : 0);
 309     sbufWriteU32(dst, geometry->sectors);
 310     sbufWriteU32(dst, geometry->totalSize);
 311     sbufWriteU32(dst, flashfsGetOffset()); // Effectively the current number of bytes stored on the volume
 312 #else
 313     sbufWriteU8(dst, 0);
 314     sbufWriteU32(dst, 0);
 315     sbufWriteU32(dst, 0);
 316     sbufWriteU32(dst, 0);
 317 #endif
 318 }
 319
 320 #ifdef USE_FLASHFS
 321 static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, uint16_t size)
 322 {
 323     // Check how much bytes we can read
 324     const int bytesRemainingInBuf = sbufBytesRemaining(dst);
 325     uint16_t readLen = (size > bytesRemainingInBuf) ? bytesRemainingInBuf : size;
 326
 327     // size will be lower than that requested if we reach end of volume
 328     const uint32_t flashfsSize = flashfsGetSize();
 329     if (readLen > flashfsSize - address) {
 330         // truncate the request
 331         readLen = flashfsSize - address;
 332     }
 333
 334     // Write address
 335     sbufWriteU32(dst, address);
 336
 337     // Read into streambuf directly
 338     const int bytesRead = flashfsReadAbs(address, sbufPtr(dst), readLen);
 339     sbufAdvance(dst, bytesRead);
 340 }
 341 #endif
 342
 343 /*
 344  * Returns true if the command was processd, false otherwise.
 345  * May set mspPostProcessFunc to a function to be called once the command has been processed
 346  */
 347 static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
 348 {
 349     switch (cmdMSP) {
 350     case MSP_API_VERSION:
 351         sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
 352         sbufWriteU8(dst, API_VERSION_MAJOR);
 353         sbufWriteU8(dst, API_VERSION_MINOR);
 354         break;
 355
 356     case MSP_FC_VARIANT:
 357         sbufWriteData(dst, flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
 358         break;
 359
 360     case MSP_FC_VERSION:
 361         sbufWriteU8(dst, FC_VERSION_MAJOR);
 362         sbufWriteU8(dst, FC_VERSION_MINOR);
 363         sbufWriteU8(dst, FC_VERSION_PATCH_LEVEL);
 364         break;
 365
 366     case MSP_BOARD_INFO:
 367     {
 368         sbufWriteData(dst, boardIdentifier, BOARD_IDENTIFIER_LENGTH);
 369 #ifdef USE_HARDWARE_REVISION_DETECTION
 370         sbufWriteU16(dst, hardwareRevision);
 371 #else
 372         sbufWriteU16(dst, 0); // No other build targets currently have hardware revision detection.
 373 #endif
 374         // OSD support (for BF compatibility):
 375         // 0 = no OSD
 376         // 1 = OSD slave (not supported in INAV)
 377         // 2 = OSD chip on board
 378 #if defined(USE_OSD)
 379         sbufWriteU8(dst, 2);
 380 #else
 381         sbufWriteU8(dst, 0);
 382 #endif
 383         // Board communication capabilities (uint8)
 384         // Bit 0: 1 iff the board has VCP
 385         // Bit 1: 1 iff the board supports software serial
 386         uint8_t commCapabilities = 0;
 387 #ifdef USE_VCP
 388         commCapabilities |= 1 << 0;
 389 #endif
 390 #if defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)
 391         commCapabilities |= 1 << 1;
 392 #endif
 393         sbufWriteU8(dst, commCapabilities);
 394
 395         sbufWriteU8(dst, strlen(targetName));
 396         sbufWriteData(dst, targetName, strlen(targetName));
 397         break;
 398     }
 399
 400     case MSP_BUILD_INFO:
 401         sbufWriteData(dst, buildDate, BUILD_DATE_LENGTH);
 402         sbufWriteData(dst, buildTime, BUILD_TIME_LENGTH);
 403         sbufWriteData(dst, shortGitRevision, GIT_SHORT_REVISION_LENGTH);
 404         break;
 405
 406     case MSP_SENSOR_STATUS:
 407         sbufWriteU8(dst, isHardwareHealthy() ? 1 : 0);
 408         sbufWriteU8(dst, getHwGyroStatus());
 409         sbufWriteU8(dst, getHwAccelerometerStatus());
 410         sbufWriteU8(dst, getHwCompassStatus());
 411         sbufWriteU8(dst, getHwBarometerStatus());
 412         sbufWriteU8(dst, getHwGPSStatus());
 413         sbufWriteU8(dst, getHwRangefinderStatus());
 414         sbufWriteU8(dst, getHwPitotmeterStatus());
 415         sbufWriteU8(dst, getHwOpticalFlowStatus());
 416         break;
 417
 418     case MSP_ACTIVEBOXES:
 419         {
 420             boxBitmask_t mspBoxModeFlags;
 421             packBoxModeFlags(&mspBoxModeFlags);
 422             sbufWriteData(dst, &mspBoxModeFlags, sizeof(mspBoxModeFlags));
 423         }
 424         break;
 425
 426     case MSP_STATUS_EX:
 427     case MSP_STATUS:
 428         {
 429             boxBitmask_t mspBoxModeFlags;
 430             packBoxModeFlags(&mspBoxModeFlags);
 431
 432             sbufWriteU16(dst, (uint16_t)cycleTime);
 433 #ifdef USE_I2C
 434             sbufWriteU16(dst, i2cGetErrorCounter());
 435 #else
 436             sbufWriteU16(dst, 0);
 437 #endif
 438             sbufWriteU16(dst, packSensorStatus());
 439             sbufWriteData(dst, &mspBoxModeFlags, 4);
 440             sbufWriteU8(dst, getConfigProfile());
 441
 442             if (cmdMSP == MSP_STATUS_EX) {
 443                 sbufWriteU16(dst, averageSystemLoadPercent);
 444                 sbufWriteU16(dst, armingFlags);
 445                 sbufWriteU8(dst, accGetCalibrationAxisFlags());
 446             }
 447         }
 448         break;
 449
 450         case MSP2_INAV_STATUS:
 451         {
 452             // Preserves full arming flags and box modes
 453             boxBitmask_t mspBoxModeFlags;
 454             packBoxModeFlags(&mspBoxModeFlags);
 455
 456             sbufWriteU16(dst, (uint16_t)cycleTime);
 457 #ifdef USE_I2C
 458             sbufWriteU16(dst, i2cGetErrorCounter());
 459 #else
 460             sbufWriteU16(dst, 0);
 461 #endif
 462             sbufWriteU16(dst, packSensorStatus());
 463             sbufWriteU16(dst, averageSystemLoadPercent);
 464             sbufWriteU8(dst, (getConfigBatteryProfile() << 4) | getConfigProfile());
 465             sbufWriteU32(dst, armingFlags);
 466             sbufWriteData(dst, &mspBoxModeFlags, sizeof(mspBoxModeFlags));
 467             sbufWriteU8(dst, getConfigMixerProfile());
 468         }
 469         break;
 470
 471     case MSP_RAW_IMU:
 472         {
 473             for (int i = 0; i < 3; i++) {
 474                 sbufWriteU16(dst, (int16_t)lrintf(acc.accADCf[i] * 512));
 475             }
 476             for (int i = 0; i < 3; i++) {
 477                 sbufWriteU16(dst, gyroRateDps(i));
 478             }
 479             for (int i = 0; i < 3; i++) {
 480 #ifdef USE_MAG
 481                 sbufWriteU16(dst, lrintf(mag.magADC[i]));
 482 #else
 483                 sbufWriteU16(dst, 0);
 484 #endif
 485             }
 486         }
 487         break;
 488
 489     case MSP_SERVO:
 490         sbufWriteData(dst, &servo, MAX_SUPPORTED_SERVOS * 2);
 491         break;
 492     case MSP_SERVO_CONFIGURATIONS:
 493         for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
 494             sbufWriteU16(dst, servoParams(i)->min);
 495             sbufWriteU16(dst, servoParams(i)->max);
 496             sbufWriteU16(dst, servoParams(i)->middle);
 497             sbufWriteU8(dst, servoParams(i)->rate);
 498             sbufWriteU8(dst, 0);
 499             sbufWriteU8(dst, 0);
 500             sbufWriteU8(dst, 255); // used to be forwardFromChannel, not used anymore, send 0xff for compatibility reasons
 501             sbufWriteU32(dst, 0); //Input reversing is not required since it can be done on mixer level
 502         }
 503         break;
 504     case MSP_SERVO_MIX_RULES:
 505         for (int i = 0; i < MAX_SERVO_RULES; i++) {
 506             sbufWriteU8(dst, customServoMixers(i)->targetChannel);
 507             sbufWriteU8(dst, customServoMixers(i)->inputSource);
 508             sbufWriteU16(dst, customServoMixers(i)->rate);
 509             sbufWriteU8(dst, customServoMixers(i)->speed);
 510             sbufWriteU8(dst, 0);
 511             sbufWriteU8(dst, 100);
 512             sbufWriteU8(dst, 0);
 513         }
 514         break;
 515     case MSP2_INAV_SERVO_MIXER:
 516         for (int i = 0; i < MAX_SERVO_RULES; i++) {
 517             sbufWriteU8(dst, customServoMixers(i)->targetChannel);
 518             sbufWriteU8(dst, customServoMixers(i)->inputSource);
 519             sbufWriteU16(dst, customServoMixers(i)->rate);
 520             sbufWriteU8(dst, customServoMixers(i)->speed);
 521         #ifdef USE_PROGRAMMING_FRAMEWORK
 522             sbufWriteU8(dst, customServoMixers(i)->conditionId);
 523         #else
 524             sbufWriteU8(dst, -1);
 525         #endif
 526         }
 527         if(MAX_MIXER_PROFILE_COUNT==1) break;
 528         for (int i = 0; i < MAX_SERVO_RULES; i++) {
 529             sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].targetChannel);
 530             sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].inputSource);
 531             sbufWriteU16(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].rate);
 532             sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].speed);
 533         #ifdef USE_PROGRAMMING_FRAMEWORK
 534             sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].conditionId);
 535         #else
 536             sbufWriteU8(dst, -1);
 537         #endif
 538         }
 539         break;
 540 #ifdef USE_PROGRAMMING_FRAMEWORK
 541     case MSP2_INAV_LOGIC_CONDITIONS:
 542         for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
 543             sbufWriteU8(dst, logicConditions(i)->enabled);
 544             sbufWriteU8(dst, logicConditions(i)->activatorId);
 545             sbufWriteU8(dst, logicConditions(i)->operation);
 546             sbufWriteU8(dst, logicConditions(i)->operandA.type);
 547             sbufWriteU32(dst, logicConditions(i)->operandA.value);
 548             sbufWriteU8(dst, logicConditions(i)->operandB.type);
 549             sbufWriteU32(dst, logicConditions(i)->operandB.value);
 550             sbufWriteU8(dst, logicConditions(i)->flags);
 551         }
 552         break;
 553     case MSP2_INAV_LOGIC_CONDITIONS_STATUS:
 554         for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
 555             sbufWriteU32(dst, logicConditionGetValue(i));
 556         }
 557         break;
 558     case MSP2_INAV_GVAR_STATUS:
 559         for (int i = 0; i < MAX_GLOBAL_VARIABLES; i++) {
 560             sbufWriteU32(dst, gvGet(i));
 561         }
 562         break;
 563     case MSP2_INAV_PROGRAMMING_PID:
 564         for (int i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
 565             sbufWriteU8(dst, programmingPids(i)->enabled);
 566             sbufWriteU8(dst, programmingPids(i)->setpoint.type);
 567             sbufWriteU32(dst, programmingPids(i)->setpoint.value);
 568             sbufWriteU8(dst, programmingPids(i)->measurement.type);
 569             sbufWriteU32(dst, programmingPids(i)->measurement.value);
 570             sbufWriteU16(dst, programmingPids(i)->gains.P);
 571             sbufWriteU16(dst, programmingPids(i)->gains.I);
 572             sbufWriteU16(dst, programmingPids(i)->gains.D);
 573             sbufWriteU16(dst, programmingPids(i)->gains.FF);
 574         }
 575         break;
 576     case MSP2_INAV_PROGRAMMING_PID_STATUS:
 577         for (int i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
 578             sbufWriteU32(dst, programmingPidGetOutput(i));
 579         }
 580         break;
 581 #endif
 582     case MSP2_COMMON_MOTOR_MIXER:
 583         for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 584             sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->throttle + 2.0f, 0.0f, 4.0f) * 1000);
 585             sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->roll + 2.0f, 0.0f, 4.0f) * 1000);
 586             sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->pitch + 2.0f, 0.0f, 4.0f) * 1000);
 587             sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->yaw + 2.0f, 0.0f, 4.0f) * 1000);
 588         }
 589         if (MAX_MIXER_PROFILE_COUNT==1) break;
 590         for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 591             sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].throttle + 2.0f, 0.0f, 4.0f) * 1000);
 592             sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].roll + 2.0f, 0.0f, 4.0f) * 1000);
 593             sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].pitch + 2.0f, 0.0f, 4.0f) * 1000);
 594             sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].yaw + 2.0f, 0.0f, 4.0f) * 1000);
 595         }
 596         break;
 597
 598     case MSP_MOTOR:
 599         for (unsigned i = 0; i < 8; i++) {
 600             sbufWriteU16(dst, i < MAX_SUPPORTED_MOTORS ? motor[i] : 0);
 601         }
 602         break;
 603
 604     case MSP_RC:
 605         for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
 606             sbufWriteU16(dst, rxGetChannelValue(i));
 607         }
 608         break;
 609
 610     case MSP_ATTITUDE:
 611         sbufWriteU16(dst, attitude.values.roll);
 612         sbufWriteU16(dst, attitude.values.pitch);
 613         sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
 614         break;
 615
 616     case MSP_ALTITUDE:
 617         sbufWriteU32(dst, lrintf(getEstimatedActualPosition(Z)));
 618         sbufWriteU16(dst, lrintf(getEstimatedActualVelocity(Z)));
 619 #if defined(USE_BARO)
 620         sbufWriteU32(dst, baroGetLatestAltitude());
 621 #else
 622         sbufWriteU32(dst, 0);
 623 #endif
 624         break;
 625
 626     case MSP_SONAR_ALTITUDE:
 627 #ifdef USE_RANGEFINDER
 628         sbufWriteU32(dst, rangefinderGetLatestAltitude());
 629 #else
 630         sbufWriteU32(dst, 0);
 631 #endif
 632         break;
 633
 634     case MSP2_INAV_OPTICAL_FLOW:
 635 #ifdef USE_OPFLOW
 636         sbufWriteU8(dst, opflow.rawQuality);
 637         sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.flowRate[X]));
 638         sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.flowRate[Y]));
 639         sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.bodyRate[X]));
 640         sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.bodyRate[Y]));
 641 #else
 642         sbufWriteU8(dst, 0);
 643         sbufWriteU16(dst, 0);
 644         sbufWriteU16(dst, 0);
 645         sbufWriteU16(dst, 0);
 646         sbufWriteU16(dst, 0);
 647 #endif
 648         break;
 649
 650     case MSP_ANALOG:
 651         sbufWriteU8(dst, (uint8_t)constrain(getBatteryVoltage() / 10, 0, 255));
 652         sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
 653         sbufWriteU16(dst, getRSSI());
 654         sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
 655         break;
 656
 657     case MSP2_INAV_ANALOG:
 658         // Bit 1: battery full, Bit 2: use capacity threshold, Bit 3-4: battery state, Bit 5-8: battery cell count
 659         sbufWriteU8(dst, batteryWasFullWhenPluggedIn() | (batteryUsesCapacityThresholds() << 1) | (getBatteryState() << 2) | (getBatteryCellCount() << 4));
 660         sbufWriteU16(dst, getBatteryVoltage());
 661         sbufWriteU16(dst, getAmperage()); // send amperage in 0.01 A steps
 662         sbufWriteU32(dst, getPower());    // power draw
 663         sbufWriteU32(dst, getMAhDrawn()); // milliamp hours drawn from battery
 664         sbufWriteU32(dst, getMWhDrawn()); // milliWatt hours drawn from battery
 665         sbufWriteU32(dst, getBatteryRemainingCapacity());
 666         sbufWriteU8(dst, calculateBatteryPercentage());
 667         sbufWriteU16(dst, getRSSI());
 668         break;
 669
 670     case MSP_ARMING_CONFIG:
 671         sbufWriteU8(dst, 0);
 672         sbufWriteU8(dst, armingConfig()->disarm_kill_switch);
 673         break;
 674
 675     case MSP_LOOP_TIME:
 676         sbufWriteU16(dst, gyroConfig()->looptime);
 677         break;
 678
 679     case MSP_RC_TUNING:
 680         sbufWriteU8(dst, 100); //rcRate8 kept for compatibity reasons, this setting is no longer used
 681         sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8);
 682         for (int i = 0 ; i < 3; i++) {
 683             sbufWriteU8(dst, currentControlRateProfile->stabilized.rates[i]); // R,P,Y see flight_dynamics_index_t
 684         }
 685         sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID);
 686         sbufWriteU8(dst, currentControlRateProfile->throttle.rcMid8);
 687         sbufWriteU8(dst, currentControlRateProfile->throttle.rcExpo8);
 688         sbufWriteU16(dst, currentControlRateProfile->throttle.pa_breakpoint);
 689         sbufWriteU8(dst, currentControlRateProfile->stabilized.rcYawExpo8);
 690         break;
 691
 692     case MSP2_INAV_RATE_PROFILE:
 693         // throttle
 694         sbufWriteU8(dst, currentControlRateProfile->throttle.rcMid8);
 695         sbufWriteU8(dst, currentControlRateProfile->throttle.rcExpo8);
 696         sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID);
 697         sbufWriteU16(dst, currentControlRateProfile->throttle.pa_breakpoint);
 698
 699         // stabilized
 700         sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8);
 701         sbufWriteU8(dst, currentControlRateProfile->stabilized.rcYawExpo8);
 702         for (uint8_t i = 0 ; i < 3; ++i) {
 703             sbufWriteU8(dst, currentControlRateProfile->stabilized.rates[i]); // R,P,Y see flight_dynamics_index_t
 704         }
 705
 706         // manual
 707         sbufWriteU8(dst, currentControlRateProfile->manual.rcExpo8);
 708         sbufWriteU8(dst, currentControlRateProfile->manual.rcYawExpo8);
 709         for (uint8_t i = 0 ; i < 3; ++i) {
 710             sbufWriteU8(dst, currentControlRateProfile->manual.rates[i]); // R,P,Y see flight_dynamics_index_t
 711         }
 712         break;
 713
 714     case MSP2_PID:
 715         for (int i = 0; i < PID_ITEM_COUNT; i++) {
 716             sbufWriteU8(dst, constrain(pidBank()->pid[i].P, 0, 255));
 717             sbufWriteU8(dst, constrain(pidBank()->pid[i].I, 0, 255));
 718             sbufWriteU8(dst, constrain(pidBank()->pid[i].D, 0, 255));
 719             sbufWriteU8(dst, constrain(pidBank()->pid[i].FF, 0, 255));
 720         }
 721         #ifdef USE_EZ_TUNE
 722             ezTuneUpdate();
 723         #endif
 724         break;
 725
 726     case MSP_PIDNAMES:
 727         for (const char *c = pidnames; *c; c++) {
 728             sbufWriteU8(dst, *c);
 729         }
 730         break;
 731
 732     case MSP_MODE_RANGES:
 733         for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
 734             const modeActivationCondition_t *mac = modeActivationConditions(i);
 735             const box_t *box = findBoxByActiveBoxId(mac->modeId);
 736             sbufWriteU8(dst, box ? box->permanentId : 0);
 737             sbufWriteU8(dst, mac->auxChannelIndex);
 738             sbufWriteU8(dst, mac->range.startStep);
 739             sbufWriteU8(dst, mac->range.endStep);
 740         }
 741         break;
 742
 743     case MSP_ADJUSTMENT_RANGES:
 744         for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
 745             const adjustmentRange_t *adjRange = adjustmentRanges(i);
 746             sbufWriteU8(dst, adjRange->adjustmentIndex);
 747             sbufWriteU8(dst, adjRange->auxChannelIndex);
 748             sbufWriteU8(dst, adjRange->range.startStep);
 749             sbufWriteU8(dst, adjRange->range.endStep);
 750             sbufWriteU8(dst, adjRange->adjustmentFunction);
 751             sbufWriteU8(dst, adjRange->auxSwitchChannelIndex);
 752         }
 753         break;
 754
 755     case MSP_BOXNAMES:
 756         if (!serializeBoxNamesReply(dst)) {
 757             return false;
 758         }
 759         break;
 760
 761     case MSP_BOXIDS:
 762         serializeBoxReply(dst);
 763         break;
 764
 765     case MSP_MISC:
 766         sbufWriteU16(dst, PWM_RANGE_MIDDLE);
 767
 768         sbufWriteU16(dst, 0); // Was min_throttle
 769         sbufWriteU16(dst, motorConfig()->maxthrottle);
 770         sbufWriteU16(dst, motorConfig()->mincommand);
 771
 772         sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle);
 773
 774 #ifdef USE_GPS
 775         sbufWriteU8(dst, gpsConfig()->provider); // gps_type
 776         sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
 777         sbufWriteU8(dst, gpsConfig()->sbasMode); // gps_ubx_sbas
 778 #else
 779         sbufWriteU8(dst, 0); // gps_type
 780         sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
 781         sbufWriteU8(dst, 0); // gps_ubx_sbas
 782 #endif
 783         sbufWriteU8(dst, 0); // multiwiiCurrentMeterOutput
 784         sbufWriteU8(dst, rxConfig()->rssi_channel);
 785         sbufWriteU8(dst, 0);
 786
 787 #ifdef USE_MAG
 788         sbufWriteU16(dst, compassConfig()->mag_declination / 10);
 789 #else
 790         sbufWriteU16(dst, 0);
 791 #endif
 792
 793 #ifdef USE_ADC
 794         sbufWriteU8(dst, batteryMetersConfig()->voltage.scale / 10);
 795         sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10);
 796         sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10);
 797         sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10);
 798 #else
 799         sbufWriteU8(dst, 0);
 800         sbufWriteU8(dst, 0);
 801         sbufWriteU8(dst, 0);
 802         sbufWriteU8(dst, 0);
 803 #endif
 804         break;
 805
 806     case MSP2_INAV_MISC:
 807         sbufWriteU16(dst, PWM_RANGE_MIDDLE);
 808
 809         sbufWriteU16(dst, 0); //Was min_throttle
 810         sbufWriteU16(dst, motorConfig()->maxthrottle);
 811         sbufWriteU16(dst, motorConfig()->mincommand);
 812
 813         sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle);
 814
 815 #ifdef USE_GPS
 816         sbufWriteU8(dst, gpsConfig()->provider); // gps_type
 817         sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
 818         sbufWriteU8(dst, gpsConfig()->sbasMode); // gps_ubx_sbas
 819 #else
 820         sbufWriteU8(dst, 0); // gps_type
 821         sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
 822         sbufWriteU8(dst, 0); // gps_ubx_sbas
 823 #endif
 824         sbufWriteU8(dst, rxConfig()->rssi_channel);
 825
 826 #ifdef USE_MAG
 827         sbufWriteU16(dst, compassConfig()->mag_declination / 10);
 828 #else
 829         sbufWriteU16(dst, 0);
 830 #endif
 831
 832 #ifdef USE_ADC
 833         sbufWriteU16(dst, batteryMetersConfig()->voltage.scale);
 834         sbufWriteU8(dst, batteryMetersConfig()->voltageSource);
 835         sbufWriteU8(dst, currentBatteryProfile->cells);
 836         sbufWriteU16(dst, currentBatteryProfile->voltage.cellDetect);
 837         sbufWriteU16(dst, currentBatteryProfile->voltage.cellMin);
 838         sbufWriteU16(dst, currentBatteryProfile->voltage.cellMax);
 839         sbufWriteU16(dst, currentBatteryProfile->voltage.cellWarning);
 840 #else
 841         sbufWriteU16(dst, 0);
 842         sbufWriteU8(dst, 0);
 843         sbufWriteU8(dst, 0);
 844         sbufWriteU16(dst, 0);
 845         sbufWriteU16(dst, 0);
 846         sbufWriteU16(dst, 0);
 847         sbufWriteU16(dst, 0);
 848 #endif
 849
 850         sbufWriteU32(dst, currentBatteryProfile->capacity.value);
 851         sbufWriteU32(dst, currentBatteryProfile->capacity.warning);
 852         sbufWriteU32(dst, currentBatteryProfile->capacity.critical);
 853         sbufWriteU8(dst, currentBatteryProfile->capacity.unit);
 854         break;
 855
 856     case MSP2_INAV_MISC2:
 857         // Timers
 858         sbufWriteU32(dst, micros() / 1000000); // On time (seconds)
 859         sbufWriteU32(dst, getFlightTime()); // Flight time (seconds)
 860
 861         // Throttle
 862         sbufWriteU8(dst, getThrottlePercent(true)); // Throttle Percent
 863         sbufWriteU8(dst, navigationIsControllingThrottle() ? 1 : 0); // Auto Throttle Flag (0 or 1)
 864
 865         break;
 866
 867     case MSP2_INAV_BATTERY_CONFIG:
 868 #ifdef USE_ADC
 869         sbufWriteU16(dst, batteryMetersConfig()->voltage.scale);
 870         sbufWriteU8(dst, batteryMetersConfig()->voltageSource);
 871         sbufWriteU8(dst, currentBatteryProfile->cells);
 872         sbufWriteU16(dst, currentBatteryProfile->voltage.cellDetect);
 873         sbufWriteU16(dst, currentBatteryProfile->voltage.cellMin);
 874         sbufWriteU16(dst, currentBatteryProfile->voltage.cellMax);
 875         sbufWriteU16(dst, currentBatteryProfile->voltage.cellWarning);
 876 #else
 877         sbufWriteU16(dst, 0);
 878         sbufWriteU8(dst, 0);
 879         sbufWriteU8(dst, 0);
 880         sbufWriteU16(dst, 0);
 881         sbufWriteU16(dst, 0);
 882         sbufWriteU16(dst, 0);
 883         sbufWriteU16(dst, 0);
 884 #endif
 885
 886         sbufWriteU16(dst, batteryMetersConfig()->current.offset);
 887         sbufWriteU16(dst, batteryMetersConfig()->current.scale);
 888
 889         sbufWriteU32(dst, currentBatteryProfile->capacity.value);
 890         sbufWriteU32(dst, currentBatteryProfile->capacity.warning);
 891         sbufWriteU32(dst, currentBatteryProfile->capacity.critical);
 892         sbufWriteU8(dst, currentBatteryProfile->capacity.unit);
 893         break;
 894
 895     case MSP_MOTOR_PINS:
 896         // FIXME This is hardcoded and should not be.
 897         for (int i = 0; i < 8; i++) {
 898             sbufWriteU8(dst, i + 1);
 899         }
 900         break;
 901
 902 #ifdef USE_GPS
 903     case MSP_RAW_GPS:
 904         sbufWriteU8(dst, gpsSol.fixType);
 905         sbufWriteU8(dst, gpsSol.numSat);
 906         sbufWriteU32(dst, gpsSol.llh.lat);
 907         sbufWriteU32(dst, gpsSol.llh.lon);
 908         sbufWriteU16(dst, gpsSol.llh.alt/100); // meters
 909         sbufWriteU16(dst, gpsSol.groundSpeed);
 910         sbufWriteU16(dst, gpsSol.groundCourse);
 911         sbufWriteU16(dst, gpsSol.hdop);
 912         break;
 913
 914     case MSP_COMP_GPS:
 915         sbufWriteU16(dst, GPS_distanceToHome);
 916         sbufWriteU16(dst, GPS_directionToHome);
 917         sbufWriteU8(dst, gpsSol.flags.gpsHeartbeat ? 1 : 0);
 918         break;
 919     case MSP_NAV_STATUS:
 920         sbufWriteU8(dst, NAV_Status.mode);
 921         sbufWriteU8(dst, NAV_Status.state);
 922         sbufWriteU8(dst, NAV_Status.activeWpAction);
 923         sbufWriteU8(dst, NAV_Status.activeWpNumber);
 924         sbufWriteU8(dst, NAV_Status.error);
 925         //sbufWriteU16(dst,  (int16_t)(target_bearing/100));
 926         sbufWriteU16(dst, getHeadingHoldTarget());
 927         break;
 928
 929
 930     case MSP_GPSSVINFO:
 931         /* Compatibility stub - return zero SVs */
 932         sbufWriteU8(dst, 1);
 933
 934         // HDOP
 935         sbufWriteU8(dst, 0);
 936         sbufWriteU8(dst, 0);
 937         sbufWriteU8(dst, gpsSol.hdop / 100);
 938         sbufWriteU8(dst, gpsSol.hdop / 100);
 939         break;
 940
 941     case MSP_GPSSTATISTICS:
 942         sbufWriteU16(dst, gpsStats.lastMessageDt);
 943         sbufWriteU32(dst, gpsStats.errors);
 944         sbufWriteU32(dst, gpsStats.timeouts);
 945         sbufWriteU32(dst, gpsStats.packetCount);
 946         sbufWriteU16(dst, gpsSol.hdop);
 947         sbufWriteU16(dst, gpsSol.eph);
 948         sbufWriteU16(dst, gpsSol.epv);
 949         break;
 950 #endif
 951     case MSP_DEBUG:
 952         // output some useful QA statistics
 953         // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000);         // XX0YY [crystal clock : core clock]
 954
 955         for (int i = 0; i < 4; i++) {
 956             sbufWriteU16(dst, debug[i]);      // 4 variables are here for general monitoring purpose
 957         }
 958         break;
 959
 960     case MSP2_INAV_DEBUG:
 961         for (int i = 0; i < DEBUG32_VALUE_COUNT; i++) {
 962             sbufWriteU32(dst, debug[i]);      // 8 variables are here for general monitoring purpose
 963         }
 964         break;
 965
 966     case MSP_UID:
 967         sbufWriteU32(dst, U_ID_0);
 968         sbufWriteU32(dst, U_ID_1);
 969         sbufWriteU32(dst, U_ID_2);
 970         break;
 971
 972     case MSP_FEATURE:
 973         sbufWriteU32(dst, featureMask());
 974         break;
 975
 976     case MSP_BOARD_ALIGNMENT:
 977         sbufWriteU16(dst, boardAlignment()->rollDeciDegrees);
 978         sbufWriteU16(dst, boardAlignment()->pitchDeciDegrees);
 979         sbufWriteU16(dst, boardAlignment()->yawDeciDegrees);
 980         break;
 981
 982     case MSP_VOLTAGE_METER_CONFIG:
 983 #ifdef USE_ADC
 984         sbufWriteU8(dst, batteryMetersConfig()->voltage.scale / 10);
 985         sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10);
 986         sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10);
 987         sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10);
 988 #else
 989         sbufWriteU8(dst, 0);
 990         sbufWriteU8(dst, 0);
 991         sbufWriteU8(dst, 0);
 992         sbufWriteU8(dst, 0);
 993 #endif
 994         break;
 995
 996     case MSP_CURRENT_METER_CONFIG:
 997         sbufWriteU16(dst, batteryMetersConfig()->current.scale);
 998         sbufWriteU16(dst, batteryMetersConfig()->current.offset);
 999         sbufWriteU8(dst, batteryMetersConfig()->current.type);
1000         sbufWriteU16(dst, constrain(currentBatteryProfile->capacity.value, 0, 0xFFFF));
1001         break;
1002
1003     case MSP_MIXER:
1004         sbufWriteU8(dst, 3); // mixerMode no longer supported, send 3 (QuadX) as fallback
1005         break;
1006
1007     case MSP_RX_CONFIG:
1008         sbufWriteU8(dst, rxConfig()->serialrx_provider);
1009         sbufWriteU16(dst, rxConfig()->maxcheck);
1010         sbufWriteU16(dst, PWM_RANGE_MIDDLE);
1011         sbufWriteU16(dst, rxConfig()->mincheck);
1012 #ifdef USE_SPEKTRUM_BIND
1013         sbufWriteU8(dst, rxConfig()->spektrum_sat_bind);
1014 #else
1015         sbufWriteU8(dst, 0);
1016 #endif
1017         sbufWriteU16(dst, rxConfig()->rx_min_usec);
1018         sbufWriteU16(dst, rxConfig()->rx_max_usec);
1019         sbufWriteU8(dst, 0); // for compatibility with betaflight (rcInterpolation)
1020         sbufWriteU8(dst, 0); // for compatibility with betaflight (rcInterpolationInterval)
1021         sbufWriteU16(dst, 0); // for compatibility with betaflight (airModeActivateThreshold)
1022         sbufWriteU8(dst, 0);
1023         sbufWriteU32(dst, 0);
1024         sbufWriteU8(dst, 0);
1025         sbufWriteU8(dst, 0); // for compatibility with betaflight (fpvCamAngleDegrees)
1026         sbufWriteU8(dst, rxConfig()->receiverType);
1027         break;
1028
1029     case MSP_FAILSAFE_CONFIG:
1030         sbufWriteU8(dst, failsafeConfig()->failsafe_delay);
1031         sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay);
1032         sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle);
1033         sbufWriteU8(dst, 0);    // was failsafe_kill_switch
1034         sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay);
1035         sbufWriteU8(dst, failsafeConfig()->failsafe_procedure);
1036         sbufWriteU8(dst, failsafeConfig()->failsafe_recovery_delay);
1037         sbufWriteU16(dst, failsafeConfig()->failsafe_fw_roll_angle);
1038         sbufWriteU16(dst, failsafeConfig()->failsafe_fw_pitch_angle);
1039         sbufWriteU16(dst, failsafeConfig()->failsafe_fw_yaw_rate);
1040         sbufWriteU16(dst, failsafeConfig()->failsafe_stick_motion_threshold);
1041         sbufWriteU16(dst, failsafeConfig()->failsafe_min_distance);
1042         sbufWriteU8(dst, failsafeConfig()->failsafe_min_distance_procedure);
1043         break;
1044
1045     case MSP_RSSI_CONFIG:
1046         sbufWriteU8(dst, rxConfig()->rssi_channel);
1047         break;
1048
1049     case MSP_RX_MAP:
1050         sbufWriteData(dst, rxConfig()->rcmap, MAX_MAPPABLE_RX_INPUTS);
1051         break;
1052
1053     case MSP2_COMMON_SERIAL_CONFIG:
1054         for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
1055             if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
1056                 continue;
1057             };
1058             sbufWriteU8(dst, serialConfig()->portConfigs[i].identifier);
1059             sbufWriteU32(dst, serialConfig()->portConfigs[i].functionMask);
1060             sbufWriteU8(dst, serialConfig()->portConfigs[i].msp_baudrateIndex);
1061             sbufWriteU8(dst, serialConfig()->portConfigs[i].gps_baudrateIndex);
1062             sbufWriteU8(dst, serialConfig()->portConfigs[i].telemetry_baudrateIndex);
1063             sbufWriteU8(dst, serialConfig()->portConfigs[i].peripheral_baudrateIndex);
1064         }
1065         break;
1066
1067 #ifdef USE_LED_STRIP
1068     case MSP_LED_COLORS:
1069         for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
1070             const hsvColor_t *color = &ledStripConfig()->colors[i];
1071             sbufWriteU16(dst, color->h);
1072             sbufWriteU8(dst, color->s);
1073             sbufWriteU8(dst, color->v);
1074         }
1075         break;
1076
1077     case MSP_LED_STRIP_CONFIG:
1078         for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
1079             const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
1080
1081             uint32_t legacyLedConfig = ledConfig->led_position;
1082             int shiftCount = 8;
1083             legacyLedConfig |= ledConfig->led_function << shiftCount;
1084             shiftCount += 4;
1085             legacyLedConfig |= (ledConfig->led_overlay & 0x3F) << (shiftCount);
1086             shiftCount += 6;
1087             legacyLedConfig |= (ledConfig->led_color) << (shiftCount);
1088             shiftCount += 4;
1089             legacyLedConfig |= (ledConfig->led_direction) << (shiftCount);
1090             shiftCount += 6;
1091             legacyLedConfig |= (ledConfig->led_params) << (shiftCount);
1092
1093             sbufWriteU32(dst, legacyLedConfig);
1094         }
1095         break;
1096
1097     case MSP2_INAV_LED_STRIP_CONFIG_EX:
1098         for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
1099             const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
1100             sbufWriteDataSafe(dst, ledConfig, sizeof(ledConfig_t));
1101         }
1102         break;
1103
1104
1105     case MSP_LED_STRIP_MODECOLOR:
1106         for (int i = 0; i < LED_MODE_COUNT; i++) {
1107             for (int j = 0; j < LED_DIRECTION_COUNT; j++) {
1108                 sbufWriteU8(dst, i);
1109                 sbufWriteU8(dst, j);
1110                 sbufWriteU8(dst, ledStripConfig()->modeColors[i].color[j]);
1111             }
1112         }
1113
1114         for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) {
1115             sbufWriteU8(dst, LED_MODE_COUNT);
1116             sbufWriteU8(dst, j);
1117             sbufWriteU8(dst, ledStripConfig()->specialColors.color[j]);
1118         }
1119         break;
1120 #endif
1121
1122     case MSP_DATAFLASH_SUMMARY:
1123         serializeDataflashSummaryReply(dst);
1124         break;
1125
1126     case MSP_BLACKBOX_CONFIG:
1127         sbufWriteU8(dst, 0); // API no longer supported
1128         sbufWriteU8(dst, 0);
1129         sbufWriteU8(dst, 0);
1130         sbufWriteU8(dst, 0);
1131         break;
1132
1133     case MSP2_BLACKBOX_CONFIG:
1134 #ifdef USE_BLACKBOX
1135         sbufWriteU8(dst, 1); //Blackbox supported
1136         sbufWriteU8(dst, blackboxConfig()->device);
1137         sbufWriteU16(dst, blackboxConfig()->rate_num);
1138         sbufWriteU16(dst, blackboxConfig()->rate_denom);
1139         sbufWriteU32(dst,blackboxConfig()->includeFlags);
1140 #else
1141         sbufWriteU8(dst, 0); // Blackbox not supported
1142         sbufWriteU8(dst, 0);
1143         sbufWriteU16(dst, 0);
1144         sbufWriteU16(dst, 0);
1145 #endif
1146         break;
1147
1148     case MSP_SDCARD_SUMMARY:
1149         serializeSDCardSummaryReply(dst);
1150         break;
1151
1152 #if defined (USE_DJI_HD_OSD) || defined (USE_MSP_DISPLAYPORT)
1153     case MSP_BATTERY_STATE:
1154         // Battery characteristics
1155         sbufWriteU8(dst, constrain(getBatteryCellCount(), 0, 255));
1156         sbufWriteU16(dst, currentBatteryProfile->capacity.value);
1157
1158         // Battery state
1159         sbufWriteU8(dst, constrain(getBatteryVoltage() / 10, 0, 255)); // in 0.1V steps
1160         sbufWriteU16(dst, constrain(getMAhDrawn(), 0, 0xFFFF));
1161         sbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF));
1162
1163         // Battery alerts - used values match Betaflight's/DJI's
1164         sbufWriteU8(dst,  getBatteryState());
1165
1166         // Additional battery voltage field (in 0.01V steps)
1167         sbufWriteU16(dst, getBatteryVoltage());
1168         break;
1169 #endif
1170
1171     case MSP_OSD_CONFIG:
1172 #ifdef USE_OSD
1173         sbufWriteU8(dst, OSD_DRIVER_MAX7456); // OSD supported
1174         // send video system (AUTO/PAL/NTSC)
1175         sbufWriteU8(dst, osdConfig()->video_system);
1176         sbufWriteU8(dst, osdConfig()->units);
1177         sbufWriteU8(dst, osdConfig()->rssi_alarm);
1178         sbufWriteU16(dst, currentBatteryProfile->capacity.warning);
1179         sbufWriteU16(dst, osdConfig()->time_alarm);
1180         sbufWriteU16(dst, osdConfig()->alt_alarm);
1181         sbufWriteU16(dst, osdConfig()->dist_alarm);
1182         sbufWriteU16(dst, osdConfig()->neg_alt_alarm);
1183         for (int i = 0; i < OSD_ITEM_COUNT; i++) {
1184             sbufWriteU16(dst, osdLayoutsConfig()->item_pos[0][i]);
1185         }
1186 #else
1187         sbufWriteU8(dst, OSD_DRIVER_NONE); // OSD not supported
1188 #endif
1189         break;
1190
1191     case MSP_3D:
1192         sbufWriteU16(dst, reversibleMotorsConfig()->deadband_low);
1193         sbufWriteU16(dst, reversibleMotorsConfig()->deadband_high);
1194         sbufWriteU16(dst, reversibleMotorsConfig()->neutral);
1195         break;
1196
1197     case MSP_RC_DEADBAND:
1198         sbufWriteU8(dst, rcControlsConfig()->deadband);
1199         sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
1200         sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband);
1201         sbufWriteU16(dst, rcControlsConfig()->mid_throttle_deadband);
1202         break;
1203
1204     case MSP_SENSOR_ALIGNMENT:
1205         sbufWriteU8(dst, 0); // was gyroConfig()->gyro_align
1206         sbufWriteU8(dst, 0); // was accelerometerConfig()->acc_align
1207 #ifdef USE_MAG
1208         sbufWriteU8(dst, compassConfig()->mag_align);
1209 #else
1210         sbufWriteU8(dst, 0);
1211 #endif
1212 #ifdef USE_OPFLOW
1213         sbufWriteU8(dst, opticalFlowConfig()->opflow_align);
1214 #else
1215         sbufWriteU8(dst, 0);
1216 #endif
1217         break;
1218
1219     case MSP_ADVANCED_CONFIG:
1220         sbufWriteU8(dst, 1);    // gyroConfig()->gyroSyncDenominator
1221         sbufWriteU8(dst, 1);    // BF: masterConfig.pid_process_denom
1222         sbufWriteU8(dst, 1);    // BF: motorConfig()->useUnsyncedPwm
1223         sbufWriteU8(dst, motorConfig()->motorPwmProtocol);
1224         sbufWriteU16(dst, motorConfig()->motorPwmRate);
1225         sbufWriteU16(dst, servoConfig()->servoPwmRate);
1226         sbufWriteU8(dst, 0);
1227         break;
1228
1229     case MSP_FILTER_CONFIG :
1230         sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz);
1231         sbufWriteU16(dst, pidProfile()->dterm_lpf_hz);
1232         sbufWriteU16(dst, pidProfile()->yaw_lpf_hz);
1233         sbufWriteU16(dst, 0); //Was gyroConfig()->gyro_notch_hz
1234         sbufWriteU16(dst, 1); //Was  gyroConfig()->gyro_notch_cutoff
1235         sbufWriteU16(dst, 0); //BF: pidProfile()->dterm_notch_hz
1236         sbufWriteU16(dst, 1); //pidProfile()->dterm_notch_cutoff
1237
1238         sbufWriteU16(dst, 0); //BF: masterConfig.gyro_soft_notch_hz_2
1239         sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_cutoff_2
1240
1241         sbufWriteU16(dst, accelerometerConfig()->acc_notch_hz);
1242         sbufWriteU16(dst, accelerometerConfig()->acc_notch_cutoff);
1243
1244         sbufWriteU16(dst, 0);    //Was gyroConfig()->gyro_stage2_lowpass_hz
1245         break;
1246
1247     case MSP_PID_ADVANCED:
1248         sbufWriteU16(dst, 0); // pidProfile()->rollPitchItermIgnoreRate
1249         sbufWriteU16(dst, 0); // pidProfile()->yawItermIgnoreRate
1250         sbufWriteU16(dst, 0); //pidProfile()->yaw_p_limit
1251         sbufWriteU8(dst, 0); //BF: pidProfile()->deltaMethod
1252         sbufWriteU8(dst, 0); //BF: pidProfile()->vbatPidCompensation
1253         sbufWriteU8(dst, 0); //BF: pidProfile()->setpointRelaxRatio
1254         sbufWriteU8(dst, 0);
1255         sbufWriteU16(dst, pidProfile()->pidSumLimit);
1256         sbufWriteU8(dst, 0); //BF: pidProfile()->itermThrottleGain
1257
1258         /*
1259          * To keep compatibility on MSP frame length level with Betaflight, axis axisAccelerationLimitYaw
1260          * limit will be sent and received in [dps / 10]
1261          */
1262         sbufWriteU16(dst, constrain(pidProfile()->axisAccelerationLimitRollPitch / 10, 0, 65535));
1263         sbufWriteU16(dst, constrain(pidProfile()->axisAccelerationLimitYaw / 10, 0, 65535));
1264         break;
1265
1266     case MSP_INAV_PID:
1267         sbufWriteU8(dst, 0); //Legacy, no longer in use async processing value
1268         sbufWriteU16(dst, 0); //Legacy, no longer in use async processing value
1269         sbufWriteU16(dst, 0); //Legacy, no longer in use async processing value
1270         sbufWriteU8(dst, pidProfile()->heading_hold_rate_limit);
1271         sbufWriteU8(dst, HEADING_HOLD_ERROR_LPF_FREQ);
1272         sbufWriteU16(dst, 0);
1273         sbufWriteU8(dst, gyroConfig()->gyro_lpf);
1274         sbufWriteU8(dst, accelerometerConfig()->acc_lpf_hz);
1275         sbufWriteU8(dst, 0); //reserved
1276         sbufWriteU8(dst, 0); //reserved
1277         sbufWriteU8(dst, 0); //reserved
1278         sbufWriteU8(dst, 0); //reserved
1279         break;
1280
1281     case MSP_SENSOR_CONFIG:
1282         sbufWriteU8(dst, accelerometerConfig()->acc_hardware);
1283 #ifdef USE_BARO
1284         sbufWriteU8(dst, barometerConfig()->baro_hardware);
1285 #else
1286         sbufWriteU8(dst, 0);
1287 #endif
1288 #ifdef USE_MAG
1289         sbufWriteU8(dst, compassConfig()->mag_hardware);
1290 #else
1291         sbufWriteU8(dst, 0);
1292 #endif
1293 #ifdef USE_PITOT
1294         sbufWriteU8(dst, pitotmeterConfig()->pitot_hardware);
1295 #else
1296         sbufWriteU8(dst, 0);
1297 #endif
1298 #ifdef USE_RANGEFINDER
1299         sbufWriteU8(dst, rangefinderConfig()->rangefinder_hardware);
1300 #else
1301         sbufWriteU8(dst, 0);
1302 #endif
1303 #ifdef USE_OPFLOW
1304         sbufWriteU8(dst, opticalFlowConfig()->opflow_hardware);
1305 #else
1306         sbufWriteU8(dst, 0);
1307 #endif
1308         break;
1309
1310     case MSP_NAV_POSHOLD:
1311         sbufWriteU8(dst, navConfig()->general.flags.user_control_mode);
1312         sbufWriteU16(dst, navConfig()->general.max_auto_speed);
1313         sbufWriteU16(dst, navConfig()->general.max_auto_climb_rate);
1314         sbufWriteU16(dst, navConfig()->general.max_manual_speed);
1315         sbufWriteU16(dst, navConfig()->general.max_manual_climb_rate);
1316         sbufWriteU8(dst, navConfig()->mc.max_bank_angle);
1317         sbufWriteU8(dst, navConfig()->mc.althold_throttle_type);
1318         sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle);
1319         break;
1320
1321     case MSP_RTH_AND_LAND_CONFIG:
1322         sbufWriteU16(dst, navConfig()->general.min_rth_distance);
1323         sbufWriteU8(dst, navConfig()->general.flags.rth_climb_first);
1324         sbufWriteU8(dst, navConfig()->general.flags.rth_climb_ignore_emerg);
1325         sbufWriteU8(dst, navConfig()->general.flags.rth_tail_first);
1326         sbufWriteU8(dst, navConfig()->general.flags.rth_allow_landing);
1327         sbufWriteU8(dst, navConfig()->general.flags.rth_alt_control_mode);
1328         sbufWriteU16(dst, navConfig()->general.rth_abort_threshold);
1329         sbufWriteU16(dst, navConfig()->general.rth_altitude);
1330         sbufWriteU16(dst, navConfig()->general.land_minalt_vspd);
1331         sbufWriteU16(dst, navConfig()->general.land_maxalt_vspd);
1332         sbufWriteU16(dst, navConfig()->general.land_slowdown_minalt);
1333         sbufWriteU16(dst, navConfig()->general.land_slowdown_maxalt);
1334         sbufWriteU16(dst, navConfig()->general.emerg_descent_rate);
1335         break;
1336
1337     case MSP_FW_CONFIG:
1338         sbufWriteU16(dst, currentBatteryProfile->nav.fw.cruise_throttle);
1339         sbufWriteU16(dst, currentBatteryProfile->nav.fw.min_throttle);
1340         sbufWriteU16(dst, currentBatteryProfile->nav.fw.max_throttle);
1341         sbufWriteU8(dst, navConfig()->fw.max_bank_angle);
1342         sbufWriteU8(dst, navConfig()->fw.max_climb_angle);
1343         sbufWriteU8(dst, navConfig()->fw.max_dive_angle);
1344         sbufWriteU8(dst, currentBatteryProfile->nav.fw.pitch_to_throttle);
1345         sbufWriteU16(dst, navConfig()->fw.loiter_radius);
1346         break;
1347
1348     case MSP_CALIBRATION_DATA:
1349         sbufWriteU8(dst, accGetCalibrationAxisFlags());
1350         sbufWriteU16(dst, accelerometerConfig()->accZero.raw[X]);
1351         sbufWriteU16(dst, accelerometerConfig()->accZero.raw[Y]);
1352         sbufWriteU16(dst, accelerometerConfig()->accZero.raw[Z]);
1353         sbufWriteU16(dst, accelerometerConfig()->accGain.raw[X]);
1354         sbufWriteU16(dst, accelerometerConfig()->accGain.raw[Y]);
1355         sbufWriteU16(dst, accelerometerConfig()->accGain.raw[Z]);
1356
1357     #ifdef USE_MAG
1358         sbufWriteU16(dst, compassConfig()->magZero.raw[X]);
1359         sbufWriteU16(dst, compassConfig()->magZero.raw[Y]);
1360         sbufWriteU16(dst, compassConfig()->magZero.raw[Z]);
1361     #else
1362         sbufWriteU16(dst, 0);
1363         sbufWriteU16(dst, 0);
1364         sbufWriteU16(dst, 0);
1365     #endif
1366
1367     #ifdef USE_OPFLOW
1368         sbufWriteU16(dst, opticalFlowConfig()->opflow_scale * 256);
1369     #else
1370         sbufWriteU16(dst, 0);
1371     #endif
1372
1373     #ifdef USE_MAG
1374         sbufWriteU16(dst, compassConfig()->magGain[X]);
1375         sbufWriteU16(dst, compassConfig()->magGain[Y]);
1376         sbufWriteU16(dst, compassConfig()->magGain[Z]);
1377     #else
1378         sbufWriteU16(dst, 0);
1379         sbufWriteU16(dst, 0);
1380         sbufWriteU16(dst, 0);
1381     #endif
1382
1383         break;
1384
1385     case MSP_POSITION_ESTIMATION_CONFIG:
1386
1387         sbufWriteU16(dst, positionEstimationConfig()->w_z_baro_p * 100); //     inav_w_z_baro_p float as value * 100
1388         sbufWriteU16(dst, positionEstimationConfig()->w_z_gps_p * 100);  // 2   inav_w_z_gps_p  float as value * 100
1389         sbufWriteU16(dst, positionEstimationConfig()->w_z_gps_v * 100);  // 2   inav_w_z_gps_v  float as value * 100
1390         sbufWriteU16(dst, positionEstimationConfig()->w_xy_gps_p * 100); // 2   inav_w_xy_gps_p float as value * 100
1391         sbufWriteU16(dst, positionEstimationConfig()->w_xy_gps_v * 100); // 2   inav_w_xy_gps_v float as value * 100
1392         sbufWriteU8(dst, gpsConfigMutable()->gpsMinSats);                // 1
1393         sbufWriteU8(dst, positionEstimationConfig()->use_gps_velned);    // 1   inav_use_gps_velned ON/OFF
1394
1395         break;
1396
1397     case MSP_REBOOT:
1398         if (!ARMING_FLAG(ARMED)) {
1399             if (mspPostProcessFn) {
1400                 *mspPostProcessFn = mspRebootFn;
1401             }
1402         }
1403         break;
1404
1405     case MSP_WP_GETINFO:
1406         sbufWriteU8(dst, 0);                        // Reserved for waypoint capabilities
1407         sbufWriteU8(dst, NAV_MAX_WAYPOINTS);        // Maximum number of waypoints supported
1408         sbufWriteU8(dst, isWaypointListValid());    // Is current mission valid
1409         sbufWriteU8(dst, getWaypointCount());       // Number of waypoints in current mission
1410         break;
1411
1412     case MSP_TX_INFO:
1413         sbufWriteU8(dst, getRSSISource());
1414         uint8_t rtcDateTimeIsSet = 0;
1415         dateTime_t dt;
1416         if (rtcGetDateTime(&dt)) {
1417             rtcDateTimeIsSet = 1;
1418         }
1419         sbufWriteU8(dst, rtcDateTimeIsSet);
1420         break;
1421
1422     case MSP_RTC:
1423         {
1424             int32_t seconds = 0;
1425             uint16_t millis = 0;
1426             rtcTime_t t;
1427             if (rtcGet(&t)) {
1428                 seconds = rtcTimeGetSeconds(&t);
1429                 millis = rtcTimeGetMillis(&t);
1430             }
1431             sbufWriteU32(dst, (uint32_t)seconds);
1432             sbufWriteU16(dst, millis);
1433         }
1434         break;
1435
1436     case MSP_VTX_CONFIG:
1437 #ifdef USE_VTX_CONTROL
1438         {
1439             vtxDevice_t *vtxDevice = vtxCommonDevice();
1440             if (vtxDevice) {
1441
1442                 uint8_t deviceType = vtxCommonGetDeviceType(vtxDevice);
1443
1444                 // Return band, channel and power from vtxSettingsConfig_t
1445                 // since the VTX might be configured but temporarily offline.
1446                 uint8_t pitmode = 0;
1447                 vtxCommonGetPitMode(vtxDevice, &pitmode);
1448
1449                 sbufWriteU8(dst, deviceType);
1450                 sbufWriteU8(dst, vtxSettingsConfig()->band);
1451                 sbufWriteU8(dst, vtxSettingsConfig()->channel);
1452                 sbufWriteU8(dst, vtxSettingsConfig()->power);
1453                 sbufWriteU8(dst, pitmode);
1454
1455                 // Betaflight < 4 doesn't send these fields
1456                 sbufWriteU8(dst, vtxCommonDeviceIsReady(vtxDevice) ? 1 : 0);
1457                 sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm);
1458                 // future extensions here...
1459             }
1460             else {
1461                 sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX configured
1462             }
1463         }
1464 #else
1465         sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX configured
1466 #endif
1467         break;
1468
1469     case MSP_NAME:
1470         {
1471             const char *name = systemConfig()->craftName;
1472             while (*name) {
1473                 sbufWriteU8(dst, *name++);
1474             }
1475         }
1476         break;
1477
1478     case MSP2_COMMON_TZ:
1479         sbufWriteU16(dst, (uint16_t)timeConfig()->tz_offset);
1480         sbufWriteU8(dst, (uint8_t)timeConfig()->tz_automatic_dst);
1481         break;
1482
1483     case MSP2_INAV_AIR_SPEED:
1484 #ifdef USE_PITOT
1485         sbufWriteU32(dst, getAirspeedEstimate());
1486 #else
1487         sbufWriteU32(dst, 0);
1488 #endif
1489         break;
1490
1491     case MSP2_INAV_MIXER:
1492         sbufWriteU8(dst, mixerConfig()->motorDirectionInverted);
1493         sbufWriteU8(dst, 0);
1494         sbufWriteU8(dst, mixerConfig()->motorstopOnLow);
1495         sbufWriteU8(dst, mixerConfig()->platformType);
1496         sbufWriteU8(dst, mixerConfig()->hasFlaps);
1497         sbufWriteU16(dst, mixerConfig()->appliedMixerPreset);
1498         sbufWriteU8(dst, MAX_SUPPORTED_MOTORS);
1499         sbufWriteU8(dst, MAX_SUPPORTED_SERVOS);
1500         break;
1501
1502 #if defined(USE_OSD)
1503     case MSP2_INAV_OSD_ALARMS:
1504         sbufWriteU8(dst, osdConfig()->rssi_alarm);
1505         sbufWriteU16(dst, osdConfig()->time_alarm);
1506         sbufWriteU16(dst, osdConfig()->alt_alarm);
1507         sbufWriteU16(dst, osdConfig()->dist_alarm);
1508         sbufWriteU16(dst, osdConfig()->neg_alt_alarm);
1509         sbufWriteU16(dst, osdConfig()->gforce_alarm * 1000);
1510         sbufWriteU16(dst, (int16_t)(osdConfig()->gforce_axis_alarm_min * 1000));
1511         sbufWriteU16(dst, (int16_t)(osdConfig()->gforce_axis_alarm_max * 1000));
1512         sbufWriteU8(dst, osdConfig()->current_alarm);
1513         sbufWriteU16(dst, osdConfig()->imu_temp_alarm_min);
1514         sbufWriteU16(dst, osdConfig()->imu_temp_alarm_max);
1515 #ifdef USE_BARO
1516         sbufWriteU16(dst, osdConfig()->baro_temp_alarm_min);
1517         sbufWriteU16(dst, osdConfig()->baro_temp_alarm_max);
1518 #else
1519         sbufWriteU16(dst, 0);
1520         sbufWriteU16(dst, 0);
1521 #endif
1522         break;
1523
1524     case MSP2_INAV_OSD_PREFERENCES:
1525         sbufWriteU8(dst, osdConfig()->video_system);
1526         sbufWriteU8(dst, osdConfig()->main_voltage_decimals);
1527         sbufWriteU8(dst, osdConfig()->ahi_reverse_roll);
1528         sbufWriteU8(dst, osdConfig()->crosshairs_style);
1529         sbufWriteU8(dst, osdConfig()->left_sidebar_scroll);
1530         sbufWriteU8(dst, osdConfig()->right_sidebar_scroll);
1531         sbufWriteU8(dst, osdConfig()->sidebar_scroll_arrows);
1532         sbufWriteU8(dst, osdConfig()->units);
1533         sbufWriteU8(dst, osdConfig()->stats_energy_unit);
1534         break;
1535
1536 #endif
1537
1538     case MSP2_INAV_OUTPUT_MAPPING:
1539         for (uint8_t i = 0; i < timerHardwareCount; ++i)
1540             if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) {
1541                 sbufWriteU8(dst, timerHardware[i].usageFlags);
1542             }
1543         break;
1544
1545     case MSP2_INAV_OUTPUT_MAPPING_EXT:
1546         for (uint8_t i = 0; i < timerHardwareCount; ++i)
1547             if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) {
1548                 #if defined(SITL_BUILD)
1549                 sbufWriteU8(dst, i);
1550                 #else
1551                 sbufWriteU8(dst, timer2id(timerHardware[i].tim));
1552                 #endif
1553                 sbufWriteU8(dst, timerHardware[i].usageFlags);
1554             }
1555         break;
1556
1557     case MSP2_INAV_MC_BRAKING:
1558 #ifdef USE_MR_BRAKING_MODE
1559         sbufWriteU16(dst, navConfig()->mc.braking_speed_threshold);
1560         sbufWriteU16(dst, navConfig()->mc.braking_disengage_speed);
1561         sbufWriteU16(dst, navConfig()->mc.braking_timeout);
1562         sbufWriteU8(dst, navConfig()->mc.braking_boost_factor);
1563         sbufWriteU16(dst, navConfig()->mc.braking_boost_timeout);
1564         sbufWriteU16(dst, navConfig()->mc.braking_boost_speed_threshold);
1565         sbufWriteU16(dst, navConfig()->mc.braking_boost_disengage_speed);
1566         sbufWriteU8(dst, navConfig()->mc.braking_bank_angle);
1567 #endif
1568         break;
1569
1570 #ifdef USE_TEMPERATURE_SENSOR
1571     case MSP2_INAV_TEMP_SENSOR_CONFIG:
1572         for (uint8_t index = 0; index < MAX_TEMP_SENSORS; ++index) {
1573             const tempSensorConfig_t *sensorConfig = tempSensorConfig(index);
1574             sbufWriteU8(dst, sensorConfig->type);
1575             for (uint8_t addrIndex = 0; addrIndex < 8; ++addrIndex)
1576                 sbufWriteU8(dst, ((uint8_t *)&sensorConfig->address)[addrIndex]);
1577             sbufWriteU16(dst, sensorConfig->alarm_min);
1578             sbufWriteU16(dst, sensorConfig->alarm_max);
1579             sbufWriteU8(dst, sensorConfig->osdSymbol);
1580             for (uint8_t labelIndex = 0; labelIndex < TEMPERATURE_LABEL_LEN; ++labelIndex)
1581                 sbufWriteU8(dst, sensorConfig->label[labelIndex]);
1582         }
1583         break;
1584 #endif
1585
1586 #ifdef USE_TEMPERATURE_SENSOR
1587     case MSP2_INAV_TEMPERATURES:
1588         for (uint8_t index = 0; index < MAX_TEMP_SENSORS; ++index) {
1589             int16_t temperature;
1590             bool valid = getSensorTemperature(index, &temperature);
1591             sbufWriteU16(dst, valid ? temperature : -1000);
1592         }
1593         break;
1594 #endif
1595
1596 #ifdef USE_ESC_SENSOR
1597     case MSP2_INAV_ESC_RPM:
1598         {
1599             uint8_t motorCount = getMotorCount();
1600
1601             for (uint8_t i = 0; i < motorCount; i++){
1602                 const escSensorData_t *escState = getEscTelemetry(i); //Get ESC telemetry
1603                 sbufWriteU32(dst, escState->rpm);
1604             }
1605         }
1606         break;
1607 #endif
1608
1609 #ifdef USE_EZ_TUNE
1610
1611     case MSP2_INAV_EZ_TUNE:
1612         {
1613             sbufWriteU8(dst, ezTune()->enabled);
1614             sbufWriteU16(dst, ezTune()->filterHz);
1615             sbufWriteU8(dst, ezTune()->axisRatio);
1616             sbufWriteU8(dst, ezTune()->response);
1617             sbufWriteU8(dst, ezTune()->damping);
1618             sbufWriteU8(dst, ezTune()->stability);
1619             sbufWriteU8(dst, ezTune()->aggressiveness);
1620             sbufWriteU8(dst, ezTune()->rate);
1621             sbufWriteU8(dst, ezTune()->expo);
1622         }
1623         break;
1624 #endif
1625
1626 #ifdef USE_RATE_DYNAMICS
1627
1628     case MSP2_INAV_RATE_DYNAMICS:
1629         {
1630             sbufWriteU8(dst, currentControlRateProfile->rateDynamics.sensitivityCenter);
1631             sbufWriteU8(dst, currentControlRateProfile->rateDynamics.sensitivityEnd);
1632             sbufWriteU8(dst, currentControlRateProfile->rateDynamics.correctionCenter);
1633             sbufWriteU8(dst, currentControlRateProfile->rateDynamics.correctionEnd);
1634             sbufWriteU8(dst, currentControlRateProfile->rateDynamics.weightCenter);
1635             sbufWriteU8(dst, currentControlRateProfile->rateDynamics.weightEnd);
1636         }
1637         break;
1638
1639 #endif
1640
1641     default:
1642         return false;
1643     }
1644     return true;
1645 }
1646
1647 #ifdef USE_SAFE_HOME
1648 static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
1649 {
1650     const uint8_t safe_home_no = sbufReadU8(src);    // get the home number
1651     if(safe_home_no < MAX_SAFE_HOMES) {
1652         sbufWriteU8(dst, safe_home_no);
1653         sbufWriteU8(dst, safeHomeConfig(safe_home_no)->enabled);
1654         sbufWriteU32(dst, safeHomeConfig(safe_home_no)->lat);
1655         sbufWriteU32(dst, safeHomeConfig(safe_home_no)->lon);
1656         return MSP_RESULT_ACK;
1657     } else {
1658          return MSP_RESULT_ERROR;
1659     }
1660 }
1661 #endif
1662
1663 static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src)
1664 {
1665     const uint8_t idx = sbufReadU8(src);
1666     if(idx < MAX_FW_LAND_APPOACH_SETTINGS) {
1667         sbufWriteU8(dst, idx);
1668         sbufWriteU32(dst, fwAutolandApproachConfig(idx)->approachAlt);
1669         sbufWriteU32(dst, fwAutolandApproachConfig(idx)->landAlt);
1670         sbufWriteU8(dst, fwAutolandApproachConfig(idx)->approachDirection);
1671         sbufWriteU16(dst, fwAutolandApproachConfig(idx)->landApproachHeading1);
1672         sbufWriteU16(dst, fwAutolandApproachConfig(idx)->landApproachHeading2);
1673         sbufWriteU8(dst, fwAutolandApproachConfig(idx)->isSeaLevelRef);
1674         return MSP_RESULT_ACK;
1675     } else {
1676          return MSP_RESULT_ERROR;
1677     }
1678 }
1679
1680 static mspResult_e mspFcLogicConditionCommand(sbuf_t *dst, sbuf_t *src) {
1681     const uint8_t idx = sbufReadU8(src);
1682     if (idx < MAX_LOGIC_CONDITIONS) {
1683         sbufWriteU8(dst, logicConditions(idx)->enabled);
1684         sbufWriteU8(dst, logicConditions(idx)->activatorId);
1685         sbufWriteU8(dst, logicConditions(idx)->operation);
1686         sbufWriteU8(dst, logicConditions(idx)->operandA.type);
1687         sbufWriteU32(dst, logicConditions(idx)->operandA.value);
1688         sbufWriteU8(dst, logicConditions(idx)->operandB.type);
1689         sbufWriteU32(dst, logicConditions(idx)->operandB.value);
1690         sbufWriteU8(dst, logicConditions(idx)->flags);
1691         return MSP_RESULT_ACK;
1692     } else {
1693         return MSP_RESULT_ERROR;
1694     }
1695 }
1696
1697 static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src)
1698 {
1699     const uint8_t msp_wp_no = sbufReadU8(src);    // get the wp number
1700     navWaypoint_t msp_wp;
1701     getWaypoint(msp_wp_no, &msp_wp);
1702     sbufWriteU8(dst, msp_wp_no);      // wp_no
1703     sbufWriteU8(dst, msp_wp.action);  // action (WAYPOINT)
1704     sbufWriteU32(dst, msp_wp.lat);    // lat
1705     sbufWriteU32(dst, msp_wp.lon);    // lon
1706     sbufWriteU32(dst, msp_wp.alt);    // altitude (cm)
1707     sbufWriteU16(dst, msp_wp.p1);     // P1
1708     sbufWriteU16(dst, msp_wp.p2);     // P2
1709     sbufWriteU16(dst, msp_wp.p3);     // P3
1710     sbufWriteU8(dst, msp_wp.flag);    // flags
1711 }
1712
1713 #ifdef USE_FLASHFS
1714 static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
1715 {
1716     const unsigned int dataSize = sbufBytesRemaining(src);
1717     uint16_t readLength;
1718
1719     const uint32_t readAddress = sbufReadU32(src);
1720
1721     // Request payload:
1722     //  uint32_t    - address to read from
1723     //  uint16_t    - size of block to read (optional)
1724     if (dataSize == sizeof(uint32_t) + sizeof(uint16_t)) {
1725         readLength = sbufReadU16(src);
1726     }
1727     else {
1728         readLength = 128;
1729     }
1730
1731     serializeDataflashReadReply(dst, readAddress, readLength);
1732 }
1733 #endif
1734
1735 static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
1736 {
1737     uint8_t tmp_u8;
1738     uint16_t tmp_u16;
1739
1740     const unsigned int dataSize = sbufBytesRemaining(src);
1741
1742     switch (cmdMSP) {
1743     case MSP_SELECT_SETTING:
1744         if (sbufReadU8Safe(&tmp_u8, src) && (!ARMING_FLAG(ARMED)))
1745             setConfigProfileAndWriteEEPROM(tmp_u8);
1746         else
1747             return MSP_RESULT_ERROR;
1748         break;
1749
1750     case MSP_SET_HEAD:
1751         if (sbufReadU16Safe(&tmp_u16, src))
1752             updateHeadingHoldTarget(tmp_u16);
1753         else
1754             return MSP_RESULT_ERROR;
1755         break;
1756
1757 #ifdef USE_RX_MSP
1758     case MSP_SET_RAW_RC:
1759         {
1760             uint8_t channelCount = dataSize / sizeof(uint16_t);
1761             if ((channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) || (dataSize > channelCount * sizeof(uint16_t))) {
1762                 return MSP_RESULT_ERROR;
1763             } else {
1764                 uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
1765                 for (int i = 0; i < channelCount; i++) {
1766                     frame[i] = sbufReadU16(src);
1767                 }
1768                 rxMspFrameReceive(frame, channelCount);
1769             }
1770         }
1771         break;
1772 #endif
1773
1774     case MSP_SET_ARMING_CONFIG:
1775         if (dataSize == 2) {
1776             sbufReadU8(src); //Swallow the first byte, used to be auto_disarm_delay
1777             armingConfigMutable()->disarm_kill_switch = !!sbufReadU8(src);
1778         } else
1779             return MSP_RESULT_ERROR;
1780         break;
1781
1782     case MSP_SET_LOOP_TIME:
1783         if (sbufReadU16Safe(&tmp_u16, src))
1784             gyroConfigMutable()->looptime = tmp_u16;
1785         else
1786             return MSP_RESULT_ERROR;
1787         break;
1788
1789     case MSP2_SET_PID:
1790         if (dataSize == PID_ITEM_COUNT * 4) {
1791             for (int i = 0; i < PID_ITEM_COUNT; i++) {
1792                 pidBankMutable()->pid[i].P = sbufReadU8(src);
1793                 pidBankMutable()->pid[i].I = sbufReadU8(src);
1794                 pidBankMutable()->pid[i].D = sbufReadU8(src);
1795                 pidBankMutable()->pid[i].FF = sbufReadU8(src);
1796             }
1797             schedulePidGainsUpdate();
1798             navigationUsePIDs();
1799         } else
1800             return MSP_RESULT_ERROR;
1801         break;
1802
1803     case MSP_SET_MODE_RANGE:
1804         sbufReadU8Safe(&tmp_u8, src);
1805         if ((dataSize == 5) && (tmp_u8 < MAX_MODE_ACTIVATION_CONDITION_COUNT)) {
1806             modeActivationCondition_t *mac = modeActivationConditionsMutable(tmp_u8);
1807             tmp_u8 = sbufReadU8(src);
1808             const box_t *box = findBoxByPermanentId(tmp_u8);
1809             if (box) {
1810                 mac->modeId = box->boxId;
1811                 mac->auxChannelIndex = sbufReadU8(src);
1812                 mac->range.startStep = sbufReadU8(src);
1813                 mac->range.endStep = sbufReadU8(src);
1814
1815                 updateUsedModeActivationConditionFlags();
1816             } else {
1817                 return MSP_RESULT_ERROR;
1818             }
1819         } else {
1820             return MSP_RESULT_ERROR;
1821         }
1822         break;
1823
1824     case MSP_SET_ADJUSTMENT_RANGE:
1825         sbufReadU8Safe(&tmp_u8, src);
1826         if ((dataSize == 7) && (tmp_u8 < MAX_ADJUSTMENT_RANGE_COUNT)) {
1827             adjustmentRange_t *adjRange = adjustmentRangesMutable(tmp_u8);
1828             tmp_u8 = sbufReadU8(src);
1829             if (tmp_u8 < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) {
1830                 adjRange->adjustmentIndex = tmp_u8;
1831                 adjRange->auxChannelIndex = sbufReadU8(src);
1832                 adjRange->range.startStep = sbufReadU8(src);
1833                 adjRange->range.endStep = sbufReadU8(src);
1834                 adjRange->adjustmentFunction = sbufReadU8(src);
1835                 adjRange->auxSwitchChannelIndex = sbufReadU8(src);
1836             } else {
1837                 return MSP_RESULT_ERROR;
1838             }
1839         } else {
1840             return MSP_RESULT_ERROR;
1841         }
1842         break;
1843
1844     case MSP_SET_RC_TUNING:
1845         if ((dataSize == 10) || (dataSize == 11)) {
1846             sbufReadU8(src); //Read rcRate8, kept for protocol compatibility reasons
1847             // need to cast away const to set controlRateProfile
1848             ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcExpo8 = sbufReadU8(src);
1849             for (int i = 0; i < 3; i++) {
1850                 tmp_u8 = sbufReadU8(src);
1851                 if (i == FD_YAW) {
1852                     ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX);
1853                 }
1854                 else {
1855                     ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX);
1856                 }
1857             }
1858             tmp_u8 = sbufReadU8(src);
1859             ((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = MIN(tmp_u8, SETTING_TPA_RATE_MAX);
1860             ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcMid8 = sbufReadU8(src);
1861             ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcExpo8 = sbufReadU8(src);
1862             ((controlRateConfig_t*)currentControlRateProfile)->throttle.pa_breakpoint = sbufReadU16(src);
1863             if (dataSize > 10) {
1864                 ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcYawExpo8 = sbufReadU8(src);
1865             }
1866
1867             schedulePidGainsUpdate();
1868         } else {
1869             return MSP_RESULT_ERROR;
1870         }
1871         break;
1872
1873     case MSP2_INAV_SET_RATE_PROFILE:
1874         if (dataSize == 15) {
1875             controlRateConfig_t *currentControlRateProfile_p = (controlRateConfig_t*)currentControlRateProfile; // need to cast away const to set controlRateProfile
1876
1877             // throttle
1878             currentControlRateProfile_p->throttle.rcMid8 = sbufReadU8(src);
1879             currentControlRateProfile_p->throttle.rcExpo8 = sbufReadU8(src);
1880             currentControlRateProfile_p->throttle.dynPID = sbufReadU8(src);
1881             currentControlRateProfile_p->throttle.pa_breakpoint = sbufReadU16(src);
1882
1883             // stabilized
1884             currentControlRateProfile_p->stabilized.rcExpo8 = sbufReadU8(src);
1885             currentControlRateProfile_p->stabilized.rcYawExpo8 = sbufReadU8(src);
1886             for (uint8_t i = 0; i < 3; ++i) {
1887                 tmp_u8 = sbufReadU8(src);
1888                 if (i == FD_YAW) {
1889                     currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX);
1890                 } else {
1891                     currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX);
1892                 }
1893             }
1894
1895             // manual
1896             currentControlRateProfile_p->manual.rcExpo8 = sbufReadU8(src);
1897             currentControlRateProfile_p->manual.rcYawExpo8 = sbufReadU8(src);
1898             for (uint8_t i = 0; i < 3; ++i) {
1899                 tmp_u8 = sbufReadU8(src);
1900                 if (i == FD_YAW) {
1901                     currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX);
1902                 } else {
1903                     currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX);
1904                 }
1905             }
1906
1907         } else {
1908             return MSP_RESULT_ERROR;
1909         }
1910         break;
1911
1912     case MSP_SET_MISC:
1913         if (dataSize == 22) {
1914         sbufReadU16(src);   // midrc
1915
1916         sbufReadU16(src); //Was min_throttle
1917         motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
1918         motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX);
1919
1920         currentBatteryProfileMutable->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
1921
1922 #ifdef USE_GPS
1923         gpsConfigMutable()->provider = sbufReadU8(src); // gps_type
1924         sbufReadU8(src); // gps_baudrate
1925         gpsConfigMutable()->sbasMode = sbufReadU8(src); // gps_ubx_sbas
1926 #else
1927         sbufReadU8(src); // gps_type
1928         sbufReadU8(src); // gps_baudrate
1929         sbufReadU8(src); // gps_ubx_sbas
1930 #endif
1931         sbufReadU8(src); // multiwiiCurrentMeterOutput
1932         tmp_u8 = sbufReadU8(src);
1933         if (tmp_u8 <= MAX_SUPPORTED_RC_CHANNEL_COUNT) {
1934             rxConfigMutable()->rssi_channel = tmp_u8;
1935             rxUpdateRSSISource(); // Changing rssi_channel might change the RSSI source
1936         }
1937         sbufReadU8(src);
1938
1939 #ifdef USE_MAG
1940         compassConfigMutable()->mag_declination = sbufReadU16(src) * 10;
1941 #else
1942         sbufReadU16(src);
1943 #endif
1944
1945 #ifdef USE_ADC
1946         batteryMetersConfigMutable()->voltage.scale = sbufReadU8(src) * 10;
1947         currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10;         // vbatlevel_warn1 in MWC2.3 GUI
1948         currentBatteryProfileMutable->voltage.cellMax = sbufReadU8(src) * 10;         // vbatlevel_warn2 in MWC2.3 GUI
1949         currentBatteryProfileMutable->voltage.cellWarning = sbufReadU8(src) * 10;     // vbatlevel when buzzer starts to alert
1950 #else
1951         sbufReadU8(src);
1952         sbufReadU8(src);
1953         sbufReadU8(src);
1954         sbufReadU8(src);
1955 #endif
1956         } else
1957             return MSP_RESULT_ERROR;
1958         break;
1959
1960     case MSP2_INAV_SET_MISC:
1961         if (dataSize == 41) {
1962             sbufReadU16(src);       // midrc
1963
1964             sbufReadU16(src);   // was min_throttle
1965             motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
1966             motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX);
1967
1968             currentBatteryProfileMutable->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
1969
1970 #ifdef USE_GPS
1971             gpsConfigMutable()->provider = sbufReadU8(src); // gps_type
1972             sbufReadU8(src); // gps_baudrate
1973             gpsConfigMutable()->sbasMode = sbufReadU8(src); // gps_ubx_sbas
1974 #else
1975             sbufReadU8(src); // gps_type
1976             sbufReadU8(src); // gps_baudrate
1977             sbufReadU8(src); // gps_ubx_sbas
1978 #endif
1979
1980             tmp_u8 = sbufReadU8(src);
1981             if (tmp_u8 <= MAX_SUPPORTED_RC_CHANNEL_COUNT)
1982                 rxConfigMutable()->rssi_channel = tmp_u8;
1983
1984 #ifdef USE_MAG
1985             compassConfigMutable()->mag_declination = sbufReadU16(src) * 10;
1986 #else
1987             sbufReadU16(src);
1988 #endif
1989
1990 #ifdef USE_ADC
1991             batteryMetersConfigMutable()->voltage.scale = sbufReadU16(src);
1992             batteryMetersConfigMutable()->voltageSource = sbufReadU8(src);
1993             currentBatteryProfileMutable->cells = sbufReadU8(src);
1994             currentBatteryProfileMutable->voltage.cellDetect = sbufReadU16(src);
1995             currentBatteryProfileMutable->voltage.cellMin = sbufReadU16(src);
1996             currentBatteryProfileMutable->voltage.cellMax = sbufReadU16(src);
1997             currentBatteryProfileMutable->voltage.cellWarning = sbufReadU16(src);
1998 #else
1999             sbufReadU16(src);
2000             sbufReadU8(src);
2001             sbufReadU8(src);
2002             sbufReadU16(src);
2003             sbufReadU16(src);
2004             sbufReadU16(src);
2005             sbufReadU16(src);
2006 #endif
2007
2008             currentBatteryProfileMutable->capacity.value = sbufReadU32(src);
2009             currentBatteryProfileMutable->capacity.warning = sbufReadU32(src);
2010             currentBatteryProfileMutable->capacity.critical = sbufReadU32(src);
2011             currentBatteryProfileMutable->capacity.unit = sbufReadU8(src);
2012             if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) {
2013                 batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW;
2014                 return MSP_RESULT_ERROR;
2015             }
2016             if ((currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MWH)) {
2017                 currentBatteryProfileMutable->capacity.unit = BAT_CAPACITY_UNIT_MAH;
2018                 return MSP_RESULT_ERROR;
2019             }
2020         } else
2021             return MSP_RESULT_ERROR;
2022         break;
2023
2024     case MSP2_INAV_SET_BATTERY_CONFIG:
2025         if (dataSize == 29) {
2026 #ifdef USE_ADC
2027             batteryMetersConfigMutable()->voltage.scale = sbufReadU16(src);
2028             batteryMetersConfigMutable()->voltageSource = sbufReadU8(src);
2029             currentBatteryProfileMutable->cells = sbufReadU8(src);
2030             currentBatteryProfileMutable->voltage.cellDetect = sbufReadU16(src);
2031             currentBatteryProfileMutable->voltage.cellMin = sbufReadU16(src);
2032             currentBatteryProfileMutable->voltage.cellMax = sbufReadU16(src);
2033             currentBatteryProfileMutable->voltage.cellWarning = sbufReadU16(src);
2034 #else
2035             sbufReadU16(src);
2036             sbufReadU8(src);
2037             sbufReadU8(src);
2038             sbufReadU16(src);
2039             sbufReadU16(src);
2040             sbufReadU16(src);
2041             sbufReadU16(src);
2042 #endif
2043
2044             batteryMetersConfigMutable()->current.offset = sbufReadU16(src);
2045             batteryMetersConfigMutable()->current.scale = sbufReadU16(src);
2046
2047             currentBatteryProfileMutable->capacity.value = sbufReadU32(src);
2048             currentBatteryProfileMutable->capacity.warning = sbufReadU32(src);
2049             currentBatteryProfileMutable->capacity.critical = sbufReadU32(src);
2050             currentBatteryProfileMutable->capacity.unit = sbufReadU8(src);
2051             if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) {
2052                 batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW;
2053                 return MSP_RESULT_ERROR;
2054             }
2055             if ((currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MWH)) {
2056                 currentBatteryProfileMutable->capacity.unit = BAT_CAPACITY_UNIT_MAH;
2057                 return MSP_RESULT_ERROR;
2058             }
2059         } else
2060             return MSP_RESULT_ERROR;
2061         break;
2062
2063     case MSP_SET_MOTOR:
2064         if (dataSize == 8 * sizeof(uint16_t)) {
2065             for (int i = 0; i < 8; i++) {
2066                 const int16_t disarmed = sbufReadU16(src);
2067                 if (i < MAX_SUPPORTED_MOTORS) {
2068                     motor_disarmed[i] = disarmed;
2069                 }
2070             }
2071         } else
2072             return MSP_RESULT_ERROR;
2073         break;
2074
2075     case MSP_SET_SERVO_CONFIGURATION:
2076         if (dataSize != (1 + 14)) {
2077             return MSP_RESULT_ERROR;
2078         }
2079         tmp_u8 = sbufReadU8(src);
2080         if (tmp_u8 >= MAX_SUPPORTED_SERVOS) {
2081             return MSP_RESULT_ERROR;
2082         } else {
2083             servoParamsMutable(tmp_u8)->min = sbufReadU16(src);
2084             servoParamsMutable(tmp_u8)->max = sbufReadU16(src);
2085             servoParamsMutable(tmp_u8)->middle = sbufReadU16(src);
2086             servoParamsMutable(tmp_u8)->rate = sbufReadU8(src);
2087             sbufReadU8(src);
2088             sbufReadU8(src);
2089             sbufReadU8(src); // used to be forwardFromChannel, ignored
2090             sbufReadU32(src); // used to be reversedSources
2091             servoComputeScalingFactors(tmp_u8);
2092         }
2093         break;
2094
2095     case MSP_SET_SERVO_MIX_RULE:
2096         sbufReadU8Safe(&tmp_u8, src);
2097         if ((dataSize == 9) && (tmp_u8 < MAX_SERVO_RULES)) {
2098             customServoMixersMutable(tmp_u8)->targetChannel = sbufReadU8(src);
2099             customServoMixersMutable(tmp_u8)->inputSource = sbufReadU8(src);
2100             customServoMixersMutable(tmp_u8)->rate = sbufReadU16(src);
2101             customServoMixersMutable(tmp_u8)->speed = sbufReadU8(src);
2102             sbufReadU16(src); //Read 2bytes for min/max and ignore it
2103             sbufReadU8(src); //Read 1 byte for `box` and ignore it
2104             loadCustomServoMixer();
2105         } else
2106             return MSP_RESULT_ERROR;
2107         break;
2108
2109     case MSP2_INAV_SET_SERVO_MIXER:
2110         sbufReadU8Safe(&tmp_u8, src);
2111         if ((dataSize == 7) && (tmp_u8 < MAX_SERVO_RULES)) {
2112             customServoMixersMutable(tmp_u8)->targetChannel = sbufReadU8(src);
2113             customServoMixersMutable(tmp_u8)->inputSource = sbufReadU8(src);
2114             customServoMixersMutable(tmp_u8)->rate = sbufReadU16(src);
2115             customServoMixersMutable(tmp_u8)->speed = sbufReadU8(src);
2116         #ifdef USE_PROGRAMMING_FRAMEWORK
2117             customServoMixersMutable(tmp_u8)->conditionId = sbufReadU8(src);
2118         #else
2119             sbufReadU8(src);
2120         #endif
2121             loadCustomServoMixer();
2122         } else
2123             return MSP_RESULT_ERROR;
2124         break;
2125 #ifdef USE_PROGRAMMING_FRAMEWORK
2126     case MSP2_INAV_SET_LOGIC_CONDITIONS:
2127         sbufReadU8Safe(&tmp_u8, src);
2128         if ((dataSize == 15) && (tmp_u8 < MAX_LOGIC_CONDITIONS)) {
2129             logicConditionsMutable(tmp_u8)->enabled = sbufReadU8(src);
2130             logicConditionsMutable(tmp_u8)->activatorId = sbufReadU8(src);
2131             logicConditionsMutable(tmp_u8)->operation = sbufReadU8(src);
2132             logicConditionsMutable(tmp_u8)->operandA.type = sbufReadU8(src);
2133             logicConditionsMutable(tmp_u8)->operandA.value = sbufReadU32(src);
2134             logicConditionsMutable(tmp_u8)->operandB.type = sbufReadU8(src);
2135             logicConditionsMutable(tmp_u8)->operandB.value = sbufReadU32(src);
2136             logicConditionsMutable(tmp_u8)->flags = sbufReadU8(src);
2137         } else
2138             return MSP_RESULT_ERROR;
2139         break;
2140
2141     case MSP2_INAV_SET_PROGRAMMING_PID:
2142         sbufReadU8Safe(&tmp_u8, src);
2143         if ((dataSize == 20) && (tmp_u8 < MAX_PROGRAMMING_PID_COUNT)) {
2144             programmingPidsMutable(tmp_u8)->enabled = sbufReadU8(src);
2145             programmingPidsMutable(tmp_u8)->setpoint.type = sbufReadU8(src);
2146             programmingPidsMutable(tmp_u8)->setpoint.value = sbufReadU32(src);
2147             programmingPidsMutable(tmp_u8)->measurement.type = sbufReadU8(src);
2148             programmingPidsMutable(tmp_u8)->measurement.value = sbufReadU32(src);
2149             programmingPidsMutable(tmp_u8)->gains.P = sbufReadU16(src);
2150             programmingPidsMutable(tmp_u8)->gains.I = sbufReadU16(src);
2151             programmingPidsMutable(tmp_u8)->gains.D = sbufReadU16(src);
2152             programmingPidsMutable(tmp_u8)->gains.FF = sbufReadU16(src);
2153         } else
2154             return MSP_RESULT_ERROR;
2155         break;
2156 #endif
2157     case MSP2_COMMON_SET_MOTOR_MIXER:
2158         sbufReadU8Safe(&tmp_u8, src);
2159         if ((dataSize == 9) && (tmp_u8 < MAX_SUPPORTED_MOTORS)) {
2160             primaryMotorMixerMutable(tmp_u8)->throttle = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f;
2161             primaryMotorMixerMutable(tmp_u8)->roll = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f;
2162             primaryMotorMixerMutable(tmp_u8)->pitch = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f;
2163             primaryMotorMixerMutable(tmp_u8)->yaw = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f;
2164         } else
2165             return MSP_RESULT_ERROR;
2166         break;
2167
2168     case MSP_SET_3D:
2169         if (dataSize == 6) {
2170             reversibleMotorsConfigMutable()->deadband_low = sbufReadU16(src);
2171             reversibleMotorsConfigMutable()->deadband_high = sbufReadU16(src);
2172             reversibleMotorsConfigMutable()->neutral = sbufReadU16(src);
2173         } else
2174             return MSP_RESULT_ERROR;
2175         break;
2176
2177     case MSP_SET_RC_DEADBAND:
2178         if (dataSize == 5) {
2179             rcControlsConfigMutable()->deadband = sbufReadU8(src);
2180             rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
2181             rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src);
2182             rcControlsConfigMutable()->mid_throttle_deadband = sbufReadU16(src);
2183         } else
2184             return MSP_RESULT_ERROR;
2185         break;
2186
2187     case MSP_SET_RESET_CURR_PID:
2188         PG_RESET_CURRENT(pidProfile);
2189         break;
2190
2191     case MSP_SET_SENSOR_ALIGNMENT:
2192         if (dataSize == 4) {
2193             sbufReadU8(src); // was gyroConfigMutable()->gyro_align
2194             sbufReadU8(src); // was accelerometerConfigMutable()->acc_align
2195 #ifdef USE_MAG
2196             compassConfigMutable()->mag_align = sbufReadU8(src);
2197 #else
2198             sbufReadU8(src);
2199 #endif
2200 #ifdef USE_OPFLOW
2201             opticalFlowConfigMutable()->opflow_align = sbufReadU8(src);
2202 #else
2203             sbufReadU8(src);
2204 #endif
2205         } else
2206             return MSP_RESULT_ERROR;
2207         break;
2208
2209     case MSP_SET_ADVANCED_CONFIG:
2210         if (dataSize == 9) {
2211             sbufReadU8(src);    // gyroConfig()->gyroSyncDenominator
2212             sbufReadU8(src);    // BF: masterConfig.pid_process_denom
2213             sbufReadU8(src);    // BF: motorConfig()->useUnsyncedPwm
2214             motorConfigMutable()->motorPwmProtocol = sbufReadU8(src);
2215             motorConfigMutable()->motorPwmRate = sbufReadU16(src);
2216             servoConfigMutable()->servoPwmRate = sbufReadU16(src);
2217             sbufReadU8(src);    //Was gyroSync
2218         } else
2219             return MSP_RESULT_ERROR;
2220         break;
2221
2222     case MSP_SET_FILTER_CONFIG :
2223         if (dataSize >= 5) {
2224             gyroConfigMutable()->gyro_main_lpf_hz = sbufReadU8(src);
2225             pidProfileMutable()->dterm_lpf_hz = constrain(sbufReadU16(src), 0, 500);
2226             pidProfileMutable()->yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255);
2227             if (dataSize >= 9) {
2228                 sbufReadU16(src); //Was gyroConfigMutable()->gyro_notch_hz
2229                 sbufReadU16(src); //Was gyroConfigMutable()->gyro_notch_cutoff
2230             } else {
2231                 return MSP_RESULT_ERROR;
2232             }
2233             if (dataSize >= 13) {
2234                 sbufReadU16(src);
2235                 sbufReadU16(src);
2236                 pidInitFilters();
2237             } else {
2238                 return MSP_RESULT_ERROR;
2239             }
2240             if (dataSize >= 17) {
2241                 sbufReadU16(src); // Was gyroConfigMutable()->gyro_soft_notch_hz_2
2242                 sbufReadU16(src); // Was gyroConfigMutable()->gyro_soft_notch_cutoff_2
2243             } else {
2244                 return MSP_RESULT_ERROR;
2245             }
2246
2247             if (dataSize >= 21) {
2248                 accelerometerConfigMutable()->acc_notch_hz = constrain(sbufReadU16(src), 0, 255);
2249                 accelerometerConfigMutable()->acc_notch_cutoff = constrain(sbufReadU16(src), 1, 255);
2250             } else {
2251                 return MSP_RESULT_ERROR;
2252             }
2253
2254             if (dataSize >= 22) {
2255                 sbufReadU16(src); //Was gyro_stage2_lowpass_hz
2256             } else {
2257                 return MSP_RESULT_ERROR;
2258             }
2259         } else
2260             return MSP_RESULT_ERROR;
2261         break;
2262
2263     case MSP_SET_PID_ADVANCED:
2264         if (dataSize == 17) {
2265             sbufReadU16(src);   // pidProfileMutable()->rollPitchItermIgnoreRate
2266             sbufReadU16(src);   // pidProfileMutable()->yawItermIgnoreRate
2267             sbufReadU16(src); //pidProfile()->yaw_p_limit
2268
2269             sbufReadU8(src); //BF: pidProfileMutable()->deltaMethod
2270             sbufReadU8(src); //BF: pidProfileMutable()->vbatPidCompensation
2271             sbufReadU8(src); //BF: pidProfileMutable()->setpointRelaxRatio
2272             sbufReadU8(src);
2273             pidProfileMutable()->pidSumLimit = sbufReadU16(src);
2274             sbufReadU8(src); //BF: pidProfileMutable()->itermThrottleGain
2275
2276             /*
2277              * To keep compatibility on MSP frame length level with Betaflight, axis axisAccelerationLimitYaw
2278              * limit will be sent and received in [dps / 10]
2279              */
2280             pidProfileMutable()->axisAccelerationLimitRollPitch = sbufReadU16(src) * 10;
2281             pidProfileMutable()->axisAccelerationLimitYaw = sbufReadU16(src) * 10;
2282         } else
2283             return MSP_RESULT_ERROR;
2284         break;
2285
2286     case MSP_SET_INAV_PID:
2287         if (dataSize == 15) {
2288             sbufReadU8(src);  //Legacy, no longer in use async processing value
2289             sbufReadU16(src);  //Legacy, no longer in use async processing value
2290             sbufReadU16(src);  //Legacy, no longer in use async processing value
2291             pidProfileMutable()->heading_hold_rate_limit = sbufReadU8(src);
2292             sbufReadU8(src); //HEADING_HOLD_ERROR_LPF_FREQ
2293             sbufReadU16(src); //Legacy yaw_jump_prevention_limit
2294             gyroConfigMutable()->gyro_lpf = sbufReadU8(src);
2295             accelerometerConfigMutable()->acc_lpf_hz = sbufReadU8(src);
2296             sbufReadU8(src); //reserved
2297             sbufReadU8(src); //reserved
2298             sbufReadU8(src); //reserved
2299             sbufReadU8(src); //reserved
2300         } else
2301             return MSP_RESULT_ERROR;
2302         break;
2303
2304     case MSP_SET_SENSOR_CONFIG:
2305         if (dataSize == 6) {
2306             accelerometerConfigMutable()->acc_hardware = sbufReadU8(src);
2307 #ifdef USE_BARO
2308             barometerConfigMutable()->baro_hardware = sbufReadU8(src);
2309 #else
2310             sbufReadU8(src);
2311 #endif
2312 #ifdef USE_MAG
2313             compassConfigMutable()->mag_hardware = sbufReadU8(src);
2314 #else
2315             sbufReadU8(src);
2316 #endif
2317 #ifdef USE_PITOT
2318             pitotmeterConfigMutable()->pitot_hardware = sbufReadU8(src);
2319 #else
2320             sbufReadU8(src);
2321 #endif
2322 #ifdef USE_RANGEFINDER
2323             rangefinderConfigMutable()->rangefinder_hardware = sbufReadU8(src);
2324 #else
2325             sbufReadU8(src);        // rangefinder hardware
2326 #endif
2327 #ifdef USE_OPFLOW
2328             opticalFlowConfigMutable()->opflow_hardware = sbufReadU8(src);
2329 #else
2330             sbufReadU8(src);        // optical flow hardware
2331 #endif
2332         } else
2333             return MSP_RESULT_ERROR;
2334         break;
2335
2336     case MSP_SET_NAV_POSHOLD:
2337         if (dataSize == 13) {
2338             navConfigMutable()->general.flags.user_control_mode = sbufReadU8(src);
2339             navConfigMutable()->general.max_auto_speed = sbufReadU16(src);
2340             navConfigMutable()->general.max_auto_climb_rate = sbufReadU16(src);
2341             navConfigMutable()->general.max_manual_speed = sbufReadU16(src);
2342             navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src);
2343             navConfigMutable()->mc.max_bank_angle = sbufReadU8(src);
2344             navConfigMutable()->mc.althold_throttle_type = sbufReadU8(src);
2345             currentBatteryProfileMutable->nav.mc.hover_throttle = sbufReadU16(src);
2346         } else
2347             return MSP_RESULT_ERROR;
2348         break;
2349
2350     case MSP_SET_RTH_AND_LAND_CONFIG:
2351         if (dataSize == 21) {
2352             navConfigMutable()->general.min_rth_distance = sbufReadU16(src);
2353             navConfigMutable()->general.flags.rth_climb_first = sbufReadU8(src);
2354             navConfigMutable()->general.flags.rth_climb_ignore_emerg = sbufReadU8(src);
2355             navConfigMutable()->general.flags.rth_tail_first = sbufReadU8(src);
2356             navConfigMutable()->general.flags.rth_allow_landing = sbufReadU8(src);
2357             navConfigMutable()->general.flags.rth_alt_control_mode = sbufReadU8(src);
2358             navConfigMutable()->general.rth_abort_threshold = sbufReadU16(src);
2359             navConfigMutable()->general.rth_altitude = sbufReadU16(src);
2360             navConfigMutable()->general.land_minalt_vspd = sbufReadU16(src);
2361             navConfigMutable()->general.land_maxalt_vspd = sbufReadU16(src);
2362             navConfigMutable()->general.land_slowdown_minalt = sbufReadU16(src);
2363             navConfigMutable()->general.land_slowdown_maxalt = sbufReadU16(src);
2364             navConfigMutable()->general.emerg_descent_rate = sbufReadU16(src);
2365         } else
2366             return MSP_RESULT_ERROR;
2367         break;
2368
2369     case MSP_SET_FW_CONFIG:
2370         if (dataSize == 12) {
2371             currentBatteryProfileMutable->nav.fw.cruise_throttle = sbufReadU16(src);
2372             currentBatteryProfileMutable->nav.fw.min_throttle = sbufReadU16(src);
2373             currentBatteryProfileMutable->nav.fw.max_throttle = sbufReadU16(src);
2374             navConfigMutable()->fw.max_bank_angle = sbufReadU8(src);
2375             navConfigMutable()->fw.max_climb_angle = sbufReadU8(src);
2376             navConfigMutable()->fw.max_dive_angle = sbufReadU8(src);
2377             currentBatteryProfileMutable->nav.fw.pitch_to_throttle = sbufReadU8(src);
2378             navConfigMutable()->fw.loiter_radius = sbufReadU16(src);
2379         } else
2380             return MSP_RESULT_ERROR;
2381         break;
2382
2383     case MSP_SET_CALIBRATION_DATA:
2384         if (dataSize >= 18) {
2385             accelerometerConfigMutable()->accZero.raw[X] = sbufReadU16(src);
2386             accelerometerConfigMutable()->accZero.raw[Y] = sbufReadU16(src);
2387             accelerometerConfigMutable()->accZero.raw[Z] = sbufReadU16(src);
2388             accelerometerConfigMutable()->accGain.raw[X] = sbufReadU16(src);
2389             accelerometerConfigMutable()->accGain.raw[Y] = sbufReadU16(src);
2390             accelerometerConfigMutable()->accGain.raw[Z] = sbufReadU16(src);
2391
2392 #ifdef USE_MAG
2393             compassConfigMutable()->magZero.raw[X] = sbufReadU16(src);
2394             compassConfigMutable()->magZero.raw[Y] = sbufReadU16(src);
2395             compassConfigMutable()->magZero.raw[Z] = sbufReadU16(src);
2396 #else
2397             sbufReadU16(src);
2398             sbufReadU16(src);
2399             sbufReadU16(src);
2400 #endif
2401 #ifdef USE_OPFLOW
2402             if (dataSize >= 20) {
2403                 opticalFlowConfigMutable()->opflow_scale = sbufReadU16(src) / 256.0f;
2404             }
2405 #endif
2406 #ifdef USE_MAG
2407             if (dataSize >= 22) {
2408                 compassConfigMutable()->magGain[X] = sbufReadU16(src);
2409                 compassConfigMutable()->magGain[Y] = sbufReadU16(src);
2410                 compassConfigMutable()->magGain[Z] = sbufReadU16(src);
2411             }
2412 #else
2413             if (dataSize >= 22) {
2414                 sbufReadU16(src);
2415                 sbufReadU16(src);
2416                 sbufReadU16(src);
2417             }
2418 #endif
2419         } else
2420             return MSP_RESULT_ERROR;
2421         break;
2422
2423     case MSP_SET_POSITION_ESTIMATION_CONFIG:
2424         if (dataSize == 12) {
2425             positionEstimationConfigMutable()->w_z_baro_p = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
2426             positionEstimationConfigMutable()->w_z_gps_p = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
2427             positionEstimationConfigMutable()->w_z_gps_v = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
2428             positionEstimationConfigMutable()->w_xy_gps_p = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
2429             positionEstimationConfigMutable()->w_xy_gps_v = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
2430             gpsConfigMutable()->gpsMinSats = constrain(sbufReadU8(src), 5, 10);
2431             positionEstimationConfigMutable()->use_gps_velned = constrain(sbufReadU8(src), 0, 1);
2432         } else
2433             return MSP_RESULT_ERROR;
2434         break;
2435
2436     case MSP_RESET_CONF:
2437         if (!ARMING_FLAG(ARMED)) {
2438             suspendRxSignal();
2439             resetEEPROM();
2440             writeEEPROM();
2441             readEEPROM();
2442             resumeRxSignal();
2443         } else
2444             return MSP_RESULT_ERROR;
2445         break;
2446
2447     case MSP_ACC_CALIBRATION:
2448         if (!ARMING_FLAG(ARMED))
2449             accStartCalibration();
2450         else
2451             return MSP_RESULT_ERROR;
2452         break;
2453
2454     case MSP_MAG_CALIBRATION:
2455         if (!ARMING_FLAG(ARMED))
2456             ENABLE_STATE(CALIBRATE_MAG);
2457         else
2458             return MSP_RESULT_ERROR;
2459         break;
2460
2461 #ifdef USE_OPFLOW
2462     case MSP2_INAV_OPFLOW_CALIBRATION:
2463         if (!ARMING_FLAG(ARMED))
2464             opflowStartCalibration();
2465         else
2466             return MSP_RESULT_ERROR;
2467         break;
2468 #endif
2469
2470     case MSP_EEPROM_WRITE:
2471         if (!ARMING_FLAG(ARMED)) {
2472             suspendRxSignal();
2473             writeEEPROM();
2474             readEEPROM();
2475             resumeRxSignal();
2476         } else
2477             return MSP_RESULT_ERROR;
2478         break;
2479
2480 #ifdef USE_BLACKBOX
2481     case MSP2_SET_BLACKBOX_CONFIG:
2482         // Don't allow config to be updated while Blackbox is logging
2483         if ((dataSize == 9) && blackboxMayEditConfig()) {
2484             blackboxConfigMutable()->device = sbufReadU8(src);
2485             blackboxConfigMutable()->rate_num = sbufReadU16(src);
2486             blackboxConfigMutable()->rate_denom = sbufReadU16(src);
2487             blackboxConfigMutable()->includeFlags = sbufReadU32(src);
2488         } else
2489             return MSP_RESULT_ERROR;
2490         break;
2491 #endif
2492
2493 #ifdef USE_OSD
2494     case MSP_SET_OSD_CONFIG:
2495         sbufReadU8Safe(&tmp_u8, src);
2496         // set all the other settings
2497         if ((int8_t)tmp_u8 == -1) {
2498             if (dataSize >= 10) {
2499                 osdConfigMutable()->video_system = sbufReadU8(src);
2500                 osdConfigMutable()->units = sbufReadU8(src);
2501                 osdConfigMutable()->rssi_alarm = sbufReadU8(src);
2502                 currentBatteryProfileMutable->capacity.warning = sbufReadU16(src);
2503                 osdConfigMutable()->time_alarm = sbufReadU16(src);
2504                 osdConfigMutable()->alt_alarm = sbufReadU16(src);
2505                 // Won't be read if they weren't provided
2506                 sbufReadU16Safe(&osdConfigMutable()->dist_alarm, src);
2507                 sbufReadU16Safe(&osdConfigMutable()->neg_alt_alarm, src);
2508             } else
2509                 return MSP_RESULT_ERROR;
2510         } else {
2511             // set a position setting
2512             if ((dataSize >= 3) && (tmp_u8 < OSD_ITEM_COUNT)) // tmp_u8 == addr
2513                 osdLayoutsConfigMutable()->item_pos[0][tmp_u8] = sbufReadU16(src);
2514             else
2515                 return MSP_RESULT_ERROR;
2516         }
2517         // Either a element position change or a units change needs
2518         // a full redraw, since an element can change size significantly
2519         // and the old position or the now unused space due to the
2520         // size change need to be erased.
2521         osdStartFullRedraw();
2522         break;
2523
2524     case MSP_OSD_CHAR_WRITE:
2525         if (dataSize >= 55) {
2526             osdCharacter_t chr;
2527             size_t osdCharacterBytes;
2528             uint16_t addr;
2529             if (dataSize >= OSD_CHAR_VISIBLE_BYTES + 2) {
2530                 if (dataSize >= OSD_CHAR_BYTES + 2) {
2531                     // 16 bit address, full char with metadata
2532                     addr = sbufReadU16(src);
2533                     osdCharacterBytes = OSD_CHAR_BYTES;
2534                 } else if (dataSize >= OSD_CHAR_BYTES + 1) {
2535                     // 8 bit address, full char with metadata
2536                     addr = sbufReadU8(src);
2537                     osdCharacterBytes = OSD_CHAR_BYTES;
2538                 } else {
2539                     // 16 bit character address, only visible char bytes
2540                     addr = sbufReadU16(src);
2541                     osdCharacterBytes = OSD_CHAR_VISIBLE_BYTES;
2542                 }
2543             } else {
2544                 // 8 bit character address, only visible char bytes
2545                 addr = sbufReadU8(src);
2546                 osdCharacterBytes = OSD_CHAR_VISIBLE_BYTES;
2547             }
2548             for (unsigned ii = 0; ii < MIN(osdCharacterBytes, sizeof(chr.data)); ii++) {
2549                 chr.data[ii] = sbufReadU8(src);
2550             }
2551             displayPort_t *osdDisplayPort = osdGetDisplayPort();
2552             if (osdDisplayPort) {
2553                 displayWriteFontCharacter(osdDisplayPort, addr, &chr);
2554             }
2555         } else {
2556             return MSP_RESULT_ERROR;
2557         }
2558         break;
2559 #endif // USE_OSD
2560
2561 #ifdef USE_VTX_CONTROL
2562     case MSP_SET_VTX_CONFIG:
2563         if (dataSize >= 2) {
2564             vtxDevice_t *vtxDevice = vtxCommonDevice();
2565             if (vtxDevice) {
2566                 if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) {
2567                     uint16_t newFrequency = sbufReadU16(src);
2568                     if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) {  //value is band and channel
2569                         const uint8_t newBand = (newFrequency / 8) + 1;
2570                         const uint8_t newChannel = (newFrequency % 8) + 1;
2571
2572                         if(vtxSettingsConfig()->band != newBand || vtxSettingsConfig()->channel != newChannel) {
2573                             vtxCommonSetBandAndChannel(vtxDevice, newBand, newChannel);
2574                         }
2575
2576                         vtxSettingsConfigMutable()->band = newBand;
2577                         vtxSettingsConfigMutable()->channel = newChannel;
2578                     }
2579
2580                     if (sbufBytesRemaining(src) > 1) {
2581                         uint8_t newPower = sbufReadU8(src);
2582                         uint8_t currentPower = 0;
2583                         vtxCommonGetPowerIndex(vtxDevice, &currentPower);
2584                         if (newPower != currentPower) {
2585                             vtxCommonSetPowerByIndex(vtxDevice, newPower);
2586                             vtxSettingsConfigMutable()->power = newPower;
2587                         }
2588
2589                         // Delegate pitmode to vtx directly
2590                         const uint8_t newPitmode = sbufReadU8(src);
2591                         uint8_t currentPitmode = 0;
2592                         vtxCommonGetPitMode(vtxDevice, &currentPitmode);
2593                         if (currentPitmode != newPitmode) {
2594                             vtxCommonSetPitMode(vtxDevice, newPitmode);
2595                         }
2596
2597                         if (sbufBytesRemaining(src) > 0) {
2598                             vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src);
2599                         }
2600
2601                         // //////////////////////////////////////////////////////////
2602                         // this code is taken from BF, it's hack for HDZERO VTX MSP frame
2603                         // API version 1.42 - this parameter kept separate since clients may already be supplying
2604                         if (sbufBytesRemaining(src) >= 2) {
2605                             sbufReadU16(src); //skip pitModeFreq
2606                         }
2607
2608                         // API version 1.42 - extensions for non-encoded versions of the band, channel or frequency
2609                         if (sbufBytesRemaining(src) >= 4) {
2610                             uint8_t newBand = sbufReadU8(src);
2611                             const uint8_t newChannel = sbufReadU8(src);
2612                             vtxSettingsConfigMutable()->band = newBand;
2613                             vtxSettingsConfigMutable()->channel = newChannel;
2614                         }
2615
2616                        /* if (sbufBytesRemaining(src) >= 4) {
2617                             sbufRead8(src); // freq_l
2618                             sbufRead8(src); // freq_h
2619                             sbufRead8(src); // band count
2620                             sbufRead8(src); // channel count
2621                         }*/
2622                         // //////////////////////////////////////////////////////////
2623                     }
2624                 }
2625             }
2626         } else {
2627             return MSP_RESULT_ERROR;
2628         }
2629         break;
2630 #endif
2631
2632 #ifdef USE_FLASHFS
2633     case MSP_DATAFLASH_ERASE:
2634         flashfsEraseCompletely();
2635         break;
2636 #endif
2637
2638 #ifdef USE_GPS
2639     case MSP_SET_RAW_GPS:
2640         if (dataSize == 14) {
2641             gpsSol.fixType = sbufReadU8(src);
2642             if (gpsSol.fixType) {
2643                 ENABLE_STATE(GPS_FIX);
2644             } else {
2645                 DISABLE_STATE(GPS_FIX);
2646             }
2647             gpsSol.flags.validVelNE = false;
2648             gpsSol.flags.validVelD = false;
2649             gpsSol.flags.validEPE = false;
2650             gpsSol.flags.validTime = false;
2651             gpsSol.numSat = sbufReadU8(src);
2652             gpsSol.llh.lat = sbufReadU32(src);
2653             gpsSol.llh.lon = sbufReadU32(src);
2654             gpsSol.llh.alt = 100 * sbufReadU16(src); // require cm
2655             gpsSol.groundSpeed = sbufReadU16(src);
2656             gpsSol.velNED[X] = 0;
2657             gpsSol.velNED[Y] = 0;
2658             gpsSol.velNED[Z] = 0;
2659             gpsSol.eph = 100;
2660             gpsSol.epv = 100;
2661             // Feed data to navigation
2662             sensorsSet(SENSOR_GPS);
2663             onNewGPSData();
2664         } else
2665             return MSP_RESULT_ERROR;
2666         break;
2667 #endif
2668
2669     case MSP_SET_WP:
2670         if (dataSize == 21) {
2671             static uint8_t mmIdx = 0;
2672             const uint8_t msp_wp_no = sbufReadU8(src);     // get the waypoint number
2673             navWaypoint_t msp_wp;
2674             msp_wp.action = sbufReadU8(src);    // action
2675             msp_wp.lat = sbufReadU32(src);      // lat
2676             msp_wp.lon = sbufReadU32(src);      // lon
2677             msp_wp.alt = sbufReadU32(src);      // to set altitude (cm)
2678             msp_wp.p1 = sbufReadU16(src);       // P1
2679             msp_wp.p2 = sbufReadU16(src);       // P2
2680             msp_wp.p3 = sbufReadU16(src);       // P3
2681             msp_wp.flag = sbufReadU8(src);      // future: to set nav flag
2682             setWaypoint(msp_wp_no, &msp_wp);
2683
2684             uint8_t fwAppraochStartIdx = 8;
2685 #ifdef USE_SAFE_HOME
2686             fwAppraochStartIdx = MAX_SAFE_HOMES;
2687 #endif
2688             if (msp_wp_no == 0) {
2689                 mmIdx = 0;
2690             } else if (msp_wp.flag == NAV_WP_FLAG_LAST) {
2691                 mmIdx++;
2692             }
2693             resetFwAutolandApproach(fwAppraochStartIdx + mmIdx);
2694         } else
2695             return MSP_RESULT_ERROR;
2696         break;
2697     case MSP2_COMMON_SET_RADAR_POS:
2698         if (dataSize == 19) {
2699             const uint8_t msp_radar_no = MIN(sbufReadU8(src), RADAR_MAX_POIS - 1); // Radar poi number, 0 to 3
2700             radar_pois[msp_radar_no].state = sbufReadU8(src);                      // 0=undefined, 1=armed, 2=lost
2701             radar_pois[msp_radar_no].gps.lat = sbufReadU32(src);                   // lat 10E7
2702             radar_pois[msp_radar_no].gps.lon = sbufReadU32(src);                   // lon 10E7
2703             radar_pois[msp_radar_no].gps.alt = sbufReadU32(src);                   // altitude (cm)
2704             radar_pois[msp_radar_no].heading = sbufReadU16(src);                   // °
2705             radar_pois[msp_radar_no].speed = sbufReadU16(src);                     // cm/s
2706             radar_pois[msp_radar_no].lq = sbufReadU8(src);                         // Link quality, from 0 to 4
2707         } else
2708             return MSP_RESULT_ERROR;
2709         break;
2710
2711     case MSP_SET_FEATURE:
2712         if (dataSize == 4) {
2713             featureClearAll();
2714             featureSet(sbufReadU32(src)); // features bitmap
2715             rxUpdateRSSISource(); // For FEATURE_RSSI_ADC
2716         } else
2717             return MSP_RESULT_ERROR;
2718         break;
2719
2720     case MSP_SET_BOARD_ALIGNMENT:
2721         if (dataSize == 6) {
2722             boardAlignmentMutable()->rollDeciDegrees = sbufReadU16(src);
2723             boardAlignmentMutable()->pitchDeciDegrees = sbufReadU16(src);
2724             boardAlignmentMutable()->yawDeciDegrees = sbufReadU16(src);
2725         } else
2726             return MSP_RESULT_ERROR;
2727         break;
2728
2729     case MSP_SET_VOLTAGE_METER_CONFIG:
2730         if (dataSize == 4) {
2731 #ifdef USE_ADC
2732             batteryMetersConfigMutable()->voltage.scale = sbufReadU8(src) * 10;
2733             currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10;
2734             currentBatteryProfileMutable->voltage.cellMax = sbufReadU8(src) * 10;
2735             currentBatteryProfileMutable->voltage.cellWarning = sbufReadU8(src) * 10;
2736 #else
2737             sbufReadU8(src);
2738             sbufReadU8(src);
2739             sbufReadU8(src);
2740             sbufReadU8(src);
2741 #endif
2742         } else
2743             return MSP_RESULT_ERROR;
2744         break;
2745
2746     case MSP_SET_CURRENT_METER_CONFIG:
2747         if (dataSize == 7) {
2748             batteryMetersConfigMutable()->current.scale = sbufReadU16(src);
2749             batteryMetersConfigMutable()->current.offset = sbufReadU16(src);
2750             batteryMetersConfigMutable()->current.type = sbufReadU8(src);
2751             currentBatteryProfileMutable->capacity.value = sbufReadU16(src);
2752         } else
2753             return MSP_RESULT_ERROR;
2754         break;
2755
2756     case MSP_SET_MIXER:
2757         if (dataSize == 1) {
2758             sbufReadU8(src); //This is ignored, no longer supporting mixerMode
2759             mixerUpdateStateFlags();    // Required for correct preset functionality
2760         } else
2761             return MSP_RESULT_ERROR;
2762         break;
2763
2764     case MSP_SET_RX_CONFIG:
2765         if (dataSize == 24) {
2766             rxConfigMutable()->serialrx_provider = sbufReadU8(src);
2767             rxConfigMutable()->maxcheck = sbufReadU16(src);
2768             sbufReadU16(src); // midrc
2769             rxConfigMutable()->mincheck = sbufReadU16(src);
2770 #ifdef USE_SPEKTRUM_BIND
2771             rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src);
2772 #else
2773             sbufReadU8(src);
2774 #endif
2775             rxConfigMutable()->rx_min_usec = sbufReadU16(src);
2776             rxConfigMutable()->rx_max_usec = sbufReadU16(src);
2777             sbufReadU8(src); // for compatibility with betaflight (rcInterpolation)
2778             sbufReadU8(src); // for compatibility with betaflight (rcInterpolationInterval)
2779             sbufReadU16(src); // for compatibility with betaflight (airModeActivateThreshold)
2780             sbufReadU8(src);
2781             sbufReadU32(src);
2782             sbufReadU8(src);
2783             sbufReadU8(src); // for compatibility with betaflight (fpvCamAngleDegrees)
2784             rxConfigMutable()->receiverType = sbufReadU8(src);              // Won't be modified if buffer is not large enough
2785         } else
2786             return MSP_RESULT_ERROR;
2787         break;
2788
2789     case MSP_SET_FAILSAFE_CONFIG:
2790         if (dataSize == 20) {
2791             failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
2792             failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src);
2793             currentBatteryProfileMutable->failsafe_throttle = sbufReadU16(src);
2794             sbufReadU8(src); // was failsafe_kill_switch
2795             failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src);
2796             failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src);
2797             failsafeConfigMutable()->failsafe_recovery_delay = sbufReadU8(src);
2798             failsafeConfigMutable()->failsafe_fw_roll_angle = (int16_t)sbufReadU16(src);
2799             failsafeConfigMutable()->failsafe_fw_pitch_angle = (int16_t)sbufReadU16(src);
2800             failsafeConfigMutable()->failsafe_fw_yaw_rate = (int16_t)sbufReadU16(src);
2801             failsafeConfigMutable()->failsafe_stick_motion_threshold = sbufReadU16(src);
2802             failsafeConfigMutable()->failsafe_min_distance = sbufReadU16(src);
2803             failsafeConfigMutable()->failsafe_min_distance_procedure = sbufReadU8(src);
2804         } else
2805             return MSP_RESULT_ERROR;
2806         break;
2807
2808     case MSP_SET_RSSI_CONFIG:
2809         sbufReadU8Safe(&tmp_u8, src);
2810         if ((dataSize == 1) && (tmp_u8 <= MAX_SUPPORTED_RC_CHANNEL_COUNT)) {
2811             rxConfigMutable()->rssi_channel = tmp_u8;
2812             rxUpdateRSSISource();
2813         } else {
2814             return MSP_RESULT_ERROR;
2815         }
2816         break;
2817
2818     case MSP_SET_RX_MAP:
2819         if (dataSize == MAX_MAPPABLE_RX_INPUTS) {
2820             for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) {
2821                 rxConfigMutable()->rcmap[i] = sbufReadU8(src);
2822             }
2823         } else
2824             return MSP_RESULT_ERROR;
2825         break;
2826
2827     case MSP2_COMMON_SET_SERIAL_CONFIG:
2828         {
2829             uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint32_t) + (sizeof(uint8_t) * 4);
2830
2831             if (dataSize % portConfigSize != 0) {
2832                 return MSP_RESULT_ERROR;
2833             }
2834
2835             uint8_t remainingPortsInPacket = dataSize / portConfigSize;
2836
2837             while (remainingPortsInPacket--) {
2838                 uint8_t identifier = sbufReadU8(src);
2839
2840                 serialPortConfig_t *portConfig = serialFindPortConfiguration(identifier);
2841                 if (!portConfig) {
2842                     return MSP_RESULT_ERROR;
2843                 }
2844
2845                 portConfig->identifier = identifier;
2846                 portConfig->functionMask = sbufReadU32(src);
2847                 portConfig->msp_baudrateIndex = constrain(sbufReadU8(src), BAUD_MIN, BAUD_MAX);
2848                 portConfig->gps_baudrateIndex = constrain(sbufReadU8(src), BAUD_MIN, BAUD_MAX);
2849                 portConfig->telemetry_baudrateIndex = constrain(sbufReadU8(src), BAUD_MIN, BAUD_MAX);
2850                 portConfig->peripheral_baudrateIndex = constrain(sbufReadU8(src), BAUD_MIN, BAUD_MAX);
2851             }
2852         }
2853         break;
2854
2855 #ifdef USE_LED_STRIP
2856     case MSP_SET_LED_COLORS:
2857         if (dataSize == LED_CONFIGURABLE_COLOR_COUNT * 4) {
2858             for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
2859                 hsvColor_t *color = &ledStripConfigMutable()->colors[i];
2860                 color->h = sbufReadU16(src);
2861                 color->s = sbufReadU8(src);
2862                 color->v = sbufReadU8(src);
2863             }
2864         } else
2865             return MSP_RESULT_ERROR;
2866         break;
2867
2868     case MSP_SET_LED_STRIP_CONFIG:
2869         if (dataSize == (1 + sizeof(uint32_t))) {
2870             tmp_u8 = sbufReadU8(src);
2871             if (tmp_u8 >= LED_MAX_STRIP_LENGTH) {
2872                 return MSP_RESULT_ERROR;
2873             }
2874             ledConfig_t *ledConfig = &ledStripConfigMutable()->ledConfigs[tmp_u8];
2875
2876             uint32_t legacyConfig = sbufReadU32(src);
2877
2878             ledConfig->led_position = legacyConfig & 0xFF;
2879             ledConfig->led_function = (legacyConfig >> 8) & 0xF;
2880             ledConfig->led_overlay = (legacyConfig >> 12) & 0x3F;
2881             ledConfig->led_color = (legacyConfig >> 18) & 0xF;
2882             ledConfig->led_direction = (legacyConfig >> 22) & 0x3F;
2883             ledConfig->led_params = (legacyConfig >> 28) & 0xF;
2884
2885             reevaluateLedConfig();
2886         } else
2887             return MSP_RESULT_ERROR;
2888         break;
2889
2890     case MSP2_INAV_SET_LED_STRIP_CONFIG_EX:
2891         if (dataSize == (1 + sizeof(ledConfig_t))) {
2892             tmp_u8 = sbufReadU8(src);
2893             if (tmp_u8 >= LED_MAX_STRIP_LENGTH) {
2894                 return MSP_RESULT_ERROR;
2895             }
2896             ledConfig_t *ledConfig = &ledStripConfigMutable()->ledConfigs[tmp_u8];
2897             sbufReadDataSafe(src, ledConfig, sizeof(ledConfig_t));
2898             reevaluateLedConfig();
2899         } else
2900             return MSP_RESULT_ERROR;
2901         break;
2902
2903     case MSP_SET_LED_STRIP_MODECOLOR:
2904         if (dataSize == 3) {
2905             ledModeIndex_e modeIdx = sbufReadU8(src);
2906             int funIdx = sbufReadU8(src);
2907             int color = sbufReadU8(src);
2908
2909             if (!setModeColor(modeIdx, funIdx, color))
2910                 return MSP_RESULT_ERROR;
2911         } else
2912             return MSP_RESULT_ERROR;
2913         break;
2914 #endif
2915
2916 #ifdef NAV_NON_VOLATILE_WAYPOINT_STORAGE
2917     case MSP_WP_MISSION_LOAD:
2918         sbufReadU8Safe(NULL, src);    // Mission ID (reserved)
2919         if ((dataSize != 1) || (!loadNonVolatileWaypointList(false)))
2920             return MSP_RESULT_ERROR;
2921         break;
2922
2923     case MSP_WP_MISSION_SAVE:
2924         sbufReadU8Safe(NULL, src);    // Mission ID (reserved)
2925         if ((dataSize != 1) || (!saveNonVolatileWaypointList()))
2926             return MSP_RESULT_ERROR;
2927         break;
2928 #endif
2929
2930     case MSP_SET_RTC:
2931         if (dataSize == 6) {
2932             // Use seconds and milliseconds to make senders
2933             // easier to implement. Generating a 64 bit value
2934             // might not be trivial in some platforms.
2935             int32_t secs = (int32_t)sbufReadU32(src);
2936             uint16_t millis = sbufReadU16(src);
2937             rtcTime_t t = rtcTimeMake(secs, millis);
2938             rtcSet(&t);
2939         } else
2940             return MSP_RESULT_ERROR;
2941         break;
2942
2943     case MSP_SET_TX_INFO:
2944         {
2945             // This message will be sent while the aircraft is
2946             // armed. Better to guard ourselves against potentially
2947             // malformed requests.
2948             uint8_t rssi;
2949             if (sbufReadU8Safe(&rssi, src)) {
2950                 setRSSIFromMSP(rssi);
2951             }
2952         }
2953         break;
2954
2955     case MSP_SET_NAME:
2956         if (dataSize <= MAX_NAME_LENGTH) {
2957             char *name = systemConfigMutable()->craftName;
2958             int len = MIN(MAX_NAME_LENGTH, (int)dataSize);
2959             sbufReadData(src, name, len);
2960             memset(&name[len], '\0', (MAX_NAME_LENGTH + 1) - len);
2961         } else
2962             return MSP_RESULT_ERROR;
2963         break;
2964
2965     case MSP2_COMMON_SET_TZ:
2966         if (dataSize == 2)
2967             timeConfigMutable()->tz_offset = (int16_t)sbufReadU16(src);
2968         else if (dataSize == 3) {
2969             timeConfigMutable()->tz_offset = (int16_t)sbufReadU16(src);
2970             timeConfigMutable()->tz_automatic_dst = (uint8_t)sbufReadU8(src);
2971         } else
2972             return MSP_RESULT_ERROR;
2973         break;
2974
2975     case MSP2_INAV_SET_MIXER:
2976         if (dataSize == 9) {
2977             mixerConfigMutable()->motorDirectionInverted = sbufReadU8(src);
2978             sbufReadU8(src); // Was yaw_jump_prevention_limit
2979         mixerConfigMutable()->motorstopOnLow = sbufReadU8(src);
2980             mixerConfigMutable()->platformType = sbufReadU8(src);
2981             mixerConfigMutable()->hasFlaps = sbufReadU8(src);
2982             mixerConfigMutable()->appliedMixerPreset = sbufReadU16(src);
2983             sbufReadU8(src); //Read and ignore MAX_SUPPORTED_MOTORS
2984             sbufReadU8(src); //Read and ignore MAX_SUPPORTED_SERVOS
2985             mixerUpdateStateFlags();
2986         } else
2987             return MSP_RESULT_ERROR;
2988         break;
2989
2990 #if defined(USE_OSD)
2991     case MSP2_INAV_OSD_SET_LAYOUT_ITEM:
2992         {
2993             uint8_t layout;
2994             if (!sbufReadU8Safe(&layout, src)) {
2995                 return MSP_RESULT_ERROR;
2996             }
2997             uint8_t item;
2998             if (!sbufReadU8Safe(&item, src)) {
2999                 return MSP_RESULT_ERROR;
3000             }
3001             if (!sbufReadU16Safe(&osdLayoutsConfigMutable()->item_pos[layout][item], src)) {
3002                 return MSP_RESULT_ERROR;
3003             }
3004             // If the layout is not already overriden and it's different
3005             // than the layout for the item that was just configured,
3006             // override it for 10 seconds.
3007             bool overridden;
3008             int activeLayout = osdGetActiveLayout(&overridden);
3009             if (activeLayout != layout && !overridden) {
3010                 osdOverrideLayout(layout, 10000);
3011             } else {
3012                 osdStartFullRedraw();
3013             }
3014         }
3015
3016         break;
3017
3018     case MSP2_INAV_OSD_SET_ALARMS:
3019         {
3020             if (dataSize == 24) {
3021                 osdConfigMutable()->rssi_alarm = sbufReadU8(src);
3022                 osdConfigMutable()->time_alarm = sbufReadU16(src);
3023                 osdConfigMutable()->alt_alarm = sbufReadU16(src);
3024                 osdConfigMutable()->dist_alarm = sbufReadU16(src);
3025                 osdConfigMutable()->neg_alt_alarm = sbufReadU16(src);
3026                 tmp_u16 = sbufReadU16(src);
3027                 osdConfigMutable()->gforce_alarm = tmp_u16 / 1000.0f;
3028                 tmp_u16 = sbufReadU16(src);
3029                 osdConfigMutable()->gforce_axis_alarm_min = (int16_t)tmp_u16 / 1000.0f;
3030                 tmp_u16 = sbufReadU16(src);
3031                 osdConfigMutable()->gforce_axis_alarm_max = (int16_t)tmp_u16 / 1000.0f;
3032                 osdConfigMutable()->current_alarm = sbufReadU8(src);
3033                 osdConfigMutable()->imu_temp_alarm_min = sbufReadU16(src);
3034                 osdConfigMutable()->imu_temp_alarm_max = sbufReadU16(src);
3035 #ifdef USE_BARO
3036                 osdConfigMutable()->baro_temp_alarm_min = sbufReadU16(src);
3037                 osdConfigMutable()->baro_temp_alarm_max = sbufReadU16(src);
3038 #endif
3039             } else
3040                 return MSP_RESULT_ERROR;
3041         }
3042
3043         break;
3044
3045     case MSP2_INAV_OSD_SET_PREFERENCES:
3046         {
3047             if (dataSize == 9) {
3048                 osdConfigMutable()->video_system = sbufReadU8(src);
3049                 osdConfigMutable()->main_voltage_decimals = sbufReadU8(src);
3050                 osdConfigMutable()->ahi_reverse_roll = sbufReadU8(src);
3051                 osdConfigMutable()->crosshairs_style = sbufReadU8(src);
3052                 osdConfigMutable()->left_sidebar_scroll = sbufReadU8(src);
3053                 osdConfigMutable()->right_sidebar_scroll = sbufReadU8(src);
3054                 osdConfigMutable()->sidebar_scroll_arrows = sbufReadU8(src);
3055                 osdConfigMutable()->units = sbufReadU8(src);
3056                 osdConfigMutable()->stats_energy_unit = sbufReadU8(src);
3057                 osdStartFullRedraw();
3058             } else
3059                 return MSP_RESULT_ERROR;
3060         }
3061
3062         break;
3063 #endif
3064
3065     case MSP2_INAV_SET_MC_BRAKING:
3066 #ifdef USE_MR_BRAKING_MODE
3067         if (dataSize == 14) {
3068             navConfigMutable()->mc.braking_speed_threshold = sbufReadU16(src);
3069             navConfigMutable()->mc.braking_disengage_speed = sbufReadU16(src);
3070             navConfigMutable()->mc.braking_timeout = sbufReadU16(src);
3071             navConfigMutable()->mc.braking_boost_factor = sbufReadU8(src);
3072             navConfigMutable()->mc.braking_boost_timeout = sbufReadU16(src);
3073             navConfigMutable()->mc.braking_boost_speed_threshold = sbufReadU16(src);
3074             navConfigMutable()->mc.braking_boost_disengage_speed = sbufReadU16(src);
3075             navConfigMutable()->mc.braking_bank_angle = sbufReadU8(src);
3076         } else
3077 #endif
3078             return MSP_RESULT_ERROR;
3079         break;
3080
3081     case MSP2_INAV_SELECT_BATTERY_PROFILE:
3082         if (!ARMING_FLAG(ARMED) && sbufReadU8Safe(&tmp_u8, src)) {
3083                 setConfigBatteryProfileAndWriteEEPROM(tmp_u8);
3084         } else {
3085             return MSP_RESULT_ERROR;
3086         }
3087         break;
3088
3089     case MSP2_INAV_SELECT_MIXER_PROFILE:
3090         if (!ARMING_FLAG(ARMED) && sbufReadU8Safe(&tmp_u8, src)) {
3091                 setConfigMixerProfileAndWriteEEPROM(tmp_u8);
3092         } else {
3093             return MSP_RESULT_ERROR;
3094         }
3095         break;
3096
3097 #ifdef USE_TEMPERATURE_SENSOR
3098     case MSP2_INAV_SET_TEMP_SENSOR_CONFIG:
3099         if (dataSize == sizeof(tempSensorConfig_t) * MAX_TEMP_SENSORS) {
3100             for (uint8_t index = 0; index < MAX_TEMP_SENSORS; ++index) {
3101                 tempSensorConfig_t *sensorConfig = tempSensorConfigMutable(index);
3102                 sensorConfig->type = sbufReadU8(src);
3103                 for (uint8_t addrIndex = 0; addrIndex < 8; ++addrIndex)
3104                     ((uint8_t *)&sensorConfig->address)[addrIndex] = sbufReadU8(src);
3105                 sensorConfig->alarm_min = sbufReadU16(src);
3106                 sensorConfig->alarm_max = sbufReadU16(src);
3107                 tmp_u8 = sbufReadU8(src);
3108                 sensorConfig->osdSymbol = tmp_u8 > TEMP_SENSOR_SYM_COUNT ? 0 : tmp_u8;
3109                 for (uint8_t labelIndex = 0; labelIndex < TEMPERATURE_LABEL_LEN; ++labelIndex)
3110                     sensorConfig->label[labelIndex] = toupper(sbufReadU8(src));
3111             }
3112         } else
3113             return MSP_RESULT_ERROR;
3114         break;
3115 #endif
3116
3117 #ifdef MSP_FIRMWARE_UPDATE
3118     case MSP2_INAV_FWUPDT_PREPARE:
3119         if (!firmwareUpdatePrepare(sbufReadU32(src))) {
3120             return MSP_RESULT_ERROR;
3121         }
3122         break;
3123     case MSP2_INAV_FWUPDT_STORE:
3124         if (!firmwareUpdateStore(sbufPtr(src), sbufBytesRemaining(src))) {
3125             return MSP_RESULT_ERROR;
3126         }
3127         break;
3128     case MSP2_INAV_FWUPDT_EXEC:
3129         firmwareUpdateExec(sbufReadU8(src));
3130         return MSP_RESULT_ERROR; // will only be reached if the update is not ready
3131         break;
3132     case MSP2_INAV_FWUPDT_ROLLBACK_PREPARE:
3133         if (!firmwareUpdateRollbackPrepare()) {
3134             return MSP_RESULT_ERROR;
3135         }
3136         break;
3137     case MSP2_INAV_FWUPDT_ROLLBACK_EXEC:
3138         firmwareUpdateRollbackExec();
3139         return MSP_RESULT_ERROR; // will only be reached if the rollback is not ready
3140         break;
3141 #endif
3142 #ifdef USE_SAFE_HOME
3143     case MSP2_INAV_SET_SAFEHOME:
3144         if (dataSize == 10) {
3145             uint8_t i;
3146             if (!sbufReadU8Safe(&i, src) || i >= MAX_SAFE_HOMES) {
3147                 return MSP_RESULT_ERROR;
3148             }
3149             safeHomeConfigMutable(i)->enabled = sbufReadU8(src);
3150             safeHomeConfigMutable(i)->lat = sbufReadU32(src);
3151             safeHomeConfigMutable(i)->lon = sbufReadU32(src);
3152             resetFwAutolandApproach(i);
3153         } else {
3154             return MSP_RESULT_ERROR;
3155         }
3156         break;
3157 #endif
3158
3159     case MSP2_INAV_SET_FW_APPROACH:
3160         if (dataSize == 15) {
3161             uint8_t i;
3162             if (!sbufReadU8Safe(&i, src) || i >= MAX_FW_LAND_APPOACH_SETTINGS) {
3163                 return MSP_RESULT_ERROR;
3164             }
3165             fwAutolandApproachConfigMutable(i)->approachAlt = sbufReadU32(src);
3166             fwAutolandApproachConfigMutable(i)->landAlt = sbufReadU32(src);
3167             fwAutolandApproachConfigMutable(i)->approachDirection = sbufReadU8(src);
3168
3169             int16_t head1 = 0, head2 = 0;
3170             if (sbufReadI16Safe(&head1, src)) {
3171                 fwAutolandApproachConfigMutable(i)->landApproachHeading1 = head1;
3172             }
3173             if (sbufReadI16Safe(&head2, src)) {
3174                 fwAutolandApproachConfigMutable(i)->landApproachHeading2 = head2;
3175             }
3176             fwAutolandApproachConfigMutable(i)->isSeaLevelRef = sbufReadU8(src);
3177         } else {
3178             return MSP_RESULT_ERROR;
3179         }
3180         break;
3181
3182 #ifdef USE_EZ_TUNE
3183
3184     case MSP2_INAV_EZ_TUNE_SET:
3185         {
3186             if (dataSize == 10) {
3187                 ezTuneMutable()->enabled = sbufReadU8(src);
3188                 ezTuneMutable()->filterHz = sbufReadU16(src);
3189                 ezTuneMutable()->axisRatio = sbufReadU8(src);
3190                 ezTuneMutable()->response = sbufReadU8(src);
3191                 ezTuneMutable()->damping = sbufReadU8(src);
3192                 ezTuneMutable()->stability = sbufReadU8(src);
3193                 ezTuneMutable()->aggressiveness = sbufReadU8(src);
3194                 ezTuneMutable()->rate = sbufReadU8(src);
3195                 ezTuneMutable()->expo = sbufReadU8(src);
3196
3197                 ezTuneUpdate();
3198             } else {
3199                 return MSP_RESULT_ERROR;
3200             }
3201         }
3202         break;
3203
3204 #endif
3205
3206 #ifdef USE_RATE_DYNAMICS
3207
3208     case MSP2_INAV_SET_RATE_DYNAMICS:
3209
3210         if (dataSize == 6) {
3211             ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.sensitivityCenter = sbufReadU8(src);
3212             ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.sensitivityEnd = sbufReadU8(src);
3213             ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.correctionCenter = sbufReadU8(src);
3214             ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.correctionEnd = sbufReadU8(src);
3215             ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightCenter = sbufReadU8(src);
3216             ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightEnd = sbufReadU8(src);
3217
3218         } else {
3219             return MSP_RESULT_ERROR;
3220         }
3221
3222         break;
3223
3224 #endif
3225
3226
3227     default:
3228         return MSP_RESULT_ERROR;
3229     }
3230     return MSP_RESULT_ACK;
3231 }
3232
3233 static const setting_t *mspReadSetting(sbuf_t *src)
3234 {
3235     char name[SETTING_MAX_NAME_LENGTH];
3236     uint16_t id;
3237     uint8_t c;
3238     size_t s = 0;
3239     while (1) {
3240         if (!sbufReadU8Safe(&c, src)) {
3241             return NULL;
3242         }
3243         name[s++] = c;
3244         if (c == '\0') {
3245             if (s == 1) {
3246                 // Payload starts with a zero, setting index
3247                 // as uint16_t follows
3248                 if (sbufReadU16Safe(&id, src)) {
3249                     return settingGet(id);
3250                 }
3251                 return NULL;
3252             }
3253             break;
3254         }
3255         if (s == SETTING_MAX_NAME_LENGTH) {
3256             // Name is too long
3257             return NULL;
3258         }
3259     }
3260     return settingFind(name);
3261 }
3262
3263 static bool mspSettingCommand(sbuf_t *dst, sbuf_t *src)
3264 {
3265     const setting_t *setting = mspReadSetting(src);
3266     if (!setting) {
3267         return false;
3268     }
3269
3270     const void *ptr = settingGetValuePointer(setting);
3271     size_t size = settingGetValueSize(setting);
3272     sbufWriteDataSafe(dst, ptr, size);
3273     return true;
3274 }
3275
3276 static bool mspSetSettingCommand(sbuf_t *dst, sbuf_t *src)
3277 {
3278     UNUSED(dst);
3279
3280     const setting_t *setting = mspReadSetting(src);
3281     if (!setting) {
3282         return false;
3283     }
3284
3285     setting_min_t min = settingGetMin(setting);
3286     setting_max_t max = settingGetMax(setting);
3287
3288     void *ptr = settingGetValuePointer(setting);
3289     switch (SETTING_TYPE(setting)) {
3290         case VAR_UINT8:
3291             {
3292                 uint8_t val;
3293                 if (!sbufReadU8Safe(&val, src)) {
3294                     return false;
3295                 }
3296                 if (val > max) {
3297                     return false;
3298                 }
3299                 *((uint8_t*)ptr) = val;
3300             }
3301             break;
3302         case VAR_INT8:
3303             {
3304                 int8_t val;
3305                 if (!sbufReadI8Safe(&val, src)) {
3306                     return false;
3307                 }
3308                 if (val < min || val > (int8_t)max) {
3309                     return false;
3310                 }
3311                 *((int8_t*)ptr) = val;
3312             }
3313             break;
3314         case VAR_UINT16:
3315             {
3316                 uint16_t val;
3317                 if (!sbufReadU16Safe(&val, src)) {
3318                     return false;
3319                 }
3320                 if (val > max) {
3321                     return false;
3322                 }
3323                 *((uint16_t*)ptr) = val;
3324             }
3325             break;
3326         case VAR_INT16:
3327             {
3328                 int16_t val;
3329                 if (!sbufReadI16Safe(&val, src)) {
3330                     return false;
3331                 }
3332                 if (val < min || val > (int16_t)max) {
3333                     return false;
3334                 }
3335                 *((int16_t*)ptr) = val;
3336             }
3337             break;
3338         case VAR_UINT32:
3339             {
3340                 uint32_t val;
3341                 if (!sbufReadU32Safe(&val, src)) {
3342                     return false;
3343                 }
3344                 if (val > max) {
3345                     return false;
3346                 }
3347                 *((uint32_t*)ptr) = val;
3348             }
3349             break;
3350         case VAR_FLOAT:
3351             {
3352                 float val;
3353                 if (!sbufReadDataSafe(src, &val, sizeof(float))) {
3354                     return false;
3355                 }
3356                 if (val < (float)min || val > (float)max) {
3357                     return false;
3358                 }
3359                 *((float*)ptr) = val;
3360             }
3361             break;
3362         case VAR_STRING:
3363             {
3364                 settingSetString(setting, (const char*)sbufPtr(src), sbufBytesRemaining(src));
3365             }
3366             break;
3367     }
3368
3369     return true;
3370 }
3371
3372 static bool mspSettingInfoCommand(sbuf_t *dst, sbuf_t *src)
3373 {
3374     const setting_t *setting = mspReadSetting(src);
3375     if (!setting) {
3376         return false;
3377     }
3378
3379     char name_buf[SETTING_MAX_WORD_LENGTH+1];
3380     settingGetName(setting, name_buf);
3381     sbufWriteDataSafe(dst, name_buf, strlen(name_buf) + 1);
3382
3383     // Parameter Group ID
3384     sbufWriteU16(dst, settingGetPgn(setting));
3385
3386     // Type, section and mode
3387     sbufWriteU8(dst, SETTING_TYPE(setting));
3388     sbufWriteU8(dst, SETTING_SECTION(setting));
3389     sbufWriteU8(dst, SETTING_MODE(setting));
3390
3391     // Min as int32_t
3392     int32_t min = settingGetMin(setting);
3393     sbufWriteU32(dst, (uint32_t)min);
3394     // Max as uint32_t
3395     uint32_t max = settingGetMax(setting);
3396     sbufWriteU32(dst, max);
3397
3398     // Absolute setting index
3399     sbufWriteU16(dst, settingGetIndex(setting));
3400
3401     // If the setting is profile based, send the current one
3402     // and the count, both as uint8_t. For MASTER_VALUE, we
3403     // send two zeroes, so the MSP client can assume there
3404     // will always be two bytes.
3405     switch (SETTING_SECTION(setting)) {
3406     case MASTER_VALUE:
3407         sbufWriteU8(dst, 0);
3408         sbufWriteU8(dst, 0);
3409         break;
3410     case EZ_TUNE_VALUE:
3411         FALLTHROUGH;
3412     case PROFILE_VALUE:
3413         FALLTHROUGH;
3414     case CONTROL_RATE_VALUE:
3415         sbufWriteU8(dst, getConfigProfile());
3416         sbufWriteU8(dst, MAX_PROFILE_COUNT);
3417         break;
3418     case BATTERY_CONFIG_VALUE:
3419         sbufWriteU8(dst, getConfigBatteryProfile());
3420         sbufWriteU8(dst, MAX_BATTERY_PROFILE_COUNT);
3421         break;
3422     case MIXER_CONFIG_VALUE:
3423         sbufWriteU8(dst, getConfigMixerProfile());
3424         sbufWriteU8(dst, MAX_MIXER_PROFILE_COUNT);
3425         break;
3426     }
3427
3428     // If the setting uses a table, send each possible string (null terminated)
3429     if (SETTING_MODE(setting) == MODE_LOOKUP) {
3430         for (int ii = (int)min; ii <= (int)max; ii++) {
3431             const char *name = settingLookupValueName(setting, ii);
3432             sbufWriteDataSafe(dst, name, strlen(name) + 1);
3433         }
3434     }
3435
3436     // Finally, include the setting value. This way resource constrained callers
3437     // (e.g. a script in the radio) don't need to perform another call to retrieve
3438     // the value.
3439     const void *ptr = settingGetValuePointer(setting);
3440     size_t size = settingGetValueSize(setting);
3441     sbufWriteDataSafe(dst, ptr, size);
3442
3443     return true;
3444 }
3445
3446 static bool mspParameterGroupsCommand(sbuf_t *dst, sbuf_t *src)
3447 {
3448     uint16_t first;
3449     uint16_t last;
3450     uint16_t start;
3451     uint16_t end;
3452
3453     if (sbufReadU16Safe(&first, src)) {
3454         last = first;
3455     } else {
3456         first = PG_ID_FIRST;
3457         last = PG_ID_LAST;
3458     }
3459
3460     for (int ii = first; ii <= last; ii++) {
3461         if (settingsGetParameterGroupIndexes(ii, &start, &end)) {
3462             sbufWriteU16(dst, ii);
3463             sbufWriteU16(dst, start);
3464             sbufWriteU16(dst, end);
3465         }
3466     }
3467     return true;
3468 }
3469
3470 #ifdef USE_SIMULATOR
3471 bool isOSDTypeSupportedBySimulator(void)
3472 {
3473 #ifdef USE_OSD
3474         displayPort_t *osdDisplayPort = osdGetDisplayPort();
3475         return (!!osdDisplayPort && !!osdDisplayPort->vTable->readChar);
3476 #else
3477     return false;
3478 #endif
3479 }
3480
3481 void mspWriteSimulatorOSD(sbuf_t *dst)
3482 {
3483         //RLE encoding
3484         //scan displayBuffer iteratively
3485         //no more than 80+3+2 bytes output in single run
3486         //0 and 255 are special symbols
3487         //255 [char] - font bank switch
3488         //0 [flags,count] [char] - font bank switch, blink switch and character repeat
3489     //original 0 is sent as 32
3490     //original 0xff, 0x100 and 0x1ff are forcibly sent inside command 0
3491
3492         static uint8_t osdPos_y = 0;
3493         static uint8_t osdPos_x = 0;
3494
3495     //indicate new format hitl 1.4.0
3496         sbufWriteU8(dst, 255);
3497
3498         if (isOSDTypeSupportedBySimulator())
3499         {
3500                 displayPort_t *osdDisplayPort = osdGetDisplayPort();
3501
3502                 sbufWriteU8(dst, osdDisplayPort->rows);
3503                 sbufWriteU8(dst, osdDisplayPort->cols);
3504
3505                 sbufWriteU8(dst, osdPos_y);
3506                 sbufWriteU8(dst, osdPos_x);
3507
3508                 int bytesCount = 0;
3509
3510                 uint16_t c = 0;
3511                 textAttributes_t attr = 0;
3512                 bool highBank = false;
3513                 bool blink = false;
3514                 int count = 0;
3515
3516                 int processedRows = osdDisplayPort->rows;
3517
3518                 while (bytesCount < 80) //whole response should be less 155 bytes at worst.
3519                 {
3520                         bool blink1;
3521                         uint16_t lastChar;
3522
3523                         count = 0;
3524                         while ( true )
3525                         {
3526                                 displayReadCharWithAttr(osdDisplayPort, osdPos_x, osdPos_y, &c, &attr);
3527                                 if (c == 0) c = 32;
3528
3529                                 //REVIEW: displayReadCharWithAttr() should return mode with _TEXT_ATTRIBUTES_BLINK_BIT !
3530                                 //for max7456 it returns mode with MAX7456_MODE_BLINK instead (wrong)
3531                                 //because max7456ReadChar() does not decode from MAX7456_MODE_BLINK to _TEXT_ATTRIBUTES_BLINK_BIT
3532                                 //it should!
3533
3534                                 //bool blink2 = TEXT_ATTRIBUTES_HAVE_BLINK(attr);
3535                                 bool blink2 = attr & (1<<4); //MAX7456_MODE_BLINK
3536
3537                                 if (count == 0)
3538                                 {
3539                                         lastChar = c;
3540                                         blink1 = blink2;
3541                                 }
3542                                 else if ((lastChar != c) || (blink2 != blink1) || (count == 63))
3543                                 {
3544                                         break;
3545                                 }
3546
3547                                 count++;
3548
3549                                 osdPos_x++;
3550                                 if (osdPos_x == osdDisplayPort->cols)
3551                                 {
3552                                         osdPos_x = 0;
3553                                         osdPos_y++;
3554                                         processedRows--;
3555                                         if (osdPos_y == osdDisplayPort->rows)
3556                                         {
3557                                                 osdPos_y = 0;
3558                                         }
3559                                 }
3560                         }
3561
3562                         uint8_t cmd = 0;
3563             uint8_t lastCharLow = (uint8_t)(lastChar & 0xff);
3564                         if (blink1 != blink)
3565                         {
3566                                 cmd |= 128;//switch blink attr
3567                                 blink = blink1;
3568                         }
3569
3570                         bool highBank1 = lastChar > 255;
3571                         if (highBank1 != highBank)
3572                         {
3573                                 cmd |= 64;//switch bank attr
3574                                 highBank = highBank1;
3575                         }
3576
3577                         if (count == 1 && cmd == 64)
3578                         {
3579                                 sbufWriteU8(dst, 255);  //short command for bank switch with char following
3580                                 sbufWriteU8(dst, lastChar & 0xff);
3581                                 bytesCount += 2;
3582                         }
3583                         else if ((count > 2) || (cmd !=0) || (lastChar == 255) || (lastChar == 0x100) || (lastChar == 0x1ff))
3584                         {
3585                                 cmd |= count;  //long command for blink/bank switch and symbol repeat
3586                                 sbufWriteU8(dst, 0);
3587                                 sbufWriteU8(dst, cmd);
3588                                 sbufWriteU8(dst, lastCharLow);
3589                                 bytesCount += 3;
3590                         }
3591                         else if (count == 2)  //cmd == 0 here
3592                         {
3593                                 sbufWriteU8(dst, lastCharLow);
3594                                 sbufWriteU8(dst, lastCharLow);
3595                                 bytesCount+=2;
3596                         }
3597                         else
3598                         {
3599                                 sbufWriteU8(dst, lastCharLow);
3600                                 bytesCount++;
3601                         }
3602
3603                         if ( processedRows <= 0 )
3604                         {
3605                                 break;
3606                         }
3607                 }
3608                 sbufWriteU8(dst, 0);  //command 0 with length=0 -> stop
3609                 sbufWriteU8(dst, 0);
3610         }
3611         else
3612         {
3613                 sbufWriteU8(dst, 0);
3614         }
3615 }
3616 #endif
3617
3618 bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResult_e *ret)
3619 {
3620     uint8_t tmp_u8;
3621     const unsigned int dataSize = sbufBytesRemaining(src);
3622
3623     switch (cmdMSP) {
3624
3625     case MSP_WP:
3626         mspFcWaypointOutCommand(dst, src);
3627         *ret = MSP_RESULT_ACK;
3628         break;
3629
3630 #if defined(USE_FLASHFS)
3631     case MSP_DATAFLASH_READ:
3632         mspFcDataFlashReadCommand(dst, src);
3633         *ret = MSP_RESULT_ACK;
3634         break;
3635 #endif
3636
3637     case MSP2_COMMON_SETTING:
3638         *ret = mspSettingCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR;
3639         break;
3640
3641     case MSP2_COMMON_SET_SETTING:
3642         *ret = mspSetSettingCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR;
3643         break;
3644
3645     case MSP2_COMMON_SETTING_INFO:
3646         *ret = mspSettingInfoCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR;
3647         break;
3648
3649     case MSP2_COMMON_PG_LIST:
3650         *ret = mspParameterGroupsCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR;
3651         break;
3652
3653 #if defined(USE_OSD)
3654     case MSP2_INAV_OSD_LAYOUTS:
3655         if (sbufBytesRemaining(src) >= 1) {
3656             uint8_t layout = sbufReadU8(src);
3657             if (layout >= OSD_LAYOUT_COUNT) {
3658                 *ret = MSP_RESULT_ERROR;
3659                 break;
3660             }
3661             if (sbufBytesRemaining(src) >= 2) {
3662                 // Asking for an specific item in a layout
3663                 uint16_t item = sbufReadU16(src);
3664                 if (item >= OSD_ITEM_COUNT) {
3665                     *ret = MSP_RESULT_ERROR;
3666                     break;
3667                 }
3668                 sbufWriteU16(dst, osdLayoutsConfig()->item_pos[layout][item]);
3669             } else {
3670                 // Asking for an specific layout
3671                 for (unsigned ii = 0; ii < OSD_ITEM_COUNT; ii++) {
3672                     sbufWriteU16(dst, osdLayoutsConfig()->item_pos[layout][ii]);
3673                 }
3674             }
3675         } else {
3676             // Return the number of layouts and items
3677             sbufWriteU8(dst, OSD_LAYOUT_COUNT);
3678             sbufWriteU8(dst, OSD_ITEM_COUNT);
3679         }
3680         *ret = MSP_RESULT_ACK;
3681         break;
3682 #endif
3683
3684 #ifdef USE_PROGRAMMING_FRAMEWORK
3685     case MSP2_INAV_LOGIC_CONDITIONS_SINGLE:
3686         *ret = mspFcLogicConditionCommand(dst, src);
3687         break;
3688 #endif
3689 #ifdef USE_SAFE_HOME
3690     case MSP2_INAV_SAFEHOME:
3691         *ret = mspFcSafeHomeOutCommand(dst, src);
3692         break;
3693 #endif
3694     case MSP2_INAV_FW_APPROACH:
3695         *ret = mspFwApproachOutCommand(dst, src);
3696         break;
3697
3698 #ifdef USE_SIMULATOR
3699     case MSP_SIMULATOR:
3700                 tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version
3701
3702         // Check the MSP version of simulator
3703                 if (tmp_u8 != SIMULATOR_MSP_VERSION) {
3704             break;
3705         }
3706
3707                 simulatorData.flags = sbufReadU8(src);
3708
3709         if (!SIMULATOR_HAS_OPTION(HITL_ENABLE)) {
3710
3711                         if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
3712                                 DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
3713
3714 #ifdef USE_BARO
3715             if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) {
3716                                 baroStartCalibration();
3717             }
3718 #endif
3719 #ifdef USE_MAG
3720                                 DISABLE_STATE(COMPASS_CALIBRATED);
3721                                 compassInit();
3722 #endif
3723                                 simulatorData.flags = HITL_RESET_FLAGS;
3724                 // Review: Many states were affected. Reboot?
3725
3726                                 disarm(DISARM_SWITCH);  // Disarm to prevent motor output!!!
3727                         }
3728         } else {
3729                         if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
3730 #ifdef USE_BARO
3731                 if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) {
3732                     sensorsSet(SENSOR_BARO);
3733                     setTaskEnabled(TASK_BARO, true);
3734                     DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
3735                                     baroStartCalibration();
3736                 }
3737 #endif
3738
3739 #ifdef USE_MAG
3740                                 if (compassConfig()->mag_hardware != MAG_NONE) {
3741                                         sensorsSet(SENSOR_MAG);
3742                                         ENABLE_STATE(COMPASS_CALIBRATED);
3743                                         DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
3744                                         mag.magADC[X] = 800;
3745                                         mag.magADC[Y] = 0;
3746                                         mag.magADC[Z] = 0;
3747                                 }
3748 #endif
3749                                 ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
3750                 ENABLE_STATE(ACCELEROMETER_CALIBRATED);
3751                                 LOG_DEBUG(SYSTEM, "Simulator enabled");
3752                         }
3753
3754                         if (dataSize >= 14) {
3755
3756                                 if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) {
3757                                         gpsSol.fixType = sbufReadU8(src);
3758                                         gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
3759                                         gpsSol.flags.hasNewData = true;
3760                                         gpsSol.numSat = sbufReadU8(src);
3761
3762                                         if (gpsSol.fixType != GPS_NO_FIX) {
3763                                                 gpsSol.flags.validVelNE = true;
3764                                                 gpsSol.flags.validVelD = true;
3765                                                 gpsSol.flags.validEPE = true;
3766                                                 gpsSol.flags.validTime = false;
3767
3768                                                 gpsSol.llh.lat = sbufReadU32(src);
3769                                                 gpsSol.llh.lon = sbufReadU32(src);
3770                                                 gpsSol.llh.alt = sbufReadU32(src);
3771                                                 gpsSol.groundSpeed = (int16_t)sbufReadU16(src);
3772                                                 gpsSol.groundCourse = (int16_t)sbufReadU16(src);
3773
3774                                                 gpsSol.velNED[X] = (int16_t)sbufReadU16(src);
3775                                                 gpsSol.velNED[Y] = (int16_t)sbufReadU16(src);
3776                                                 gpsSol.velNED[Z] = (int16_t)sbufReadU16(src);
3777
3778                                                 gpsSol.eph = 100;
3779                                                 gpsSol.epv = 100;
3780
3781                                                 ENABLE_STATE(GPS_FIX);
3782                                         } else {
3783                                                 sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
3784                                         }
3785                     // Feed data to navigation
3786                                         gpsProcessNewSolutionData();
3787                                 } else {
3788                                         sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
3789                                 }
3790
3791                                 if (!SIMULATOR_HAS_OPTION(HITL_USE_IMU)) {
3792                                         attitude.values.roll = (int16_t)sbufReadU16(src);
3793                                         attitude.values.pitch = (int16_t)sbufReadU16(src);
3794                                         attitude.values.yaw = (int16_t)sbufReadU16(src);
3795                                 } else {
3796                                         sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
3797                                 }
3798
3799                 // Get the acceleration in 1G units
3800                                 acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f;
3801                                 acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f;
3802                                 acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f;
3803                                 acc.accVibeSq[X] = 0.0f;
3804                                 acc.accVibeSq[Y] = 0.0f;
3805                                 acc.accVibeSq[Z] = 0.0f;
3806
3807                 // Get the angular velocity in DPS
3808                                 gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f;
3809                                 gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f;
3810                                 gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f;
3811
3812                                 if (sensors(SENSOR_BARO)) {
3813                                         baro.baroPressure = (int32_t)sbufReadU32(src);
3814                                         baro.baroTemperature = DEGREES_TO_CENTIDEGREES(SIMULATOR_BARO_TEMP);
3815                                 } else {
3816                                         sbufAdvance(src, sizeof(uint32_t));
3817                                 }
3818
3819                                 if (sensors(SENSOR_MAG)) {
3820                                         mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20;  // 16000 / 20 = 800uT
3821                                         mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20;   //note that mag failure is simulated by setting all readings to zero
3822                                         mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20;
3823                                 } else {
3824                                         sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
3825                                 }
3826
3827                 if (SIMULATOR_HAS_OPTION(HITL_EXT_BATTERY_VOLTAGE)) {
3828                     simulatorData.vbat = sbufReadU8(src);
3829                 } else {
3830                     simulatorData.vbat = SIMULATOR_FULL_BATTERY;
3831                 }
3832
3833                 if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
3834                     simulatorData.airSpeed = sbufReadU16(src);
3835                             } else {
3836                     if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
3837                         sbufReadU16(src);
3838                     }
3839                 }
3840
3841                 if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
3842                     simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8;
3843                 }
3844                         } else {
3845                                 DISABLE_STATE(GPS_FIX);
3846                         }
3847                 }
3848
3849                 sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_ROLL]);
3850                 sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_PITCH]);
3851                 sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_YAW]);
3852                 sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.input[INPUT_STABILIZED_THROTTLE] : -500));
3853
3854                 simulatorData.debugIndex++;
3855                 if (simulatorData.debugIndex == 8) {
3856                         simulatorData.debugIndex = 0;
3857                 }
3858
3859                 tmp_u8 = simulatorData.debugIndex |
3860                         ((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) |
3861                         (ARMING_FLAG(ARMED) ? 64 : 0) |
3862                         (!feature(FEATURE_OSD) ? 32: 0) |
3863                         (!isOSDTypeSupportedBySimulator() ? 16 : 0);
3864
3865                 sbufWriteU8(dst, tmp_u8);
3866                 sbufWriteU32(dst, debug[simulatorData.debugIndex]);
3867
3868                 sbufWriteU16(dst, attitude.values.roll);
3869                 sbufWriteU16(dst, attitude.values.pitch);
3870                 sbufWriteU16(dst, attitude.values.yaw);
3871
3872                 mspWriteSimulatorOSD(dst);
3873
3874         *ret = MSP_RESULT_ACK;
3875         break;
3876 #endif
3877 #ifndef SITL_BUILD
3878     case MSP2_INAV_TIMER_OUTPUT_MODE:
3879         if (dataSize == 0) {
3880             for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) {
3881                 sbufWriteU8(dst, i);
3882                 sbufWriteU8(dst, timerOverrides(i)->outputMode);
3883             }
3884             *ret = MSP_RESULT_ACK;
3885         } else if(dataSize == 1) {
3886             uint8_t timer = sbufReadU8(src);
3887             if(timer < HARDWARE_TIMER_DEFINITION_COUNT) {
3888                 sbufWriteU8(dst, timer);
3889                 sbufWriteU8(dst, timerOverrides(timer)->outputMode);
3890                 *ret = MSP_RESULT_ACK;
3891             } else {
3892                 *ret = MSP_RESULT_ERROR;
3893             }
3894         } else {
3895             *ret = MSP_RESULT_ERROR;
3896         }
3897         break;
3898     case MSP2_INAV_SET_TIMER_OUTPUT_MODE:
3899         if(dataSize == 2) {
3900             uint8_t timer = sbufReadU8(src);
3901             uint8_t outputMode = sbufReadU8(src);
3902             if(timer < HARDWARE_TIMER_DEFINITION_COUNT) {
3903                 timerOverridesMutable(timer)->outputMode = outputMode;
3904                 *ret = MSP_RESULT_ACK;
3905             } else {
3906                 *ret = MSP_RESULT_ERROR;
3907             }
3908         } else {
3909             *ret = MSP_RESULT_ERROR;
3910         }
3911         break;
3912 #endif
3913
3914     default:
3915         // Not handled
3916         return false;
3917     }
3918     return true;
3919 }
3920
3921 static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src)
3922 {
3923     UNUSED(src);
3924
3925     switch (cmdMSP) {
3926 #if defined(USE_RANGEFINDER_MSP)
3927         case MSP2_SENSOR_RANGEFINDER:
3928             mspRangefinderReceiveNewData(sbufPtr(src));
3929             break;
3930 #endif
3931
3932 #if defined(USE_OPFLOW_MSP)
3933         case MSP2_SENSOR_OPTIC_FLOW:
3934             mspOpflowReceiveNewData(sbufPtr(src));
3935             break;
3936 #endif
3937
3938 #if defined(USE_GPS_PROTO_MSP)
3939         case MSP2_SENSOR_GPS:
3940             mspGPSReceiveNewData(sbufPtr(src));
3941             break;
3942 #endif
3943
3944 #if defined(USE_MAG_MSP)
3945         case MSP2_SENSOR_COMPASS:
3946             mspMagReceiveNewData(sbufPtr(src));
3947             break;
3948 #endif
3949
3950 #if defined(USE_BARO_MSP)
3951         case MSP2_SENSOR_BAROMETER:
3952             mspBaroReceiveNewData(sbufPtr(src));
3953             break;
3954 #endif
3955
3956 #if defined(USE_PITOT_MSP)
3957         case MSP2_SENSOR_AIRSPEED:
3958             mspPitotmeterReceiveNewData(sbufPtr(src));
3959             break;
3960 #endif
3961     }
3962
3963     return MSP_RESULT_NO_REPLY;
3964 }
3965
3966 /*
3967  * Returns MSP_RESULT_ACK, MSP_RESULT_ERROR or MSP_RESULT_NO_REPLY
3968  */
3969 mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
3970 {
3971     mspResult_e ret = MSP_RESULT_ACK;
3972     sbuf_t *dst = &reply->buf;
3973     sbuf_t *src = &cmd->buf;
3974     const uint16_t cmdMSP = cmd->cmd;
3975     // initialize reply by default
3976     reply->cmd = cmd->cmd;
3977
3978     if (MSP2_IS_SENSOR_MESSAGE(cmdMSP)) {
3979         ret = mspProcessSensorCommand(cmdMSP, src);
3980     } else if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
3981         ret = MSP_RESULT_ACK;
3982     } else if (cmdMSP == MSP_SET_PASSTHROUGH) {
3983         mspFcSetPassthroughCommand(dst, src, mspPostProcessFn);
3984         ret = MSP_RESULT_ACK;
3985     } else {
3986         if (!mspFCProcessInOutCommand(cmdMSP, dst, src, &ret)) {
3987             ret = mspFcProcessInCommand(cmdMSP, src);
3988         }
3989     }
3990
3991     // Process DONT_REPLY flag
3992     if (cmd->flags & MSP_FLAG_DONT_REPLY) {
3993         ret = MSP_RESULT_NO_REPLY;
3994     }
3995
3996     reply->result = ret;
3997     return ret;
3998 }
3999
4000 /*
4001  * Return a pointer to the process command function
4002  */
4003 void mspFcInit(void)
4004 {
4005     initActiveBoxIds();
4006 }