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