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