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