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