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