?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Parse GPS DOP values (#11912)
[betaflight.git] / src / main / msp / msp.c
blob85467cd112680fe40a4c8628210446c074bfefed
1 /*
2 * This file is part of Cleanflight and Betaflight.
3 *
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
8 * any later version.
9 *
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
17 *
18 * If not, see <http://www.gnu.org/licenses/>.
19 */
20
21 #include <stdbool.h>
22 #include <stdint.h>
23 #include <string.h>
24 #include <math.h>
25 #include <stdlib.h>
26 #include <limits.h>
27 #include <ctype.h>
28
29 #include "platform.h"
30
31 #include "blackbox/blackbox.h"
32 #include "blackbox/blackbox_io.h"
33
34 #include "build/build_config.h"
35 #include "build/debug.h"
36 #include "build/version.h"
37
38 #include "cli/cli.h"
39
40 #include "common/axis.h"
41 #include "common/bitarray.h"
42 #include "common/color.h"
43 #include "common/huffman.h"
44 #include "common/maths.h"
45 #include "common/streambuf.h"
46 #include "common/utils.h"
47
48 #include "config/config.h"
49 #include "config/config_eeprom.h"
50 #include "config/feature.h"
51 #include "config/simplified_tuning.h"
52
53 #include "drivers/accgyro/accgyro.h"
54 #include "drivers/bus_i2c.h"
55 #include "drivers/bus_spi.h"
56 #include "drivers/camera_control.h"
57 #include "drivers/compass/compass.h"
58 #include "drivers/display.h"
59 #include "drivers/dshot.h"
60 #include "drivers/dshot_command.h"
61 #include "drivers/flash.h"
62 #include "drivers/io.h"
63 #include "drivers/motor.h"
64 #include "drivers/osd.h"
65 #include "drivers/pwm_output.h"
66 #include "drivers/sdcard.h"
67 #include "drivers/serial.h"
68 #include "drivers/serial_escserial.h"
69 #include "drivers/system.h"
70 #include "drivers/transponder_ir.h"
71 #include "drivers/usb_msc.h"
72 #include "drivers/vtx_common.h"
73 #include "drivers/vtx_table.h"
74
75 #include "fc/board_info.h"
76 #include "fc/controlrate_profile.h"
77 #include "fc/core.h"
78 #include "fc/dispatch.h"
79 #include "fc/rc.h"
80 #include "fc/rc_adjustments.h"
81 #include "fc/rc_controls.h"
82 #include "fc/rc_modes.h"
83 #include "fc/runtime_config.h"
84
85 #include "flight/failsafe.h"
86 #include "flight/gps_rescue.h"
87 #include "flight/imu.h"
88 #include "flight/mixer.h"
89 #include "flight/pid.h"
90 #include "flight/pid_init.h"
91 #include "flight/position.h"
92 #include "flight/rpm_filter.h"
93 #include "flight/servos.h"
94
95 #include "io/asyncfatfs/asyncfatfs.h"
96 #include "io/beeper.h"
97 #include "io/flashfs.h"
98 #include "io/gimbal.h"
99 #include "io/gps.h"
100 #include "io/ledstrip.h"
101 #include "io/serial.h"
102 #include "io/serial_4way.h"
103 #include "io/servos.h"
104 #include "io/transponder_ir.h"
105 #include "io/usb_msc.h"
106 #include "io/vtx_control.h"
107 #include "io/vtx.h"
108 #include "io/vtx_msp.h"
109
110 #include "msp/msp_box.h"
111 #include "msp/msp_protocol.h"
112 #include "msp/msp_protocol_v2_betaflight.h"
113 #include "msp/msp_protocol_v2_common.h"
114 #include "msp/msp_serial.h"
115
116 #include "osd/osd.h"
117 #include "osd/osd_elements.h"
118 #include "osd/osd_warnings.h"
119
120 #include "pg/beeper.h"
121 #include "pg/board.h"
122 #include "pg/dyn_notch.h"
123 #include "pg/gyrodev.h"
124 #include "pg/motor.h"
125 #include "pg/rx.h"
126 #include "pg/rx_spi.h"
127 #ifdef USE_RX_EXPRESSLRS
128 #include "pg/rx_spi_expresslrs.h"
129 #endif
130 #include "pg/usb.h"
131 #include "pg/vcd.h"
132 #include "pg/vtx_table.h"
133
134 #include "rx/rx.h"
135 #include "rx/rx_bind.h"
136 #include "rx/msp.h"
137
138 #include "scheduler/scheduler.h"
139
140 #include "sensors/acceleration.h"
141 #include "sensors/barometer.h"
142 #include "sensors/battery.h"
143 #include "sensors/boardalignment.h"
144 #include "sensors/compass.h"
145 #include "sensors/gyro.h"
146 #include "sensors/gyro_init.h"
147 #include "sensors/rangefinder.h"
148
149 #include "telemetry/msp_shared.h"
150 #include "telemetry/telemetry.h"
151
152 #ifdef USE_HARDWARE_REVISION_DETECTION
153 #include "hardware_revision.h"
154 #endif
155
156 #include "msp.h"
157
158
159 static const char * const flightControllerIdentifier = FC_FIRMWARE_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
160
161 enum {
162 MSP_REBOOT_FIRMWARE = 0,
163 MSP_REBOOT_BOOTLOADER_ROM,
164 MSP_REBOOT_MSC,
165 MSP_REBOOT_MSC_UTC,
166 MSP_REBOOT_BOOTLOADER_FLASH,
167 MSP_REBOOT_COUNT,
168 };
169
170 static uint8_t rebootMode;
171
172 typedef enum {
173 MSP_SDCARD_STATE_NOT_PRESENT = 0,
174 MSP_SDCARD_STATE_FATAL = 1,
175 MSP_SDCARD_STATE_CARD_INIT = 2,
176 MSP_SDCARD_STATE_FS_INIT = 3,
177 MSP_SDCARD_STATE_READY = 4
178 } mspSDCardState_e;
179
180 typedef enum {
181 MSP_SDCARD_FLAG_SUPPORTED = 1
182 } mspSDCardFlags_e;
183
184 typedef enum {
185 MSP_FLASHFS_FLAG_READY = 1,
186 MSP_FLASHFS_FLAG_SUPPORTED = 2
187 } mspFlashFsFlags_e;
188
189 typedef enum {
190 MSP_PASSTHROUGH_ESC_SIMONK = PROTOCOL_SIMONK,
191 MSP_PASSTHROUGH_ESC_BLHELI = PROTOCOL_BLHELI,
192 MSP_PASSTHROUGH_ESC_KISS = PROTOCOL_KISS,
193 MSP_PASSTHROUGH_ESC_KISSALL = PROTOCOL_KISSALL,
194 MSP_PASSTHROUGH_ESC_CASTLE = PROTOCOL_CASTLE,
195
196 MSP_PASSTHROUGH_SERIAL_ID = 0xFD,
197 MSP_PASSTHROUGH_SERIAL_FUNCTION_ID = 0xFE,
198
199 MSP_PASSTHROUGH_ESC_4WAY = 0xFF,
200 } mspPassthroughType_e;
201
202 #define RATEPROFILE_MASK (1 << 7)
203
204 #define RTC_NOT_SUPPORTED 0xff
205
206 typedef enum {
207 DEFAULTS_TYPE_BASE = 0,
208 DEFAULTS_TYPE_CUSTOM,
209 } defaultsType_e;
210
211 #ifdef USE_VTX_TABLE
212 static bool vtxTableNeedsInit = false;
213 #endif
214
215 static int mspDescriptor = 0;
216
217 mspDescriptor_t mspDescriptorAlloc(void)
218 {
219 return (mspDescriptor_t)mspDescriptor++;
220 }
221
222 static uint32_t mspArmingDisableFlags = 0;
223
224 static void mspArmingDisableByDescriptor(mspDescriptor_t desc)
225 {
226 mspArmingDisableFlags |= (1 << desc);
227 }
228
229 static void mspArmingEnableByDescriptor(mspDescriptor_t desc)
230 {
231 mspArmingDisableFlags &= ~(1 << desc);
232 }
233
234 static bool mspIsMspArmingEnabled(void)
235 {
236 return mspArmingDisableFlags == 0;
237 }
238
239 #define MSP_PASSTHROUGH_ESC_4WAY 0xff
240
241 static uint8_t mspPassthroughMode;
242 static uint8_t mspPassthroughArgument;
243
244 #ifdef USE_ESCSERIAL
245 static void mspEscPassthroughFn(serialPort_t *serialPort)
246 {
247 escEnablePassthrough(serialPort, &motorConfig()->dev, mspPassthroughArgument, mspPassthroughMode);
248 }
249 #endif
250
251 static serialPort_t *mspFindPassthroughSerialPort(void)
252 {
253 serialPortUsage_t *portUsage = NULL;
254
255 switch (mspPassthroughMode) {
256 case MSP_PASSTHROUGH_SERIAL_ID:
257 {
258 portUsage = findSerialPortUsageByIdentifier(mspPassthroughArgument);
259 break;
260 }
261 case MSP_PASSTHROUGH_SERIAL_FUNCTION_ID:
262 {
263 const serialPortConfig_t *portConfig = findSerialPortConfig(1 << mspPassthroughArgument);
264 if (portConfig) {
265 portUsage = findSerialPortUsageByIdentifier(portConfig->identifier);
266 }
267 break;
268 }
269 }
270 return portUsage ? portUsage->serialPort : NULL;
271 }
272
273 static void mspSerialPassthroughFn(serialPort_t *serialPort)
274 {
275 serialPort_t *passthroughPort = mspFindPassthroughSerialPort();
276 if (passthroughPort && serialPort) {
277 serialPassthrough(passthroughPort, serialPort, NULL, NULL);
278 }
279 }
280
281 static void mspFcSetPassthroughCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
282 {
283 const unsigned int dataSize = sbufBytesRemaining(src);
284 if (dataSize == 0) {
285 // Legacy format
286 mspPassthroughMode = MSP_PASSTHROUGH_ESC_4WAY;
287 } else {
288 mspPassthroughMode = sbufReadU8(src);
289 mspPassthroughArgument = sbufReadU8(src);
290 }
291
292 switch (mspPassthroughMode) {
293 case MSP_PASSTHROUGH_SERIAL_ID:
294 case MSP_PASSTHROUGH_SERIAL_FUNCTION_ID:
295 if (mspFindPassthroughSerialPort()) {
296 if (mspPostProcessFn) {
297 *mspPostProcessFn = mspSerialPassthroughFn;
298 }
299 sbufWriteU8(dst, 1);
300 } else {
301 sbufWriteU8(dst, 0);
302 }
303 break;
304 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
305 case MSP_PASSTHROUGH_ESC_4WAY:
306 // get channel number
307 // switch all motor lines HI
308 // reply with the count of ESC found
309 sbufWriteU8(dst, esc4wayInit());
310
311 if (mspPostProcessFn) {
312 *mspPostProcessFn = esc4wayProcess;
313 }
314 break;
315
316 #ifdef USE_ESCSERIAL
317 case MSP_PASSTHROUGH_ESC_SIMONK:
318 case MSP_PASSTHROUGH_ESC_BLHELI:
319 case MSP_PASSTHROUGH_ESC_KISS:
320 case MSP_PASSTHROUGH_ESC_KISSALL:
321 case MSP_PASSTHROUGH_ESC_CASTLE:
322 if (mspPassthroughArgument < getMotorCount() || (mspPassthroughMode == MSP_PASSTHROUGH_ESC_KISS && mspPassthroughArgument == ALL_MOTORS)) {
323 sbufWriteU8(dst, 1);
324
325 if (mspPostProcessFn) {
326 *mspPostProcessFn = mspEscPassthroughFn;
327 }
328
329 break;
330 }
331 FALLTHROUGH;
332 #endif // USE_ESCSERIAL
333 #endif //USE_SERIAL_4WAY_BLHELI_INTERFACE
334 default:
335 sbufWriteU8(dst, 0);
336 }
337 }
338
339 // TODO: Remove the pragma once this is called from unconditional code
340 #pragma GCC diagnostic ignored "-Wunused-function"
341 static void configRebootUpdateCheckU8(uint8_t *parm, uint8_t value)
342 {
343 if (*parm != value) {
344 setRebootRequired();
345 }
346 *parm = value;
347 }
348 #pragma GCC diagnostic pop
349
350 static void mspRebootFn(serialPort_t *serialPort)
351 {
352 UNUSED(serialPort);
353
354 motorShutdown();
355
356 switch (rebootMode) {
357 case MSP_REBOOT_FIRMWARE:
358 systemReset();
359
360 break;
361 case MSP_REBOOT_BOOTLOADER_ROM:
362 systemResetToBootloader(BOOTLOADER_REQUEST_ROM);
363
364 break;
365 #if defined(USE_USB_MSC)
366 case MSP_REBOOT_MSC:
367 case MSP_REBOOT_MSC_UTC: {
368 #ifdef USE_RTC_TIME
369 const int16_t timezoneOffsetMinutes = (rebootMode == MSP_REBOOT_MSC) ? timeConfig()->tz_offsetMinutes : 0;
370 systemResetToMsc(timezoneOffsetMinutes);
371 #else
372 systemResetToMsc(0);
373 #endif
374 }
375 break;
376 #endif
377 #if defined(USE_FLASH_BOOT_LOADER)
378 case MSP_REBOOT_BOOTLOADER_FLASH:
379 systemResetToBootloader(BOOTLOADER_REQUEST_FLASH);
380
381 break;
382 #endif
383 default:
384
385 return;
386 }
387
388 // control should never return here.
389 while (true) ;
390 }
391
392 #define MSP_DISPATCH_DELAY_US 1000000
393
394 void mspReboot(dispatchEntry_t* self)
395 {
396 UNUSED(self);
397
398 if (ARMING_FLAG(ARMED)) {
399 return;
400 }
401
402 mspRebootFn(NULL);
403 }
404
405 dispatchEntry_t mspRebootEntry =
406 {
407 mspReboot, 0, NULL, false
408 };
409
410 void writeReadEeprom(dispatchEntry_t* self)
411 {
412 UNUSED(self);
413
414 if (ARMING_FLAG(ARMED)) {
415 return;
416 }
417
418 writeEEPROM();
419 readEEPROM();
420
421 #ifdef USE_VTX_TABLE
422 if (vtxTableNeedsInit) {
423 vtxTableNeedsInit = false;
424 vtxTableInit(); // Reinitialize and refresh the in-memory copies
425 }
426 #endif
427 }
428
429 dispatchEntry_t writeReadEepromEntry =
430 {
431 writeReadEeprom, 0, NULL, false
432 };
433
434 static void serializeSDCardSummaryReply(sbuf_t *dst)
435 {
436 uint8_t flags = 0;
437 uint8_t state = 0;
438 uint8_t lastError = 0;
439 uint32_t freeSpace = 0;
440 uint32_t totalSpace = 0;
441
442 #if defined(USE_SDCARD)
443 if (sdcardConfig()->mode != SDCARD_MODE_NONE) {
444 flags = MSP_SDCARD_FLAG_SUPPORTED;
445
446 // Merge the card and filesystem states together
447 if (!sdcard_isInserted()) {
448 state = MSP_SDCARD_STATE_NOT_PRESENT;
449 } else if (!sdcard_isFunctional()) {
450 state = MSP_SDCARD_STATE_FATAL;
451 } else {
452 switch (afatfs_getFilesystemState()) {
453 case AFATFS_FILESYSTEM_STATE_READY:
454 state = MSP_SDCARD_STATE_READY;
455 break;
456
457 case AFATFS_FILESYSTEM_STATE_INITIALIZATION:
458 if (sdcard_isInitialized()) {
459 state = MSP_SDCARD_STATE_FS_INIT;
460 } else {
461 state = MSP_SDCARD_STATE_CARD_INIT;
462 }
463 break;
464
465 case AFATFS_FILESYSTEM_STATE_FATAL:
466 case AFATFS_FILESYSTEM_STATE_UNKNOWN:
467 default:
468 state = MSP_SDCARD_STATE_FATAL;
469 break;
470 }
471 }
472
473 lastError = afatfs_getLastError();
474 // Write free space and total space in kilobytes
475 if (state == MSP_SDCARD_STATE_READY) {
476 freeSpace = afatfs_getContiguousFreeSpace() / 1024;
477 totalSpace = sdcard_getMetadata()->numBlocks / 2;
478 }
479 }
480 #endif
481
482 sbufWriteU8(dst, flags);
483 sbufWriteU8(dst, state);
484 sbufWriteU8(dst, lastError);
485 sbufWriteU32(dst, freeSpace);
486 sbufWriteU32(dst, totalSpace);
487 }
488
489 static void serializeDataflashSummaryReply(sbuf_t *dst)
490 {
491 #ifdef USE_FLASHFS
492 if (flashfsIsSupported()) {
493 uint8_t flags = MSP_FLASHFS_FLAG_SUPPORTED;
494 flags |= (flashfsIsReady() ? MSP_FLASHFS_FLAG_READY : 0);
495
496 const flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_FLASHFS);
497
498 sbufWriteU8(dst, flags);
499 sbufWriteU32(dst, FLASH_PARTITION_SECTOR_COUNT(flashPartition));
500 sbufWriteU32(dst, flashfsGetSize());
501 sbufWriteU32(dst, flashfsGetOffset()); // Effectively the current number of bytes stored on the volume
502 } else
503 #endif
504
505 // FlashFS is not configured or valid device is not detected
506 {
507 sbufWriteU8(dst, 0);
508 sbufWriteU32(dst, 0);
509 sbufWriteU32(dst, 0);
510 sbufWriteU32(dst, 0);
511 }
512 }
513
514 #ifdef USE_FLASHFS
515 enum compressionType_e {
516 NO_COMPRESSION,
517 HUFFMAN
518 };
519
520 static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uint16_t size, bool useLegacyFormat, bool allowCompression)
521 {
522 STATIC_ASSERT(MSP_PORT_DATAFLASH_INFO_SIZE >= 16, MSP_PORT_DATAFLASH_INFO_SIZE_invalid);
523
524 uint16_t readLen = size;
525 const int bytesRemainingInBuf = sbufBytesRemaining(dst) - MSP_PORT_DATAFLASH_INFO_SIZE;
526 if (readLen > bytesRemainingInBuf) {
527 readLen = bytesRemainingInBuf;
528 }
529 // size will be lower than that requested if we reach end of volume
530 const uint32_t flashfsSize = flashfsGetSize();
531 if (readLen > flashfsSize - address) {
532 // truncate the request
533 readLen = flashfsSize - address;
534 }
535 sbufWriteU32(dst, address);
536
537 // legacy format does not support compression
538 #ifdef USE_HUFFMAN
539 const uint8_t compressionMethod = (!allowCompression || useLegacyFormat) ? NO_COMPRESSION : HUFFMAN;
540 #else
541 const uint8_t compressionMethod = NO_COMPRESSION;
542 UNUSED(allowCompression);
543 #endif
544
545 if (compressionMethod == NO_COMPRESSION) {
546
547 uint16_t *readLenPtr = (uint16_t *)sbufPtr(dst);
548 if (!useLegacyFormat) {
549 // new format supports variable read lengths
550 sbufWriteU16(dst, readLen);
551 sbufWriteU8(dst, 0); // placeholder for compression format
552 }
553
554 const int bytesRead = flashfsReadAbs(address, sbufPtr(dst), readLen);
555
556 if (!useLegacyFormat) {
557 // update the 'read length' with the actual amount read from flash.
558 *readLenPtr = bytesRead;
559 }
560
561 sbufAdvance(dst, bytesRead);
562
563 if (useLegacyFormat) {
564 // pad the buffer with zeros
565 for (int i = bytesRead; i < size; i++) {
566 sbufWriteU8(dst, 0);
567 }
568 }
569 } else {
570 #ifdef USE_HUFFMAN
571 // compress in 256-byte chunks
572 const uint16_t READ_BUFFER_SIZE = 256;
573 // This may be DMAable, so make it cache aligned
574 __attribute__ ((aligned(32))) uint8_t readBuffer[READ_BUFFER_SIZE];
575
576 huffmanState_t state = {
577 .bytesWritten = 0,
578 .outByte = sbufPtr(dst) + sizeof(uint16_t) + sizeof(uint8_t) + HUFFMAN_INFO_SIZE,
579 .outBufLen = readLen,
580 .outBit = 0x80,
581 };
582 *state.outByte = 0;
583
584 uint16_t bytesReadTotal = 0;
585 // read until output buffer overflows or flash is exhausted
586 while (state.bytesWritten < state.outBufLen && address + bytesReadTotal < flashfsSize) {
587 const int bytesRead = flashfsReadAbs(address + bytesReadTotal, readBuffer,
588 MIN(sizeof(readBuffer), flashfsSize - address - bytesReadTotal));
589
590 const int status = huffmanEncodeBufStreaming(&state, readBuffer, bytesRead, huffmanTable);
591 if (status == -1) {
592 // overflow
593 break;
594 }
595
596 bytesReadTotal += bytesRead;
597 }
598
599 if (state.outBit != 0x80) {
600 ++state.bytesWritten;
601 }
602
603 // header
604 sbufWriteU16(dst, HUFFMAN_INFO_SIZE + state.bytesWritten);
605 sbufWriteU8(dst, compressionMethod);
606 // payload
607 sbufWriteU16(dst, bytesReadTotal);
608 sbufAdvance(dst, state.bytesWritten);
609 #endif
610 }
611 }
612 #endif // USE_FLASHFS
613
614 /*
615 * Returns true if the command was processd, false otherwise.
616 * May set mspPostProcessFunc to a function to be called once the command has been processed
617 */
618 static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
619 {
620 UNUSED(mspPostProcessFn);
621
622 switch (cmdMSP) {
623 case MSP_API_VERSION:
624 sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
625 sbufWriteU8(dst, API_VERSION_MAJOR);
626 sbufWriteU8(dst, API_VERSION_MINOR);
627 break;
628
629 case MSP_FC_VARIANT:
630 sbufWriteData(dst, flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
631 break;
632
633 case MSP_FC_VERSION:
634 sbufWriteU8(dst, FC_VERSION_MAJOR);
635 sbufWriteU8(dst, FC_VERSION_MINOR);
636 sbufWriteU8(dst, FC_VERSION_PATCH_LEVEL);
637 break;
638
639 case MSP_BOARD_INFO:
640 {
641 sbufWriteData(dst, systemConfig()->boardIdentifier, BOARD_IDENTIFIER_LENGTH);
642 #ifdef USE_HARDWARE_REVISION_DETECTION
643 sbufWriteU16(dst, hardwareRevision);
644 #else
645 sbufWriteU16(dst, 0); // No other build targets currently have hardware revision detection.
646 #endif
647 #if defined(USE_MAX7456)
648 sbufWriteU8(dst, 2); // 2 == FC with MAX7456
649 #else
650 sbufWriteU8(dst, 0); // 0 == FC
651 #endif
652
653 // Target capabilities (uint8)
654 #define TARGET_HAS_VCP 0
655 #define TARGET_HAS_SOFTSERIAL 1
656 #define TARGET_IS_UNIFIED 2
657 #define TARGET_HAS_FLASH_BOOTLOADER 3
658 #define TARGET_SUPPORTS_CUSTOM_DEFAULTS 4
659 #define TARGET_HAS_CUSTOM_DEFAULTS 5
660 #define TARGET_SUPPORTS_RX_BIND 6
661
662 uint8_t targetCapabilities = 0;
663 #ifdef USE_VCP
664 targetCapabilities |= BIT(TARGET_HAS_VCP);
665 #endif
666 #if defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)
667 targetCapabilities |= BIT(TARGET_HAS_SOFTSERIAL);
668 #endif
669 targetCapabilities |= BIT(TARGET_IS_UNIFIED);
670 #if defined(USE_FLASH_BOOT_LOADER)
671 targetCapabilities |= BIT(TARGET_HAS_FLASH_BOOTLOADER);
672 #endif
673 #if defined(USE_CUSTOM_DEFAULTS)
674 targetCapabilities |= BIT(TARGET_SUPPORTS_CUSTOM_DEFAULTS);
675 if (hasCustomDefaults()) {
676 targetCapabilities |= BIT(TARGET_HAS_CUSTOM_DEFAULTS);
677 }
678 #endif
679 #if defined(USE_RX_BIND)
680 if (getRxBindSupported()) {
681 targetCapabilities |= BIT(TARGET_SUPPORTS_RX_BIND);
682 }
683 #endif
684
685 sbufWriteU8(dst, targetCapabilities);
686
687 // Target name with explicit length
688 sbufWriteU8(dst, strlen(targetName));
689 sbufWriteData(dst, targetName, strlen(targetName));
690
691 #if defined(USE_BOARD_INFO)
692 // Board name with explicit length
693 char *value = getBoardName();
694 sbufWriteU8(dst, strlen(value));
695 sbufWriteString(dst, value);
696
697 // Manufacturer id with explicit length
698 value = getManufacturerId();
699 sbufWriteU8(dst, strlen(value));
700 sbufWriteString(dst, value);
701 #else
702 sbufWriteU8(dst, 0);
703 sbufWriteU8(dst, 0);
704 #endif
705
706 #if defined(USE_SIGNATURE)
707 // Signature
708 sbufWriteData(dst, getSignature(), SIGNATURE_LENGTH);
709 #else
710 uint8_t emptySignature[SIGNATURE_LENGTH];
711 memset(emptySignature, 0, sizeof(emptySignature));
712 sbufWriteData(dst, &emptySignature, sizeof(emptySignature));
713 #endif
714
715 sbufWriteU8(dst, getMcuTypeId());
716
717 // Added in API version 1.42
718 sbufWriteU8(dst, systemConfig()->configurationState);
719
720 // Added in API version 1.43
721 sbufWriteU16(dst, gyro.sampleRateHz); // informational so the configurator can display the correct gyro/pid frequencies in the drop-down
722
723 // Configuration warnings / problems (uint32_t)
724 #define PROBLEM_ACC_NEEDS_CALIBRATION 0
725 #define PROBLEM_MOTOR_PROTOCOL_DISABLED 1
726
727 uint32_t configurationProblems = 0;
728
729 #if defined(USE_ACC)
730 if (!accHasBeenCalibrated()) {
731 configurationProblems |= BIT(PROBLEM_ACC_NEEDS_CALIBRATION);
732 }
733 #endif
734
735 if (!checkMotorProtocolEnabled(&motorConfig()->dev, NULL)) {
736 configurationProblems |= BIT(PROBLEM_MOTOR_PROTOCOL_DISABLED);
737 }
738
739 sbufWriteU32(dst, configurationProblems);
740
741 // Added in MSP API 1.44
742 #if defined(USE_SPI)
743 sbufWriteU8(dst, spiGetRegisteredDeviceCount());
744 #else
745 sbufWriteU8(dst, 0);
746 #endif
747 #if defined(USE_I2C)
748 sbufWriteU8(dst, i2cGetRegisteredDeviceCount());
749 #else
750 sbufWriteU8(dst, 0);
751 #endif
752
753 break;
754 }
755
756 case MSP_BUILD_INFO:
757 sbufWriteData(dst, buildDate, BUILD_DATE_LENGTH);
758 sbufWriteData(dst, buildTime, BUILD_TIME_LENGTH);
759 sbufWriteData(dst, shortGitRevision, GIT_SHORT_REVISION_LENGTH);
760 break;
761
762 case MSP_ANALOG:
763 sbufWriteU8(dst, (uint8_t)constrain(getLegacyBatteryVoltage(), 0, 255));
764 sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
765 sbufWriteU16(dst, getRssi());
766 sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send current in 0.01 A steps, range is -320A to 320A
767 sbufWriteU16(dst, getBatteryVoltage());
768 break;
769
770 case MSP_DEBUG:
771 for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) {
772 sbufWriteU16(dst, debug[i]); // 4 variables are here for general monitoring purpose
773 }
774 break;
775
776 case MSP_UID:
777 sbufWriteU32(dst, U_ID_0);
778 sbufWriteU32(dst, U_ID_1);
779 sbufWriteU32(dst, U_ID_2);
780 break;
781
782 case MSP_FEATURE_CONFIG:
783 sbufWriteU32(dst, featureConfig()->enabledFeatures);
784 break;
785
786 #ifdef USE_BEEPER
787 case MSP_BEEPER_CONFIG:
788 sbufWriteU32(dst, beeperConfig()->beeper_off_flags);
789 sbufWriteU8(dst, beeperConfig()->dshotBeaconTone);
790 sbufWriteU32(dst, beeperConfig()->dshotBeaconOffFlags);
791 break;
792 #endif
793
794 case MSP_BATTERY_STATE: {
795 // battery characteristics
796 sbufWriteU8(dst, (uint8_t)constrain(getBatteryCellCount(), 0, 255)); // 0 indicates battery not detected.
797 sbufWriteU16(dst, batteryConfig()->batteryCapacity); // in mAh
798
799 // battery state
800 sbufWriteU8(dst, (uint8_t)constrain(getLegacyBatteryVoltage(), 0, 255)); // in 0.1V steps
801 sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
802 sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send current in 0.01 A steps, range is -320A to 320A
803
804 // battery alerts
805 sbufWriteU8(dst, (uint8_t)getBatteryState());
806
807 sbufWriteU16(dst, getBatteryVoltage()); // in 0.01V steps
808 break;
809 }
810
811 case MSP_VOLTAGE_METERS: {
812 // write out id and voltage meter values, once for each meter we support
813 uint8_t count = supportedVoltageMeterCount;
814 #ifdef USE_ESC_SENSOR
815 count -= VOLTAGE_METER_ID_ESC_COUNT - getMotorCount();
816 #endif
817
818 for (int i = 0; i < count; i++) {
819
820 voltageMeter_t meter;
821 uint8_t id = (uint8_t)voltageMeterIds[i];
822 voltageMeterRead(id, &meter);
823
824 sbufWriteU8(dst, id);
825 sbufWriteU8(dst, (uint8_t)constrain((meter.displayFiltered + 5) / 10, 0, 255));
826 }
827 break;
828 }
829
830 case MSP_CURRENT_METERS: {
831 // write out id and current meter values, once for each meter we support
832 uint8_t count = supportedCurrentMeterCount;
833 #ifdef USE_ESC_SENSOR
834 count -= VOLTAGE_METER_ID_ESC_COUNT - getMotorCount();
835 #endif
836 for (int i = 0; i < count; i++) {
837
838 currentMeter_t meter;
839 uint8_t id = (uint8_t)currentMeterIds[i];
840 currentMeterRead(id, &meter);
841
842 sbufWriteU8(dst, id);
843 sbufWriteU16(dst, (uint16_t)constrain(meter.mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
844 sbufWriteU16(dst, (uint16_t)constrain(meter.amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps (mA). Negative range is truncated to zero
845 }
846 break;
847 }
848
849 case MSP_VOLTAGE_METER_CONFIG:
850 {
851 // by using a sensor type and a sub-frame length it's possible to configure any type of voltage meter,
852 // e.g. an i2c/spi/can sensor or any sensor not built directly into the FC such as ESC/RX/SPort/SBus that has
853 // different configuration requirements.
854 STATIC_ASSERT(VOLTAGE_SENSOR_ADC_VBAT == 0, VOLTAGE_SENSOR_ADC_VBAT_incorrect); // VOLTAGE_SENSOR_ADC_VBAT should be the first index
855 sbufWriteU8(dst, MAX_VOLTAGE_SENSOR_ADC); // voltage meters in payload
856 for (int i = VOLTAGE_SENSOR_ADC_VBAT; i < MAX_VOLTAGE_SENSOR_ADC; i++) {
857 const uint8_t adcSensorSubframeLength = 1 + 1 + 1 + 1 + 1; // length of id, type, vbatscale, vbatresdivval, vbatresdivmultipler, in bytes
858 sbufWriteU8(dst, adcSensorSubframeLength); // ADC sensor sub-frame length
859
860 sbufWriteU8(dst, voltageMeterADCtoIDMap[i]); // id of the sensor
861 sbufWriteU8(dst, VOLTAGE_SENSOR_TYPE_ADC_RESISTOR_DIVIDER); // indicate the type of sensor that the next part of the payload is for
862
863 sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatscale);
864 sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivval);
865 sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivmultiplier);
866 }
867 // if we had any other voltage sensors, this is where we would output any needed configuration
868 }
869
870 break;
871 case MSP_CURRENT_METER_CONFIG: {
872 // the ADC and VIRTUAL sensors have the same configuration requirements, however this API reflects
873 // that this situation may change and allows us to support configuration of any current sensor with
874 // specialist configuration requirements.
875
876 int currentMeterCount = 1;
877
878 #ifdef USE_VIRTUAL_CURRENT_METER
879 currentMeterCount++;
880 #endif
881 sbufWriteU8(dst, currentMeterCount);
882
883 const uint8_t adcSensorSubframeLength = 1 + 1 + 2 + 2; // length of id, type, scale, offset, in bytes
884 sbufWriteU8(dst, adcSensorSubframeLength);
885 sbufWriteU8(dst, CURRENT_METER_ID_BATTERY_1); // the id of the meter
886 sbufWriteU8(dst, CURRENT_SENSOR_ADC); // indicate the type of sensor that the next part of the payload is for
887 sbufWriteU16(dst, currentSensorADCConfig()->scale);
888 sbufWriteU16(dst, currentSensorADCConfig()->offset);
889
890 #ifdef USE_VIRTUAL_CURRENT_METER
891 const int8_t virtualSensorSubframeLength = 1 + 1 + 2 + 2; // length of id, type, scale, offset, in bytes
892 sbufWriteU8(dst, virtualSensorSubframeLength);
893 sbufWriteU8(dst, CURRENT_METER_ID_VIRTUAL_1); // the id of the meter
894 sbufWriteU8(dst, CURRENT_SENSOR_VIRTUAL); // indicate the type of sensor that the next part of the payload is for
895 sbufWriteU16(dst, currentSensorVirtualConfig()->scale);
896 sbufWriteU16(dst, currentSensorVirtualConfig()->offset);
897 #endif
898
899 // if we had any other current sensors, this is where we would output any needed configuration
900 break;
901 }
902
903 case MSP_BATTERY_CONFIG:
904 sbufWriteU8(dst, (batteryConfig()->vbatmincellvoltage + 5) / 10);
905 sbufWriteU8(dst, (batteryConfig()->vbatmaxcellvoltage + 5) / 10);
906 sbufWriteU8(dst, (batteryConfig()->vbatwarningcellvoltage + 5) / 10);
907 sbufWriteU16(dst, batteryConfig()->batteryCapacity);
908 sbufWriteU8(dst, batteryConfig()->voltageMeterSource);
909 sbufWriteU8(dst, batteryConfig()->currentMeterSource);
910 sbufWriteU16(dst, batteryConfig()->vbatmincellvoltage);
911 sbufWriteU16(dst, batteryConfig()->vbatmaxcellvoltage);
912 sbufWriteU16(dst, batteryConfig()->vbatwarningcellvoltage);
913 break;
914
915 case MSP_TRANSPONDER_CONFIG: {
916 #ifdef USE_TRANSPONDER
917 // Backward compatibility to BFC 3.1.1 is lost for this message type
918 sbufWriteU8(dst, TRANSPONDER_PROVIDER_COUNT);
919 for (unsigned int i = 0; i < TRANSPONDER_PROVIDER_COUNT; i++) {
920 sbufWriteU8(dst, transponderRequirements[i].provider);
921 sbufWriteU8(dst, transponderRequirements[i].dataLength);
922 }
923
924 uint8_t provider = transponderConfig()->provider;
925 sbufWriteU8(dst, provider);
926
927 if (provider) {
928 uint8_t requirementIndex = provider - 1;
929 uint8_t providerDataLength = transponderRequirements[requirementIndex].dataLength;
930
931 for (unsigned int i = 0; i < providerDataLength; i++) {
932 sbufWriteU8(dst, transponderConfig()->data[i]);
933 }
934 }
935 #else
936 sbufWriteU8(dst, 0); // no providers
937 #endif
938 break;
939 }
940
941 case MSP_OSD_CONFIG: {
942 #define OSD_FLAGS_OSD_FEATURE (1 << 0)
943 //#define OSD_FLAGS_OSD_SLAVE (1 << 1)
944 #define OSD_FLAGS_RESERVED_1 (1 << 2)
945 #define OSD_FLAGS_OSD_HARDWARE_FRSKYOSD (1 << 3)
946 #define OSD_FLAGS_OSD_HARDWARE_MAX_7456 (1 << 4)
947 #define OSD_FLAGS_OSD_DEVICE_DETECTED (1 << 5)
948 #define OSD_FLAGS_OSD_MSP_DEVICE (1 << 6)
949
950 uint8_t osdFlags = 0;
951 #if defined(USE_OSD)
952 osdFlags |= OSD_FLAGS_OSD_FEATURE;
953
954 osdDisplayPortDevice_e deviceType;
955 displayPort_t *osdDisplayPort = osdGetDisplayPort(&deviceType);
956 bool displayIsReady = osdDisplayPort && displayCheckReady(osdDisplayPort, true);
957 switch (deviceType) {
958 case OSD_DISPLAYPORT_DEVICE_MAX7456:
959 osdFlags |= OSD_FLAGS_OSD_HARDWARE_MAX_7456;
960 if (displayIsReady) {
961 osdFlags |= OSD_FLAGS_OSD_DEVICE_DETECTED;
962 }
963
964 break;
965 case OSD_DISPLAYPORT_DEVICE_FRSKYOSD:
966 osdFlags |= OSD_FLAGS_OSD_HARDWARE_FRSKYOSD;
967 if (displayIsReady) {
968 osdFlags |= OSD_FLAGS_OSD_DEVICE_DETECTED;
969 }
970
971 break;
972 case OSD_DISPLAYPORT_DEVICE_MSP:
973 osdFlags |= OSD_FLAGS_OSD_MSP_DEVICE;
974 if (displayIsReady) {
975 osdFlags |= OSD_FLAGS_OSD_DEVICE_DETECTED;
976 }
977
978 break;
979 default:
980 break;
981 }
982 #endif
983 sbufWriteU8(dst, osdFlags);
984
985 #ifdef USE_MAX7456
986 // send video system (AUTO/PAL/NTSC)
987 sbufWriteU8(dst, vcdProfile()->video_system);
988 #else
989 sbufWriteU8(dst, 0);
990 #endif
991
992 #ifdef USE_OSD
993 // OSD specific, not applicable to OSD slaves.
994
995 // Configuration
996 sbufWriteU8(dst, osdConfig()->units);
997
998 // Alarms
999 sbufWriteU8(dst, osdConfig()->rssi_alarm);
1000 sbufWriteU16(dst, osdConfig()->cap_alarm);
1001
1002 // Reuse old timer alarm (U16) as OSD_ITEM_COUNT
1003 sbufWriteU8(dst, 0);
1004 sbufWriteU8(dst, OSD_ITEM_COUNT);
1005
1006 sbufWriteU16(dst, osdConfig()->alt_alarm);
1007
1008 // Element position and visibility
1009 for (int i = 0; i < OSD_ITEM_COUNT; i++) {
1010 sbufWriteU16(dst, osdElementConfig()->item_pos[i]);
1011 }
1012
1013 // Post flight statistics
1014 sbufWriteU8(dst, OSD_STAT_COUNT);
1015 for (int i = 0; i < OSD_STAT_COUNT; i++ ) {
1016 sbufWriteU8(dst, osdStatGetState(i));
1017 }
1018
1019 // Timers
1020 sbufWriteU8(dst, OSD_TIMER_COUNT);
1021 for (int i = 0; i < OSD_TIMER_COUNT; i++) {
1022 sbufWriteU16(dst, osdConfig()->timers[i]);
1023 }
1024
1025 // Enabled warnings
1026 // Send low word first for backwards compatibility (API < 1.41)
1027 sbufWriteU16(dst, (uint16_t)(osdConfig()->enabledWarnings & 0xFFFF));
1028 // API >= 1.41
1029 // Send the warnings count and 32bit enabled warnings flags.
1030 // Add currently active OSD profile (0 indicates OSD profiles not available).
1031 // Add OSD stick overlay mode (0 indicates OSD stick overlay not available).
1032 sbufWriteU8(dst, OSD_WARNING_COUNT);
1033 sbufWriteU32(dst, osdConfig()->enabledWarnings);
1034
1035 #ifdef USE_OSD_PROFILES
1036 sbufWriteU8(dst, OSD_PROFILE_COUNT); // available profiles
1037 sbufWriteU8(dst, osdConfig()->osdProfileIndex); // selected profile
1038 #else
1039 // If the feature is not available there is only 1 profile and it's always selected
1040 sbufWriteU8(dst, 1);
1041 sbufWriteU8(dst, 1);
1042 #endif // USE_OSD_PROFILES
1043
1044 #ifdef USE_OSD_STICK_OVERLAY
1045 sbufWriteU8(dst, osdConfig()->overlay_radio_mode);
1046 #else
1047 sbufWriteU8(dst, 0);
1048 #endif // USE_OSD_STICK_OVERLAY
1049
1050 // API >= 1.43
1051 // Add the camera frame element width/height
1052 sbufWriteU8(dst, osdConfig()->camera_frame_width);
1053 sbufWriteU8(dst, osdConfig()->camera_frame_height);
1054
1055 #endif // USE_OSD
1056 break;
1057 }
1058
1059 default:
1060 return false;
1061 }
1062 return true;
1063 }
1064
1065 static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *dst)
1066 {
1067 bool unsupportedCommand = false;
1068
1069 #if !defined(USE_VTX_COMMON) || !defined(USE_VTX_MSP)
1070 UNUSED(srcDesc);
1071 #endif
1072
1073 switch (cmdMSP) {
1074 case MSP_STATUS_EX:
1075 case MSP_STATUS:
1076 {
1077 boxBitmask_t flightModeFlags;
1078 const int flagBits = packFlightModeFlags(&flightModeFlags);
1079
1080 sbufWriteU16(dst, getTaskDeltaTimeUs(TASK_PID));
1081 #ifdef USE_I2C
1082 sbufWriteU16(dst, i2cGetErrorCounter());
1083 #else
1084 sbufWriteU16(dst, 0);
1085 #endif
1086 sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_RANGEFINDER) << 4 | sensors(SENSOR_GYRO) << 5);
1087 sbufWriteData(dst, &flightModeFlags, 4); // unconditional part of flags, first 32 bits
1088 sbufWriteU8(dst, getCurrentPidProfileIndex());
1089 sbufWriteU16(dst, constrain(getAverageSystemLoadPercent(), 0, LOAD_PERCENTAGE_ONE));
1090 if (cmdMSP == MSP_STATUS_EX) {
1091 sbufWriteU8(dst, PID_PROFILE_COUNT);
1092 sbufWriteU8(dst, getCurrentControlRateProfileIndex());
1093 } else { // MSP_STATUS
1094 sbufWriteU16(dst, 0); // gyro cycle time
1095 }
1096
1097 // write flightModeFlags header. Lowest 4 bits contain number of bytes that follow
1098 // header is emited even when all bits fit into 32 bits to allow future extension
1099 int byteCount = (flagBits - 32 + 7) / 8; // 32 already stored, round up
1100 byteCount = constrain(byteCount, 0, 15); // limit to 16 bytes (128 bits)
1101 sbufWriteU8(dst, byteCount);
1102 sbufWriteData(dst, ((uint8_t*)&flightModeFlags) + 4, byteCount);
1103
1104 // Write arming disable flags
1105 // 1 byte, flag count
1106 sbufWriteU8(dst, ARMING_DISABLE_FLAGS_COUNT);
1107 // 4 bytes, flags
1108 const uint32_t armingDisableFlags = getArmingDisableFlags();
1109 sbufWriteU32(dst, armingDisableFlags);
1110
1111 // config state flags - bits to indicate the state of the configuration, reboot required, etc.
1112 // other flags can be added as needed
1113 sbufWriteU8(dst, (getRebootRequired() << 0));
1114 }
1115 break;
1116
1117 case MSP_RAW_IMU:
1118 {
1119 #if defined(USE_ACC)
1120 // Hack scale due to choice of units for sensor data in multiwii
1121
1122 uint8_t scale;
1123 if (acc.dev.acc_1G > 512 * 4) {
1124 scale = 8;
1125 } else if (acc.dev.acc_1G > 512 * 2) {
1126 scale = 4;
1127 } else if (acc.dev.acc_1G >= 512) {
1128 scale = 2;
1129 } else {
1130 scale = 1;
1131 }
1132 #endif
1133
1134 for (int i = 0; i < 3; i++) {
1135 #if defined(USE_ACC)
1136 sbufWriteU16(dst, lrintf(acc.accADC[i] / scale));
1137 #else
1138 sbufWriteU16(dst, 0);
1139 #endif
1140 }
1141 for (int i = 0; i < 3; i++) {
1142 sbufWriteU16(dst, gyroRateDps(i));
1143 }
1144 for (int i = 0; i < 3; i++) {
1145 #if defined(USE_MAG)
1146 sbufWriteU16(dst, lrintf(mag.magADC[i]));
1147 #else
1148 sbufWriteU16(dst, 0);
1149 #endif
1150 }
1151 }
1152 break;
1153
1154 #ifdef USE_SERVOS
1155 case MSP_SERVO:
1156 sbufWriteData(dst, &servo, MAX_SUPPORTED_SERVOS * 2);
1157 break;
1158 case MSP_SERVO_CONFIGURATIONS:
1159 for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
1160 sbufWriteU16(dst, servoParams(i)->min);
1161 sbufWriteU16(dst, servoParams(i)->max);
1162 sbufWriteU16(dst, servoParams(i)->middle);
1163 sbufWriteU8(dst, servoParams(i)->rate);
1164 sbufWriteU8(dst, servoParams(i)->forwardFromChannel);
1165 sbufWriteU32(dst, servoParams(i)->reversedSources);
1166 }
1167 break;
1168
1169 case MSP_SERVO_MIX_RULES:
1170 for (int i = 0; i < MAX_SERVO_RULES; i++) {
1171 sbufWriteU8(dst, customServoMixers(i)->targetChannel);
1172 sbufWriteU8(dst, customServoMixers(i)->inputSource);
1173 sbufWriteU8(dst, customServoMixers(i)->rate);
1174 sbufWriteU8(dst, customServoMixers(i)->speed);
1175 sbufWriteU8(dst, customServoMixers(i)->min);
1176 sbufWriteU8(dst, customServoMixers(i)->max);
1177 sbufWriteU8(dst, customServoMixers(i)->box);
1178 }
1179 break;
1180 #endif
1181
1182 case MSP_MOTOR:
1183 for (unsigned i = 0; i < 8; i++) {
1184 #ifdef USE_MOTOR
1185 if (!motorIsEnabled() || i >= MAX_SUPPORTED_MOTORS || !motorIsMotorEnabled(i)) {
1186 sbufWriteU16(dst, 0);
1187 continue;
1188 }
1189
1190 sbufWriteU16(dst, motorConvertToExternal(motor[i]));
1191 #else
1192 sbufWriteU16(dst, 0);
1193 #endif
1194 }
1195
1196 break;
1197
1198 // Added in API version 1.42
1199 case MSP_MOTOR_TELEMETRY:
1200 sbufWriteU8(dst, getMotorCount());
1201 for (unsigned i = 0; i < getMotorCount(); i++) {
1202 int rpm = 0;
1203 uint16_t invalidPct = 0;
1204 uint8_t escTemperature = 0; // degrees celcius
1205 uint16_t escVoltage = 0; // 0.01V per unit
1206 uint16_t escCurrent = 0; // 0.01A per unit
1207 uint16_t escConsumption = 0; // mAh
1208
1209 bool rpmDataAvailable = false;
1210
1211 #ifdef USE_DSHOT_TELEMETRY
1212 if (motorConfig()->dev.useDshotTelemetry) {
1213 rpm = erpmToRpm(getDshotTelemetry(i));
1214 rpmDataAvailable = true;
1215 invalidPct = 10000; // 100.00%
1216
1217
1218 #ifdef USE_DSHOT_TELEMETRY_STATS
1219 if (isDshotMotorTelemetryActive(i)) {
1220 invalidPct = getDshotTelemetryMotorInvalidPercent(i);
1221 }
1222 #endif
1223
1224
1225 // Provide extended dshot telemetry
1226 if ((dshotTelemetryState.motorState[i].telemetryTypes & DSHOT_EXTENDED_TELEMETRY_MASK) != 0) {
1227 // Temperature Celsius [0, 1, ..., 255] in degree Celsius, just like Blheli_32 and KISS
1228 if ((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_TEMPERATURE)) != 0) {
1229 escTemperature = dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE];
1230 }
1231
1232 // Current -> 0-255A step 1A
1233 if ((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_CURRENT)) != 0) {
1234 escCurrent = dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_CURRENT];
1235 }
1236
1237 // Voltage -> 0-63,75V step 0,25V
1238 if ((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_VOLTAGE)) != 0) {
1239 escVoltage = dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_VOLTAGE] >> 2;
1240 }
1241 }
1242 }
1243 #endif
1244
1245 #ifdef USE_ESC_SENSOR
1246 if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
1247 escSensorData_t *escData = getEscSensorData(i);
1248 if (!rpmDataAvailable) { // We want DSHOT telemetry RPM data (if available) to have precedence
1249 rpm = erpmToRpm(escData->rpm);
1250 rpmDataAvailable = true;
1251 }
1252 escTemperature = escData->temperature;
1253 escVoltage = escData->voltage;
1254 escCurrent = escData->current;
1255 escConsumption = escData->consumption;
1256 }
1257 #endif
1258
1259 sbufWriteU32(dst, (rpmDataAvailable ? rpm : 0));
1260 sbufWriteU16(dst, invalidPct);
1261 sbufWriteU8(dst, escTemperature);
1262 sbufWriteU16(dst, escVoltage);
1263 sbufWriteU16(dst, escCurrent);
1264 sbufWriteU16(dst, escConsumption);
1265 }
1266 break;
1267
1268 case MSP2_MOTOR_OUTPUT_REORDERING:
1269 {
1270 sbufWriteU8(dst, MAX_SUPPORTED_MOTORS);
1271
1272 for (unsigned i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
1273 sbufWriteU8(dst, motorConfig()->dev.motorOutputReordering[i]);
1274 }
1275 }
1276 break;
1277
1278 #ifdef USE_VTX_COMMON
1279 case MSP2_GET_VTX_DEVICE_STATUS:
1280 {
1281 const vtxDevice_t *vtxDevice = vtxCommonDevice();
1282 vtxCommonSerializeDeviceStatus(vtxDevice, dst);
1283 }
1284 break;
1285 #endif
1286
1287 #ifdef USE_OSD
1288 case MSP2_GET_OSD_WARNINGS:
1289 {
1290 bool isBlinking;
1291 uint8_t displayAttr;
1292 char warningsBuffer[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
1293
1294 renderOsdWarning(warningsBuffer, &isBlinking, &displayAttr);
1295 const uint8_t warningsLen = strlen(warningsBuffer);
1296
1297 if (isBlinking) {
1298 displayAttr |= DISPLAYPORT_ATTR_BLINK;
1299 }
1300 sbufWriteU8(dst, displayAttr); // see displayPortAttr_e
1301 sbufWriteU8(dst, warningsLen); // length byte followed by the actual characters
1302 for (unsigned i = 0; i < warningsLen; i++) {
1303 sbufWriteU8(dst, warningsBuffer[i]);
1304 }
1305 break;
1306 }
1307 #endif
1308
1309 case MSP_RC:
1310 for (int i = 0; i < rxRuntimeState.channelCount; i++) {
1311 sbufWriteU16(dst, rcData[i]);
1312 }
1313 break;
1314
1315 case MSP_ATTITUDE:
1316 sbufWriteU16(dst, attitude.values.roll);
1317 sbufWriteU16(dst, attitude.values.pitch);
1318 sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
1319 break;
1320
1321 case MSP_ALTITUDE:
1322 sbufWriteU32(dst, getEstimatedAltitudeCm());
1323 #ifdef USE_VARIO
1324 sbufWriteU16(dst, getEstimatedVario());
1325 #else
1326 sbufWriteU16(dst, 0);
1327 #endif
1328 break;
1329
1330 case MSP_SONAR_ALTITUDE:
1331 #if defined(USE_RANGEFINDER)
1332 sbufWriteU32(dst, rangefinderGetLatestAltitude());
1333 #else
1334 sbufWriteU32(dst, 0);
1335 #endif
1336 break;
1337
1338 case MSP_BOARD_ALIGNMENT_CONFIG:
1339 sbufWriteU16(dst, boardAlignment()->rollDegrees);
1340 sbufWriteU16(dst, boardAlignment()->pitchDegrees);
1341 sbufWriteU16(dst, boardAlignment()->yawDegrees);
1342 break;
1343
1344 case MSP_ARMING_CONFIG:
1345 sbufWriteU8(dst, armingConfig()->auto_disarm_delay);
1346 sbufWriteU8(dst, 0);
1347 sbufWriteU8(dst, imuConfig()->small_angle);
1348 break;
1349
1350 case MSP_RC_TUNING:
1351 sbufWriteU8(dst, currentControlRateProfile->rcRates[FD_ROLL]);
1352 sbufWriteU8(dst, currentControlRateProfile->rcExpo[FD_ROLL]);
1353 for (int i = 0 ; i < 3; i++) {
1354 sbufWriteU8(dst, currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
1355 }
1356 sbufWriteU8(dst, 0); // was currentControlRateProfile->tpa_rate
1357 sbufWriteU8(dst, currentControlRateProfile->thrMid8);
1358 sbufWriteU8(dst, currentControlRateProfile->thrExpo8);
1359 sbufWriteU16(dst, 0); // was currentControlRateProfile->tpa_breakpoint
1360 sbufWriteU8(dst, currentControlRateProfile->rcExpo[FD_YAW]);
1361 sbufWriteU8(dst, currentControlRateProfile->rcRates[FD_YAW]);
1362 sbufWriteU8(dst, currentControlRateProfile->rcRates[FD_PITCH]);
1363 sbufWriteU8(dst, currentControlRateProfile->rcExpo[FD_PITCH]);
1364
1365 // added in 1.41
1366 sbufWriteU8(dst, currentControlRateProfile->throttle_limit_type);
1367 sbufWriteU8(dst, currentControlRateProfile->throttle_limit_percent);
1368
1369 // added in 1.42
1370 sbufWriteU16(dst, currentControlRateProfile->rate_limit[FD_ROLL]);
1371 sbufWriteU16(dst, currentControlRateProfile->rate_limit[FD_PITCH]);
1372 sbufWriteU16(dst, currentControlRateProfile->rate_limit[FD_YAW]);
1373
1374 // added in 1.43
1375 sbufWriteU8(dst, currentControlRateProfile->rates_type);
1376
1377 break;
1378
1379 case MSP_PID:
1380 for (int i = 0; i < PID_ITEM_COUNT; i++) {
1381 sbufWriteU8(dst, currentPidProfile->pid[i].P);
1382 sbufWriteU8(dst, currentPidProfile->pid[i].I);
1383 sbufWriteU8(dst, currentPidProfile->pid[i].D);
1384 }
1385 break;
1386
1387 case MSP_PIDNAMES:
1388 for (const char *c = pidNames; *c; c++) {
1389 sbufWriteU8(dst, *c);
1390 }
1391 break;
1392
1393 case MSP_PID_CONTROLLER:
1394 sbufWriteU8(dst, PID_CONTROLLER_BETAFLIGHT);
1395 break;
1396
1397 case MSP_MODE_RANGES:
1398 for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
1399 const modeActivationCondition_t *mac = modeActivationConditions(i);
1400 const box_t *box = findBoxByBoxId(mac->modeId);
1401 sbufWriteU8(dst, box->permanentId);
1402 sbufWriteU8(dst, mac->auxChannelIndex);
1403 sbufWriteU8(dst, mac->range.startStep);
1404 sbufWriteU8(dst, mac->range.endStep);
1405 }
1406 break;
1407
1408 case MSP_MODE_RANGES_EXTRA:
1409 sbufWriteU8(dst, MAX_MODE_ACTIVATION_CONDITION_COUNT); // prepend number of EXTRAs array elements
1410
1411 for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
1412 const modeActivationCondition_t *mac = modeActivationConditions(i);
1413 const box_t *box = findBoxByBoxId(mac->modeId);
1414 const box_t *linkedBox = findBoxByBoxId(mac->linkedTo);
1415 sbufWriteU8(dst, box->permanentId); // each element is aligned with MODE_RANGES by the permanentId
1416 sbufWriteU8(dst, mac->modeLogic);
1417 sbufWriteU8(dst, linkedBox->permanentId);
1418 }
1419 break;
1420
1421 case MSP_ADJUSTMENT_RANGES:
1422 for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
1423 const adjustmentRange_t *adjRange = adjustmentRanges(i);
1424 sbufWriteU8(dst, 0); // was adjRange->adjustmentIndex
1425 sbufWriteU8(dst, adjRange->auxChannelIndex);
1426 sbufWriteU8(dst, adjRange->range.startStep);
1427 sbufWriteU8(dst, adjRange->range.endStep);
1428 sbufWriteU8(dst, adjRange->adjustmentConfig);
1429 sbufWriteU8(dst, adjRange->auxSwitchChannelIndex);
1430 }
1431 break;
1432
1433 case MSP_MOTOR_CONFIG:
1434 sbufWriteU16(dst, motorConfig()->minthrottle);
1435 sbufWriteU16(dst, motorConfig()->maxthrottle);
1436 sbufWriteU16(dst, motorConfig()->mincommand);
1437
1438 // API 1.42
1439 sbufWriteU8(dst, getMotorCount());
1440 sbufWriteU8(dst, motorConfig()->motorPoleCount);
1441 #ifdef USE_DSHOT_TELEMETRY
1442 sbufWriteU8(dst, motorConfig()->dev.useDshotTelemetry);
1443 #else
1444 sbufWriteU8(dst, 0);
1445 #endif
1446
1447 #ifdef USE_ESC_SENSOR
1448 sbufWriteU8(dst, featureIsEnabled(FEATURE_ESC_SENSOR)); // ESC sensor available
1449 #else
1450 sbufWriteU8(dst, 0);
1451 #endif
1452 break;
1453
1454 // Deprecated in favor of MSP_MOTOR_TELEMETY as of API version 1.42
1455 // Used by DJI FPV
1456 case MSP_ESC_SENSOR_DATA:
1457 #if defined(USE_ESC_SENSOR)
1458 if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
1459 sbufWriteU8(dst, getMotorCount());
1460 for (int i = 0; i < getMotorCount(); i++) {
1461 const escSensorData_t *escData = getEscSensorData(i);
1462 sbufWriteU8(dst, escData->temperature);
1463 sbufWriteU16(dst, escData->rpm);
1464 }
1465 } else
1466 #endif
1467 #if defined(USE_DSHOT_TELEMETRY)
1468 if (motorConfig()->dev.useDshotTelemetry) {
1469 sbufWriteU8(dst, getMotorCount());
1470 for (int i = 0; i < getMotorCount(); i++) {
1471 sbufWriteU8(dst, dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE]);
1472 sbufWriteU16(dst, getDshotTelemetry(i) * 100 * 2 / motorConfig()->motorPoleCount);
1473 }
1474 }
1475 else
1476 #endif
1477 {
1478 unsupportedCommand = true;
1479 }
1480
1481 break;
1482
1483 #ifdef USE_GPS
1484 case MSP_GPS_CONFIG:
1485 sbufWriteU8(dst, gpsConfig()->provider);
1486 sbufWriteU8(dst, gpsConfig()->sbasMode);
1487 sbufWriteU8(dst, gpsConfig()->autoConfig);
1488 sbufWriteU8(dst, gpsConfig()->autoBaud);
1489 // Added in API version 1.43
1490 sbufWriteU8(dst, gpsConfig()->gps_set_home_point_once);
1491 sbufWriteU8(dst, gpsConfig()->gps_ublox_use_galileo);
1492 break;
1493
1494 case MSP_RAW_GPS:
1495 sbufWriteU8(dst, STATE(GPS_FIX));
1496 sbufWriteU8(dst, gpsSol.numSat);
1497 sbufWriteU32(dst, gpsSol.llh.lat);
1498 sbufWriteU32(dst, gpsSol.llh.lon);
1499 sbufWriteU16(dst, (uint16_t)constrain(gpsSol.llh.altCm / 100, 0, UINT16_MAX)); // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. To maintain backwards compatibility compensate to 1m per lsb in MSP again.
1500 sbufWriteU16(dst, gpsSol.groundSpeed);
1501 sbufWriteU16(dst, gpsSol.groundCourse);
1502 // Added in API version 1.44
1503 sbufWriteU16(dst, gpsSol.dop.hdop);
1504 break;
1505
1506 case MSP_COMP_GPS:
1507 sbufWriteU16(dst, GPS_distanceToHome);
1508 sbufWriteU16(dst, GPS_directionToHome / 10); // resolution increased in Betaflight 4.4 by factor of 10, this maintains backwards compatibility for DJI OSD
1509 sbufWriteU8(dst, GPS_update & 1);
1510 break;
1511
1512 case MSP_GPSSVINFO:
1513 sbufWriteU8(dst, GPS_numCh);
1514 for (int i = 0; i < GPS_numCh; i++) {
1515 sbufWriteU8(dst, GPS_svinfo_chn[i]);
1516 sbufWriteU8(dst, GPS_svinfo_svid[i]);
1517 sbufWriteU8(dst, GPS_svinfo_quality[i]);
1518 sbufWriteU8(dst, GPS_svinfo_cno[i]);
1519 }
1520 break;
1521
1522 #ifdef USE_GPS_RESCUE
1523 case MSP_GPS_RESCUE:
1524 sbufWriteU16(dst, gpsRescueConfig()->angle);
1525 sbufWriteU16(dst, gpsRescueConfig()->initialAltitudeM);
1526 sbufWriteU16(dst, gpsRescueConfig()->descentDistanceM);
1527 sbufWriteU16(dst, gpsRescueConfig()->rescueGroundspeed);
1528 sbufWriteU16(dst, gpsRescueConfig()->throttleMin);
1529 sbufWriteU16(dst, gpsRescueConfig()->throttleMax);
1530 sbufWriteU16(dst, gpsRescueConfig()->throttleHover);
1531 sbufWriteU8(dst, gpsRescueConfig()->sanityChecks);
1532 sbufWriteU8(dst, gpsRescueConfig()->minSats);
1533
1534 // Added in API version 1.43
1535 sbufWriteU16(dst, gpsRescueConfig()->ascendRate);
1536 sbufWriteU16(dst, gpsRescueConfig()->descendRate);
1537 sbufWriteU8(dst, gpsRescueConfig()->allowArmingWithoutFix);
1538 sbufWriteU8(dst, gpsRescueConfig()->altitudeMode);
1539 // Added in API version 1.44
1540 sbufWriteU16(dst, gpsRescueConfig()->minRescueDth);
1541 break;
1542
1543 case MSP_GPS_RESCUE_PIDS:
1544 sbufWriteU16(dst, gpsRescueConfig()->throttleP);
1545 sbufWriteU16(dst, gpsRescueConfig()->throttleI);
1546 sbufWriteU16(dst, gpsRescueConfig()->throttleD);
1547 sbufWriteU16(dst, gpsRescueConfig()->velP);
1548 sbufWriteU16(dst, gpsRescueConfig()->velI);
1549 sbufWriteU16(dst, gpsRescueConfig()->velD);
1550 sbufWriteU16(dst, gpsRescueConfig()->yawP);
1551 break;
1552 #endif
1553 #endif
1554
1555 #if defined(USE_ACC)
1556 case MSP_ACC_TRIM:
1557 sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.pitch);
1558 sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.roll);
1559
1560 break;
1561 #endif
1562 case MSP_MIXER_CONFIG:
1563 sbufWriteU8(dst, mixerConfig()->mixerMode);
1564 sbufWriteU8(dst, mixerConfig()->yaw_motors_reversed);
1565 break;
1566
1567 case MSP_RX_CONFIG:
1568 sbufWriteU8(dst, rxConfig()->serialrx_provider);
1569 sbufWriteU16(dst, rxConfig()->maxcheck);
1570 sbufWriteU16(dst, rxConfig()->midrc);
1571 sbufWriteU16(dst, rxConfig()->mincheck);
1572 sbufWriteU8(dst, rxConfig()->spektrum_sat_bind);
1573 sbufWriteU16(dst, rxConfig()->rx_min_usec);
1574 sbufWriteU16(dst, rxConfig()->rx_max_usec);
1575 sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rcInterpolation
1576 sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rcInterpolationInterval
1577 sbufWriteU16(dst, rxConfig()->airModeActivateThreshold * 10 + 1000);
1578 #ifdef USE_RX_SPI
1579 sbufWriteU8(dst, rxSpiConfig()->rx_spi_protocol);
1580 sbufWriteU32(dst, rxSpiConfig()->rx_spi_id);
1581 sbufWriteU8(dst, rxSpiConfig()->rx_spi_rf_channel_count);
1582 #else
1583 sbufWriteU8(dst, 0);
1584 sbufWriteU32(dst, 0);
1585 sbufWriteU8(dst, 0);
1586 #endif
1587 sbufWriteU8(dst, rxConfig()->fpvCamAngleDegrees);
1588 sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rcSmoothingChannels
1589 #if defined(USE_RC_SMOOTHING_FILTER)
1590 sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rc_smoothing_type
1591 sbufWriteU8(dst, rxConfig()->rc_smoothing_setpoint_cutoff);
1592 sbufWriteU8(dst, rxConfig()->rc_smoothing_feedforward_cutoff);
1593 sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rc_smoothing_input_type
1594 sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rc_smoothing_derivative_type
1595 #else
1596 sbufWriteU8(dst, 0);
1597 sbufWriteU8(dst, 0);
1598 sbufWriteU8(dst, 0);
1599 sbufWriteU8(dst, 0);
1600 sbufWriteU8(dst, 0);
1601 #endif
1602 #if defined(USE_USB_CDC_HID)
1603 sbufWriteU8(dst, usbDevConfig()->type);
1604 #else
1605 sbufWriteU8(dst, 0);
1606 #endif
1607 // Added in MSP API 1.42
1608 #if defined(USE_RC_SMOOTHING_FILTER)
1609 sbufWriteU8(dst, rxConfig()->rc_smoothing_auto_factor_rpy);
1610 #else
1611 sbufWriteU8(dst, 0);
1612 #endif
1613 // Added in MSP API 1.44
1614 #if defined(USE_RC_SMOOTHING_FILTER)
1615 sbufWriteU8(dst, rxConfig()->rc_smoothing_mode);
1616 #else
1617 sbufWriteU8(dst, 0);
1618 #endif
1619
1620 // Added in MSP API 1.45
1621 #ifdef USE_RX_EXPRESSLRS
1622 sbufWriteData(dst, rxExpressLrsSpiConfig()->UID, sizeof(rxExpressLrsSpiConfig()->UID));
1623 #else
1624 uint8_t emptyUid[6];
1625 memset(emptyUid, 0, sizeof(emptyUid));
1626 sbufWriteData(dst, &emptyUid, sizeof(emptyUid));
1627 #endif
1628 break;
1629 case MSP_FAILSAFE_CONFIG:
1630 sbufWriteU8(dst, failsafeConfig()->failsafe_delay);
1631 sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay);
1632 sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
1633 sbufWriteU8(dst, failsafeConfig()->failsafe_switch_mode);
1634 sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay);
1635 sbufWriteU8(dst, failsafeConfig()->failsafe_procedure);
1636 break;
1637
1638 case MSP_RXFAIL_CONFIG:
1639 for (int i = 0; i < rxRuntimeState.channelCount; i++) {
1640 sbufWriteU8(dst, rxFailsafeChannelConfigs(i)->mode);
1641 sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxFailsafeChannelConfigs(i)->step));
1642 }
1643 break;
1644
1645 case MSP_RSSI_CONFIG:
1646 sbufWriteU8(dst, rxConfig()->rssi_channel);
1647 break;
1648
1649 case MSP_RX_MAP:
1650 sbufWriteData(dst, rxConfig()->rcmap, RX_MAPPABLE_CHANNEL_COUNT);
1651 break;
1652
1653 case MSP_CF_SERIAL_CONFIG:
1654 for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
1655 if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
1656 continue;
1657 };
1658 sbufWriteU8(dst, serialConfig()->portConfigs[i].identifier);
1659 sbufWriteU16(dst, serialConfig()->portConfigs[i].functionMask);
1660 sbufWriteU8(dst, serialConfig()->portConfigs[i].msp_baudrateIndex);
1661 sbufWriteU8(dst, serialConfig()->portConfigs[i].gps_baudrateIndex);
1662 sbufWriteU8(dst, serialConfig()->portConfigs[i].telemetry_baudrateIndex);
1663 sbufWriteU8(dst, serialConfig()->portConfigs[i].blackbox_baudrateIndex);
1664 }
1665 break;
1666 case MSP2_COMMON_SERIAL_CONFIG: {
1667 uint8_t count = 0;
1668 for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
1669 if (serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
1670 count++;
1671 }
1672 }
1673 sbufWriteU8(dst, count);
1674 for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
1675 if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
1676 continue;
1677 };
1678 sbufWriteU8(dst, serialConfig()->portConfigs[i].identifier);
1679 sbufWriteU32(dst, serialConfig()->portConfigs[i].functionMask);
1680 sbufWriteU8(dst, serialConfig()->portConfigs[i].msp_baudrateIndex);
1681 sbufWriteU8(dst, serialConfig()->portConfigs[i].gps_baudrateIndex);
1682 sbufWriteU8(dst, serialConfig()->portConfigs[i].telemetry_baudrateIndex);
1683 sbufWriteU8(dst, serialConfig()->portConfigs[i].blackbox_baudrateIndex);
1684 }
1685 break;
1686 }
1687
1688 #ifdef USE_LED_STRIP_STATUS_MODE
1689 case MSP_LED_COLORS:
1690 for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
1691 const hsvColor_t *color = &ledStripStatusModeConfig()->colors[i];
1692 sbufWriteU16(dst, color->h);
1693 sbufWriteU8(dst, color->s);
1694 sbufWriteU8(dst, color->v);
1695 }
1696 break;
1697 #endif
1698
1699 #ifdef USE_LED_STRIP
1700 case MSP_LED_STRIP_CONFIG:
1701 for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
1702 #ifdef USE_LED_STRIP_STATUS_MODE
1703 const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[i];
1704 sbufWriteU32(dst, *ledConfig);
1705 #else
1706 sbufWriteU32(dst, 0);
1707 #endif
1708 }
1709
1710 // API 1.41 - add indicator for advanced profile support and the current profile selection
1711 // 0 = basic ledstrip available
1712 // 1 = advanced ledstrip available
1713 #ifdef USE_LED_STRIP_STATUS_MODE
1714 sbufWriteU8(dst, 1); // advanced ledstrip available
1715 #else
1716 sbufWriteU8(dst, 0); // only simple ledstrip available
1717 #endif
1718 sbufWriteU8(dst, ledStripConfig()->ledstrip_profile);
1719 break;
1720 #endif
1721
1722 #ifdef USE_LED_STRIP_STATUS_MODE
1723 case MSP_LED_STRIP_MODECOLOR:
1724 for (int i = 0; i < LED_MODE_COUNT; i++) {
1725 for (int j = 0; j < LED_DIRECTION_COUNT; j++) {
1726 sbufWriteU8(dst, i);
1727 sbufWriteU8(dst, j);
1728 sbufWriteU8(dst, ledStripStatusModeConfig()->modeColors[i].color[j]);
1729 }
1730 }
1731
1732 for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) {
1733 sbufWriteU8(dst, LED_MODE_COUNT);
1734 sbufWriteU8(dst, j);
1735 sbufWriteU8(dst, ledStripStatusModeConfig()->specialColors.color[j]);
1736 }
1737
1738 sbufWriteU8(dst, LED_AUX_CHANNEL);
1739 sbufWriteU8(dst, 0);
1740 sbufWriteU8(dst, ledStripStatusModeConfig()->ledstrip_aux_channel);
1741 break;
1742 #endif
1743
1744 case MSP_DATAFLASH_SUMMARY:
1745 serializeDataflashSummaryReply(dst);
1746 break;
1747
1748 case MSP_BLACKBOX_CONFIG:
1749 #ifdef USE_BLACKBOX
1750 sbufWriteU8(dst, 1); //Blackbox supported
1751 sbufWriteU8(dst, blackboxConfig()->device);
1752 sbufWriteU8(dst, 1); // Rate numerator, not used anymore
1753 sbufWriteU8(dst, blackboxGetRateDenom());
1754 sbufWriteU16(dst, blackboxGetPRatio());
1755 sbufWriteU8(dst, blackboxConfig()->sample_rate);
1756 // Added in MSP API 1.45
1757 sbufWriteU32(dst, blackboxConfig()->fields_disabled_mask);
1758 #else
1759 sbufWriteU8(dst, 0); // Blackbox not supported
1760 sbufWriteU8(dst, 0);
1761 sbufWriteU8(dst, 0);
1762 sbufWriteU8(dst, 0);
1763 sbufWriteU16(dst, 0);
1764 sbufWriteU8(dst, 0);
1765 // Added in MSP API 1.45
1766 sbufWriteU32(dst, 0);
1767 #endif
1768 break;
1769
1770 case MSP_SDCARD_SUMMARY:
1771 serializeSDCardSummaryReply(dst);
1772 break;
1773
1774 case MSP_MOTOR_3D_CONFIG:
1775 sbufWriteU16(dst, flight3DConfig()->deadband3d_low);
1776 sbufWriteU16(dst, flight3DConfig()->deadband3d_high);
1777 sbufWriteU16(dst, flight3DConfig()->neutral3d);
1778 break;
1779
1780 case MSP_RC_DEADBAND:
1781 sbufWriteU8(dst, rcControlsConfig()->deadband);
1782 sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
1783 sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband);
1784 sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle);
1785 break;
1786
1787
1788 case MSP_SENSOR_ALIGNMENT: {
1789 uint8_t gyroAlignment;
1790 #ifdef USE_MULTI_GYRO
1791 switch (gyroConfig()->gyro_to_use) {
1792 case GYRO_CONFIG_USE_GYRO_2:
1793 gyroAlignment = gyroDeviceConfig(1)->alignment;
1794 break;
1795 case GYRO_CONFIG_USE_GYRO_BOTH:
1796 // for dual-gyro in "BOTH" mode we only read/write gyro 0
1797 default:
1798 gyroAlignment = gyroDeviceConfig(0)->alignment;
1799 break;
1800 }
1801 #else
1802 gyroAlignment = gyroDeviceConfig(0)->alignment;
1803 #endif
1804 sbufWriteU8(dst, gyroAlignment);
1805 sbufWriteU8(dst, gyroAlignment); // Starting with 4.0 gyro and acc alignment are the same
1806 #if defined(USE_MAG)
1807 sbufWriteU8(dst, compassConfig()->mag_alignment);
1808 #else
1809 sbufWriteU8(dst, 0);
1810 #endif
1811
1812 // API 1.41 - Add multi-gyro indicator, selected gyro, and support for separate gyro 1 & 2 alignment
1813 sbufWriteU8(dst, getGyroDetectionFlags());
1814 #ifdef USE_MULTI_GYRO
1815 sbufWriteU8(dst, gyroConfig()->gyro_to_use);
1816 sbufWriteU8(dst, gyroDeviceConfig(0)->alignment);
1817 sbufWriteU8(dst, gyroDeviceConfig(1)->alignment);
1818 #else
1819 sbufWriteU8(dst, GYRO_CONFIG_USE_GYRO_1);
1820 sbufWriteU8(dst, gyroDeviceConfig(0)->alignment);
1821 sbufWriteU8(dst, ALIGN_DEFAULT);
1822 #endif
1823
1824 break;
1825 }
1826 case MSP_ADVANCED_CONFIG:
1827 sbufWriteU8(dst, 1); // was gyroConfig()->gyro_sync_denom - removed in API 1.43
1828 sbufWriteU8(dst, pidConfig()->pid_process_denom);
1829 sbufWriteU8(dst, motorConfig()->dev.useUnsyncedPwm);
1830 sbufWriteU8(dst, motorConfig()->dev.motorPwmProtocol);
1831 sbufWriteU16(dst, motorConfig()->dev.motorPwmRate);
1832 sbufWriteU16(dst, motorConfig()->digitalIdleOffsetValue);
1833 sbufWriteU8(dst, 0); // DEPRECATED: gyro_use_32kHz
1834 sbufWriteU8(dst, motorConfig()->dev.motorPwmInversion);
1835 sbufWriteU8(dst, gyroConfig()->gyro_to_use);
1836 sbufWriteU8(dst, gyroConfig()->gyro_high_fsr);
1837 sbufWriteU8(dst, gyroConfig()->gyroMovementCalibrationThreshold);
1838 sbufWriteU16(dst, gyroConfig()->gyroCalibrationDuration);
1839 sbufWriteU16(dst, gyroConfig()->gyro_offset_yaw);
1840 sbufWriteU8(dst, gyroConfig()->checkOverflow);
1841 //Added in MSP API 1.42
1842 sbufWriteU8(dst, systemConfig()->debug_mode);
1843 sbufWriteU8(dst, DEBUG_COUNT);
1844
1845 break;
1846 case MSP_FILTER_CONFIG :
1847 sbufWriteU8(dst, gyroConfig()->gyro_lpf1_static_hz);
1848 sbufWriteU16(dst, currentPidProfile->dterm_lpf1_static_hz);
1849 sbufWriteU16(dst, currentPidProfile->yaw_lowpass_hz);
1850 sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1);
1851 sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1);
1852 sbufWriteU16(dst, currentPidProfile->dterm_notch_hz);
1853 sbufWriteU16(dst, currentPidProfile->dterm_notch_cutoff);
1854 sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2);
1855 sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2);
1856 sbufWriteU8(dst, currentPidProfile->dterm_lpf1_type);
1857 sbufWriteU8(dst, gyroConfig()->gyro_hardware_lpf);
1858 sbufWriteU8(dst, 0); // DEPRECATED: gyro_32khz_hardware_lpf
1859 sbufWriteU16(dst, gyroConfig()->gyro_lpf1_static_hz);
1860 sbufWriteU16(dst, gyroConfig()->gyro_lpf2_static_hz);
1861 sbufWriteU8(dst, gyroConfig()->gyro_lpf1_type);
1862 sbufWriteU8(dst, gyroConfig()->gyro_lpf2_type);
1863 sbufWriteU16(dst, currentPidProfile->dterm_lpf2_static_hz);
1864 // Added in MSP API 1.41
1865 sbufWriteU8(dst, currentPidProfile->dterm_lpf2_type);
1866 #if defined(USE_DYN_LPF)
1867 sbufWriteU16(dst, gyroConfig()->gyro_lpf1_dyn_min_hz);
1868 sbufWriteU16(dst, gyroConfig()->gyro_lpf1_dyn_max_hz);
1869 sbufWriteU16(dst, currentPidProfile->dterm_lpf1_dyn_min_hz);
1870 sbufWriteU16(dst, currentPidProfile->dterm_lpf1_dyn_max_hz);
1871 #else
1872 sbufWriteU16(dst, 0);
1873 sbufWriteU16(dst, 0);
1874 sbufWriteU16(dst, 0);
1875 sbufWriteU16(dst, 0);
1876 #endif
1877 // Added in MSP API 1.42
1878 #if defined(USE_DYN_NOTCH_FILTER)
1879 sbufWriteU8(dst, 0); // DEPRECATED 1.43: dyn_notch_range
1880 sbufWriteU8(dst, 0); // DEPRECATED 1.44: dyn_notch_width_percent
1881 sbufWriteU16(dst, dynNotchConfig()->dyn_notch_q);
1882 sbufWriteU16(dst, dynNotchConfig()->dyn_notch_min_hz);
1883 #else
1884 sbufWriteU8(dst, 0);
1885 sbufWriteU8(dst, 0);
1886 sbufWriteU16(dst, 0);
1887 sbufWriteU16(dst, 0);
1888 #endif
1889 #if defined(USE_RPM_FILTER)
1890 sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_harmonics);
1891 sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_min_hz);
1892 #else
1893 sbufWriteU8(dst, 0);
1894 sbufWriteU8(dst, 0);
1895 #endif
1896 #if defined(USE_DYN_NOTCH_FILTER)
1897 // Added in MSP API 1.43
1898 sbufWriteU16(dst, dynNotchConfig()->dyn_notch_max_hz);
1899 #else
1900 sbufWriteU16(dst, 0);
1901 #endif
1902 #if defined(USE_DYN_LPF)
1903 // Added in MSP API 1.44
1904 sbufWriteU8(dst, currentPidProfile->dterm_lpf1_dyn_expo);
1905 #else
1906 sbufWriteU8(dst, 0);
1907 #endif
1908 #if defined(USE_DYN_NOTCH_FILTER)
1909 sbufWriteU8(dst, dynNotchConfig()->dyn_notch_count);
1910 #else
1911 sbufWriteU8(dst, 0);
1912 #endif
1913
1914 break;
1915 case MSP_PID_ADVANCED:
1916 sbufWriteU16(dst, 0);
1917 sbufWriteU16(dst, 0);
1918 sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit
1919 sbufWriteU8(dst, 0); // reserved
1920 sbufWriteU8(dst, 0); // was vbatPidCompensation
1921 #if defined(USE_FEEDFORWARD)
1922 sbufWriteU8(dst, currentPidProfile->feedforward_transition);
1923 #else
1924 sbufWriteU8(dst, 0);
1925 #endif
1926 sbufWriteU8(dst, 0); // was low byte of currentPidProfile->dtermSetpointWeight
1927 sbufWriteU8(dst, 0); // reserved
1928 sbufWriteU8(dst, 0); // reserved
1929 sbufWriteU8(dst, 0); // reserved
1930 sbufWriteU16(dst, currentPidProfile->rateAccelLimit);
1931 sbufWriteU16(dst, currentPidProfile->yawRateAccelLimit);
1932 sbufWriteU8(dst, currentPidProfile->levelAngleLimit);
1933 sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity
1934 sbufWriteU16(dst, 0); // was currentPidProfile->itermThrottleThreshold
1935 sbufWriteU16(dst, currentPidProfile->anti_gravity_gain);
1936 sbufWriteU16(dst, 0); // was currentPidProfile->dtermSetpointWeight
1937 sbufWriteU8(dst, currentPidProfile->iterm_rotation);
1938 sbufWriteU8(dst, 0); // was currentPidProfile->smart_feedforward
1939 #if defined(USE_ITERM_RELAX)
1940 sbufWriteU8(dst, currentPidProfile->iterm_relax);
1941 sbufWriteU8(dst, currentPidProfile->iterm_relax_type);
1942 #else
1943 sbufWriteU8(dst, 0);
1944 sbufWriteU8(dst, 0);
1945 #endif
1946 #if defined(USE_ABSOLUTE_CONTROL)
1947 sbufWriteU8(dst, currentPidProfile->abs_control_gain);
1948 #else
1949 sbufWriteU8(dst, 0);
1950 #endif
1951 #if defined(USE_THROTTLE_BOOST)
1952 sbufWriteU8(dst, currentPidProfile->throttle_boost);
1953 #else
1954 sbufWriteU8(dst, 0);
1955 #endif
1956 #if defined(USE_ACRO_TRAINER)
1957 sbufWriteU8(dst, currentPidProfile->acro_trainer_angle_limit);
1958 #else
1959 sbufWriteU8(dst, 0);
1960 #endif
1961 sbufWriteU16(dst, currentPidProfile->pid[PID_ROLL].F);
1962 sbufWriteU16(dst, currentPidProfile->pid[PID_PITCH].F);
1963 sbufWriteU16(dst, currentPidProfile->pid[PID_YAW].F);
1964 sbufWriteU8(dst, 0); // was currentPidProfile->antiGravityMode
1965 #if defined(USE_D_MIN)
1966 sbufWriteU8(dst, currentPidProfile->d_min[PID_ROLL]);
1967 sbufWriteU8(dst, currentPidProfile->d_min[PID_PITCH]);
1968 sbufWriteU8(dst, currentPidProfile->d_min[PID_YAW]);
1969 sbufWriteU8(dst, currentPidProfile->d_min_gain);
1970 sbufWriteU8(dst, currentPidProfile->d_min_advance);
1971 #else
1972 sbufWriteU8(dst, 0);
1973 sbufWriteU8(dst, 0);
1974 sbufWriteU8(dst, 0);
1975 sbufWriteU8(dst, 0);
1976 sbufWriteU8(dst, 0);
1977 #endif
1978 #if defined(USE_INTEGRATED_YAW_CONTROL)
1979 sbufWriteU8(dst, currentPidProfile->use_integrated_yaw);
1980 sbufWriteU8(dst, currentPidProfile->integrated_yaw_relax);
1981 #else
1982 sbufWriteU8(dst, 0);
1983 sbufWriteU8(dst, 0);
1984 #endif
1985 #if defined(USE_ITERM_RELAX)
1986 // Added in MSP API 1.42
1987 sbufWriteU8(dst, currentPidProfile->iterm_relax_cutoff);
1988 #else
1989 sbufWriteU8(dst, 0);
1990 #endif
1991 // Added in MSP API 1.43
1992 sbufWriteU8(dst, currentPidProfile->motor_output_limit);
1993 sbufWriteU8(dst, currentPidProfile->auto_profile_cell_count);
1994 #if defined(USE_DYN_IDLE)
1995 sbufWriteU8(dst, currentPidProfile->dyn_idle_min_rpm);
1996 #else
1997 sbufWriteU8(dst, 0);
1998 #endif
1999 // Added in MSP API 1.44
2000 #if defined(USE_FEEDFORWARD)
2001 sbufWriteU8(dst, currentPidProfile->feedforward_averaging);
2002 sbufWriteU8(dst, currentPidProfile->feedforward_smooth_factor);
2003 sbufWriteU8(dst, currentPidProfile->feedforward_boost);
2004 sbufWriteU8(dst, currentPidProfile->feedforward_max_rate_limit);
2005 sbufWriteU8(dst, currentPidProfile->feedforward_jitter_factor);
2006 #else
2007 sbufWriteU8(dst, 0);
2008 sbufWriteU8(dst, 0);
2009 sbufWriteU8(dst, 0);
2010 sbufWriteU8(dst, 0);
2011 sbufWriteU8(dst, 0);
2012 #endif
2013 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
2014 sbufWriteU8(dst, currentPidProfile->vbat_sag_compensation);
2015 #else
2016 sbufWriteU8(dst, 0);
2017 #endif
2018 #if defined(USE_THRUST_LINEARIZATION)
2019 sbufWriteU8(dst, currentPidProfile->thrustLinearization);
2020 #else
2021 sbufWriteU8(dst, 0);
2022 #endif
2023 sbufWriteU8(dst, currentPidProfile->tpa_rate);
2024 sbufWriteU16(dst, currentPidProfile->tpa_breakpoint); // was currentControlRateProfile->tpa_breakpoint
2025 break;
2026 case MSP_SENSOR_CONFIG:
2027 #if defined(USE_ACC)
2028 sbufWriteU8(dst, accelerometerConfig()->acc_hardware);
2029 #else
2030 sbufWriteU8(dst, 0);
2031 #endif
2032 #ifdef USE_BARO
2033 sbufWriteU8(dst, barometerConfig()->baro_hardware);
2034 #else
2035 sbufWriteU8(dst, BARO_NONE);
2036 #endif
2037 #ifdef USE_MAG
2038 sbufWriteU8(dst, compassConfig()->mag_hardware);
2039 #else
2040 sbufWriteU8(dst, MAG_NONE);
2041 #endif
2042 break;
2043
2044 #if defined(USE_VTX_COMMON)
2045 case MSP_VTX_CONFIG:
2046 {
2047 const vtxDevice_t *vtxDevice = vtxCommonDevice();
2048 unsigned vtxStatus = 0;
2049 vtxDevType_e vtxType = VTXDEV_UNKNOWN;
2050 uint8_t deviceIsReady = 0;
2051 if (vtxDevice) {
2052 vtxCommonGetStatus(vtxDevice, &vtxStatus);
2053 vtxType = vtxCommonGetDeviceType(vtxDevice);
2054 deviceIsReady = vtxCommonDeviceIsReady(vtxDevice) ? 1 : 0;
2055 }
2056 sbufWriteU8(dst, vtxType);
2057 sbufWriteU8(dst, vtxSettingsConfig()->band);
2058 sbufWriteU8(dst, vtxSettingsConfig()->channel);
2059 sbufWriteU8(dst, vtxSettingsConfig()->power);
2060 sbufWriteU8(dst, (vtxStatus & VTX_STATUS_PIT_MODE) ? 1 : 0);
2061 sbufWriteU16(dst, vtxSettingsConfig()->freq);
2062 sbufWriteU8(dst, deviceIsReady);
2063 sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm);
2064
2065 // API version 1.42
2066 sbufWriteU16(dst, vtxSettingsConfig()->pitModeFreq);
2067 #ifdef USE_VTX_TABLE
2068 sbufWriteU8(dst, 1); // vtxtable is available
2069 sbufWriteU8(dst, vtxTableConfig()->bands);
2070 sbufWriteU8(dst, vtxTableConfig()->channels);
2071 sbufWriteU8(dst, vtxTableConfig()->powerLevels);
2072 #else
2073 sbufWriteU8(dst, 0);
2074 sbufWriteU8(dst, 0);
2075 sbufWriteU8(dst, 0);
2076 sbufWriteU8(dst, 0);
2077 #endif
2078 #ifdef USE_VTX_MSP
2079 setMspVtxDeviceStatusReady(srcDesc);
2080 #endif
2081 }
2082 break;
2083 #endif
2084
2085 case MSP_TX_INFO:
2086 sbufWriteU8(dst, rssiSource);
2087 uint8_t rtcDateTimeIsSet = 0;
2088 #ifdef USE_RTC_TIME
2089 dateTime_t dt;
2090 if (rtcGetDateTime(&dt)) {
2091 rtcDateTimeIsSet = 1;
2092 }
2093 #else
2094 rtcDateTimeIsSet = RTC_NOT_SUPPORTED;
2095 #endif
2096 sbufWriteU8(dst, rtcDateTimeIsSet);
2097
2098 break;
2099 #ifdef USE_RTC_TIME
2100 case MSP_RTC:
2101 {
2102 dateTime_t dt;
2103 if (rtcGetDateTime(&dt)) {
2104 sbufWriteU16(dst, dt.year);
2105 sbufWriteU8(dst, dt.month);
2106 sbufWriteU8(dst, dt.day);
2107 sbufWriteU8(dst, dt.hours);
2108 sbufWriteU8(dst, dt.minutes);
2109 sbufWriteU8(dst, dt.seconds);
2110 sbufWriteU16(dst, dt.millis);
2111 }
2112 }
2113
2114 break;
2115 #endif
2116 default:
2117 unsupportedCommand = true;
2118 }
2119 return !unsupportedCommand;
2120 }
2121
2122
2123 #ifdef USE_SIMPLIFIED_TUNING
2124 // Reads simplified PID tuning values from MSP buffer
2125 static void readSimplifiedPids(pidProfile_t* pidProfile, sbuf_t *src)
2126 {
2127 pidProfile->simplified_pids_mode = sbufReadU8(src);
2128 pidProfile->simplified_master_multiplier = sbufReadU8(src);
2129 pidProfile->simplified_roll_pitch_ratio = sbufReadU8(src);
2130 pidProfile->simplified_i_gain = sbufReadU8(src);
2131 pidProfile->simplified_d_gain = sbufReadU8(src);
2132 pidProfile->simplified_pi_gain = sbufReadU8(src);
2133 #ifdef USE_D_MIN
2134 pidProfile->simplified_dmin_ratio = sbufReadU8(src);
2135 #else
2136 sbufReadU8(src);
2137 #endif
2138 pidProfile->simplified_feedforward_gain = sbufReadU8(src);
2139 pidProfile->simplified_pitch_pi_gain = sbufReadU8(src);
2140 sbufReadU32(src); // reserved for future use
2141 sbufReadU32(src); // reserved for future use
2142 }
2143
2144 // Writes simplified PID tuning values to MSP buffer
2145 static void writeSimplifiedPids(const pidProfile_t *pidProfile, sbuf_t *dst)
2146 {
2147 sbufWriteU8(dst, pidProfile->simplified_pids_mode);
2148 sbufWriteU8(dst, pidProfile->simplified_master_multiplier);
2149 sbufWriteU8(dst, pidProfile->simplified_roll_pitch_ratio);
2150 sbufWriteU8(dst, pidProfile->simplified_i_gain);
2151 sbufWriteU8(dst, pidProfile->simplified_d_gain);
2152 sbufWriteU8(dst, pidProfile->simplified_pi_gain);
2153 #ifdef USE_D_MIN
2154 sbufWriteU8(dst, pidProfile->simplified_dmin_ratio);
2155 #else
2156 sbufWriteU8(dst, 0);
2157 #endif
2158 sbufWriteU8(dst, pidProfile->simplified_feedforward_gain);
2159 sbufWriteU8(dst, pidProfile->simplified_pitch_pi_gain);
2160 sbufWriteU32(dst, 0); // reserved for future use
2161 sbufWriteU32(dst, 0); // reserved for future use
2162 }
2163
2164 // Reads simplified Dterm Filter values from MSP buffer
2165 static void readSimplifiedDtermFilters(pidProfile_t* pidProfile, sbuf_t *src)
2166 {
2167 pidProfile->simplified_dterm_filter = sbufReadU8(src);
2168 pidProfile->simplified_dterm_filter_multiplier = sbufReadU8(src);
2169 pidProfile->dterm_lpf1_static_hz = sbufReadU16(src);
2170 pidProfile->dterm_lpf2_static_hz = sbufReadU16(src);
2171 #if defined(USE_DYN_LPF)
2172 pidProfile->dterm_lpf1_dyn_min_hz = sbufReadU16(src);
2173 pidProfile->dterm_lpf1_dyn_max_hz = sbufReadU16(src);
2174 #else
2175 sbufReadU16(src);
2176 sbufReadU16(src);
2177 #endif
2178 sbufReadU32(src); // reserved for future use
2179 sbufReadU32(src); // reserved for future use
2180 }
2181
2182 // Writes simplified Dterm Filter values into MSP buffer
2183 static void writeSimplifiedDtermFilters(const pidProfile_t* pidProfile, sbuf_t *dst)
2184 {
2185 sbufWriteU8(dst, pidProfile->simplified_dterm_filter);
2186 sbufWriteU8(dst, pidProfile->simplified_dterm_filter_multiplier);
2187 sbufWriteU16(dst, pidProfile->dterm_lpf1_static_hz);
2188 sbufWriteU16(dst, pidProfile->dterm_lpf2_static_hz);
2189 #if defined(USE_DYN_LPF)
2190 sbufWriteU16(dst, pidProfile->dterm_lpf1_dyn_min_hz);
2191 sbufWriteU16(dst, pidProfile->dterm_lpf1_dyn_max_hz);
2192 #else
2193 sbufWriteU16(dst, 0);
2194 sbufWriteU16(dst, 0);
2195 #endif
2196 sbufWriteU32(dst, 0); // reserved for future use
2197 sbufWriteU32(dst, 0); // reserved for future use
2198 }
2199
2200 // Writes simplified Gyro Filter values from MSP buffer
2201 static void readSimplifiedGyroFilters(gyroConfig_t *gyroConfig, sbuf_t *src)
2202 {
2203 gyroConfig->simplified_gyro_filter = sbufReadU8(src);
2204 gyroConfig->simplified_gyro_filter_multiplier = sbufReadU8(src);
2205 gyroConfig->gyro_lpf1_static_hz = sbufReadU16(src);
2206 gyroConfig->gyro_lpf2_static_hz = sbufReadU16(src);
2207 #if defined(USE_DYN_LPF)
2208 gyroConfig->gyro_lpf1_dyn_min_hz = sbufReadU16(src);
2209 gyroConfig->gyro_lpf1_dyn_max_hz = sbufReadU16(src);
2210 #else
2211 sbufReadU16(src);
2212 sbufReadU16(src);
2213 #endif
2214 sbufReadU32(src); // reserved for future use
2215 sbufReadU32(src); // reserved for future use
2216 }
2217
2218 // Writes simplified Gyro Filter values into MSP buffer
2219 static void writeSimplifiedGyroFilters(const gyroConfig_t *gyroConfig, sbuf_t *dst)
2220 {
2221 sbufWriteU8(dst, gyroConfig->simplified_gyro_filter);
2222 sbufWriteU8(dst, gyroConfig->simplified_gyro_filter_multiplier);
2223 sbufWriteU16(dst, gyroConfig->gyro_lpf1_static_hz);
2224 sbufWriteU16(dst, gyroConfig->gyro_lpf2_static_hz);
2225 #if defined(USE_DYN_LPF)
2226 sbufWriteU16(dst, gyroConfig->gyro_lpf1_dyn_min_hz);
2227 sbufWriteU16(dst, gyroConfig->gyro_lpf1_dyn_max_hz);
2228 #else
2229 sbufWriteU16(dst, 0);
2230 sbufWriteU16(dst, 0);
2231 #endif
2232 sbufWriteU32(dst, 0); // reserved for future use
2233 sbufWriteU32(dst, 0); // reserved for future use
2234 }
2235
2236 // writes results of simplified PID tuning values to MSP buffer
2237 static void writePidfs(pidProfile_t* pidProfile, sbuf_t *dst)
2238 {
2239 for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
2240 sbufWriteU8(dst, pidProfile->pid[i].P);
2241 sbufWriteU8(dst, pidProfile->pid[i].I);
2242 sbufWriteU8(dst, pidProfile->pid[i].D);
2243 sbufWriteU8(dst, pidProfile->d_min[i]);
2244 sbufWriteU16(dst, pidProfile->pid[i].F);
2245 }
2246 }
2247 #endif // USE_SIMPLIFIED_TUNING
2248
2249 static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
2250 {
2251
2252 switch (cmdMSP) {
2253 case MSP_BOXNAMES:
2254 {
2255 const int page = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
2256 serializeBoxReply(dst, page, &serializeBoxNameFn);
2257 }
2258 break;
2259 case MSP_BOXIDS:
2260 {
2261 const int page = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
2262 serializeBoxReply(dst, page, &serializeBoxPermanentIdFn);
2263 }
2264 break;
2265 case MSP_REBOOT:
2266 if (sbufBytesRemaining(src)) {
2267 rebootMode = sbufReadU8(src);
2268
2269 if (rebootMode >= MSP_REBOOT_COUNT
2270 #if !defined(USE_USB_MSC)
2271 || rebootMode == MSP_REBOOT_MSC || rebootMode == MSP_REBOOT_MSC_UTC
2272 #endif
2273 ) {
2274 return MSP_RESULT_ERROR;
2275 }
2276 } else {
2277 rebootMode = MSP_REBOOT_FIRMWARE;
2278 }
2279
2280 sbufWriteU8(dst, rebootMode);
2281
2282 #if defined(USE_USB_MSC)
2283 if (rebootMode == MSP_REBOOT_MSC) {
2284 if (mscCheckFilesystemReady()) {
2285 sbufWriteU8(dst, 1);
2286 } else {
2287 sbufWriteU8(dst, 0);
2288
2289 return MSP_RESULT_ACK;
2290 }
2291 }
2292 #endif
2293
2294 #if defined(USE_MSP_OVER_TELEMETRY)
2295 if (featureIsEnabled(FEATURE_RX_SPI) && srcDesc == getMspTelemetryDescriptor()) {
2296 dispatchAdd(&mspRebootEntry, MSP_DISPATCH_DELAY_US);
2297 } else
2298 #endif
2299 if (mspPostProcessFn) {
2300 *mspPostProcessFn = mspRebootFn;
2301 }
2302
2303 break;
2304 case MSP_MULTIPLE_MSP:
2305 {
2306 uint8_t maxMSPs = 0;
2307 if (sbufBytesRemaining(src) == 0) {
2308 return MSP_RESULT_ERROR;
2309 }
2310 int bytesRemaining = sbufBytesRemaining(dst) - 1; // need to keep one byte for checksum
2311 mspPacket_t packetIn, packetOut;
2312 sbufInit(&packetIn.buf, src->end, src->end);
2313 uint8_t* resetInputPtr = src->ptr;
2314 while (sbufBytesRemaining(src) && bytesRemaining > 0) {
2315 uint8_t newMSP = sbufReadU8(src);
2316 sbufInit(&packetOut.buf, dst->ptr, dst->end);
2317 packetIn.cmd = newMSP;
2318 mspFcProcessCommand(srcDesc, &packetIn, &packetOut, NULL);
2319 uint8_t mspSize = sbufPtr(&packetOut.buf) - dst->ptr;
2320 mspSize++; // need to add length information for each MSP
2321 bytesRemaining -= mspSize;
2322 if (bytesRemaining >= 0) {
2323 maxMSPs++;
2324 }
2325 }
2326 src->ptr = resetInputPtr;
2327 sbufInit(&packetOut.buf, dst->ptr, dst->end);
2328 for (int i = 0; i < maxMSPs; i++) {
2329 uint8_t* sizePtr = sbufPtr(&packetOut.buf);
2330 sbufWriteU8(&packetOut.buf, 0); // dummy
2331 packetIn.cmd = sbufReadU8(src);
2332 mspFcProcessCommand(srcDesc, &packetIn, &packetOut, NULL);
2333 (*sizePtr) = sbufPtr(&packetOut.buf) - (sizePtr + 1);
2334 }
2335 dst->ptr = packetOut.buf.ptr;
2336 }
2337 break;
2338
2339 #ifdef USE_VTX_TABLE
2340 case MSP_VTXTABLE_BAND:
2341 {
2342 const uint8_t band = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
2343 if (band > 0 && band <= VTX_TABLE_MAX_BANDS) {
2344 sbufWriteU8(dst, band); // band number (same as request)
2345 sbufWriteU8(dst, VTX_TABLE_BAND_NAME_LENGTH); // band name length
2346 for (int i = 0; i < VTX_TABLE_BAND_NAME_LENGTH; i++) { // band name bytes
2347 sbufWriteU8(dst, vtxTableConfig()->bandNames[band - 1][i]);
2348 }
2349 sbufWriteU8(dst, vtxTableConfig()->bandLetters[band - 1]); // band letter
2350 sbufWriteU8(dst, vtxTableConfig()->isFactoryBand[band - 1]); // CUSTOM = 0; FACTORY = 1
2351 sbufWriteU8(dst, vtxTableConfig()->channels); // number of channel frequencies to follow
2352 for (int i = 0; i < vtxTableConfig()->channels; i++) { // the frequency for each channel
2353 sbufWriteU16(dst, vtxTableConfig()->frequency[band - 1][i]);
2354 }
2355 } else {
2356 return MSP_RESULT_ERROR;
2357 }
2358 #ifdef USE_VTX_MSP
2359 setMspVtxDeviceStatusReady(srcDesc);
2360 #endif
2361 }
2362 break;
2363
2364 case MSP_VTXTABLE_POWERLEVEL:
2365 {
2366 const uint8_t powerLevel = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
2367 if (powerLevel > 0 && powerLevel <= VTX_TABLE_MAX_POWER_LEVELS) {
2368 sbufWriteU8(dst, powerLevel); // powerLevel number (same as request)
2369 sbufWriteU16(dst, vtxTableConfig()->powerValues[powerLevel - 1]);
2370 sbufWriteU8(dst, VTX_TABLE_POWER_LABEL_LENGTH); // powerLevel label length
2371 for (int i = 0; i < VTX_TABLE_POWER_LABEL_LENGTH; i++) { // powerlevel label bytes
2372 sbufWriteU8(dst, vtxTableConfig()->powerLabels[powerLevel - 1][i]);
2373 }
2374 } else {
2375 return MSP_RESULT_ERROR;
2376 }
2377 #ifdef USE_VTX_MSP
2378 setMspVtxDeviceStatusReady(srcDesc);
2379 #endif
2380 }
2381 break;
2382 #endif // USE_VTX_TABLE
2383
2384 #ifdef USE_SIMPLIFIED_TUNING
2385 // Added in MSP API 1.44
2386 case MSP_SIMPLIFIED_TUNING:
2387 {
2388 writeSimplifiedPids(currentPidProfile, dst);
2389 writeSimplifiedDtermFilters(currentPidProfile, dst);
2390 writeSimplifiedGyroFilters(gyroConfig(), dst);
2391 }
2392 break;
2393
2394 case MSP_CALCULATE_SIMPLIFIED_PID:
2395 {
2396 pidProfile_t tempPidProfile = *currentPidProfile;
2397 readSimplifiedPids(&tempPidProfile, src);
2398 applySimplifiedTuningPids(&tempPidProfile);
2399 writePidfs(&tempPidProfile, dst);
2400 }
2401 break;
2402
2403 case MSP_CALCULATE_SIMPLIFIED_DTERM:
2404 {
2405 pidProfile_t tempPidProfile = *currentPidProfile;
2406 readSimplifiedDtermFilters(&tempPidProfile, src);
2407 applySimplifiedTuningDtermFilters(&tempPidProfile);
2408 writeSimplifiedDtermFilters(&tempPidProfile, dst);
2409 }
2410 break;
2411
2412 case MSP_CALCULATE_SIMPLIFIED_GYRO:
2413 {
2414 gyroConfig_t tempGyroConfig = *gyroConfig();
2415 readSimplifiedGyroFilters(&tempGyroConfig, src);
2416 applySimplifiedTuningGyroFilters(&tempGyroConfig);
2417 writeSimplifiedGyroFilters(&tempGyroConfig, dst);
2418 }
2419 break;
2420
2421 case MSP_VALIDATE_SIMPLIFIED_TUNING:
2422 {
2423 pidProfile_t tempPidProfile = *currentPidProfile;
2424 applySimplifiedTuningPids(&tempPidProfile);
2425 bool result = true;
2426
2427 for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
2428 result = result &&
2429 tempPidProfile.pid[i].P == currentPidProfile->pid[i].P &&
2430 tempPidProfile.pid[i].I == currentPidProfile->pid[i].I &&
2431 tempPidProfile.pid[i].D == currentPidProfile->pid[i].D &&
2432 tempPidProfile.d_min[i] == currentPidProfile->d_min[i] &&
2433 tempPidProfile.pid[i].F == currentPidProfile->pid[i].F;
2434 }
2435
2436 sbufWriteU8(dst, result);
2437
2438 gyroConfig_t tempGyroConfig = *gyroConfig();
2439 applySimplifiedTuningGyroFilters(&tempGyroConfig);
2440 result =
2441 tempGyroConfig.gyro_lpf1_static_hz == gyroConfig()->gyro_lpf1_static_hz &&
2442 tempGyroConfig.gyro_lpf2_static_hz == gyroConfig()->gyro_lpf2_static_hz;
2443
2444 #if defined(USE_DYN_LPF)
2445 result = result &&
2446 tempGyroConfig.gyro_lpf1_dyn_min_hz == gyroConfig()->gyro_lpf1_dyn_min_hz &&
2447 tempGyroConfig.gyro_lpf1_dyn_max_hz == gyroConfig()->gyro_lpf1_dyn_max_hz;
2448 #endif
2449
2450 sbufWriteU8(dst, result);
2451
2452 applySimplifiedTuningDtermFilters(&tempPidProfile);
2453 result =
2454 tempPidProfile.dterm_lpf1_static_hz == currentPidProfile->dterm_lpf1_static_hz &&
2455 tempPidProfile.dterm_lpf2_static_hz == currentPidProfile->dterm_lpf2_static_hz;
2456
2457 #if defined(USE_DYN_LPF)
2458 result = result &&
2459 tempPidProfile.dterm_lpf1_dyn_min_hz == currentPidProfile->dterm_lpf1_dyn_min_hz &&
2460 tempPidProfile.dterm_lpf1_dyn_max_hz == currentPidProfile->dterm_lpf1_dyn_max_hz;
2461 #endif
2462
2463 sbufWriteU8(dst, result);
2464 }
2465 break;
2466 #endif
2467
2468 case MSP_RESET_CONF:
2469 {
2470 #if defined(USE_CUSTOM_DEFAULTS)
2471 defaultsType_e defaultsType = DEFAULTS_TYPE_CUSTOM;
2472 #endif
2473 if (sbufBytesRemaining(src) >= 1) {
2474 // Added in MSP API 1.42
2475 #if defined(USE_CUSTOM_DEFAULTS)
2476 defaultsType = sbufReadU8(src);
2477 #else
2478 sbufReadU8(src);
2479 #endif
2480 }
2481
2482 bool success = false;
2483 if (!ARMING_FLAG(ARMED)) {
2484 #if defined(USE_CUSTOM_DEFAULTS)
2485 success = resetEEPROM(defaultsType == DEFAULTS_TYPE_CUSTOM);
2486 #else
2487 success = resetEEPROM(false);
2488 #endif
2489
2490 if (success && mspPostProcessFn) {
2491 rebootMode = MSP_REBOOT_FIRMWARE;
2492 *mspPostProcessFn = mspRebootFn;
2493 }
2494 }
2495
2496 // Added in API version 1.42
2497 sbufWriteU8(dst, success);
2498 }
2499
2500 break;
2501
2502 case MSP2_GET_TEXT:
2503 {
2504 // type byte, then length byte followed by the actual characters
2505 const uint8_t textType = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
2506
2507 char* textVar;
2508
2509 switch (textType) {
2510 case MSP2TEXT_PILOT_NAME:
2511 textVar = pilotConfigMutable()->pilotName;
2512 break;
2513
2514 case MSP2TEXT_CRAFT_NAME:
2515 textVar = pilotConfigMutable()->craftName;
2516 break;
2517
2518 default:
2519 return MSP_RESULT_ERROR;
2520 }
2521
2522 const uint8_t textLength = strlen(textVar);
2523
2524 // type byte, then length byte followed by the actual characters
2525 sbufWriteU8(dst, textType);
2526 sbufWriteU8(dst, textLength);
2527 for (unsigned int i = 0; i < textLength; i++) {
2528 sbufWriteU8(dst, textVar[i]);
2529 }
2530 }
2531 break;
2532
2533 default:
2534 return MSP_RESULT_CMD_UNKNOWN;
2535 }
2536 return MSP_RESULT_ACK;
2537 }
2538
2539 #ifdef USE_FLASHFS
2540 static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
2541 {
2542 const unsigned int dataSize = sbufBytesRemaining(src);
2543 const uint32_t readAddress = sbufReadU32(src);
2544 uint16_t readLength;
2545 bool allowCompression = false;
2546 bool useLegacyFormat;
2547 if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
2548 readLength = sbufReadU16(src);
2549 if (sbufBytesRemaining(src)) {
2550 allowCompression = sbufReadU8(src);
2551 }
2552 useLegacyFormat = false;
2553 } else {
2554 readLength = 128;
2555 useLegacyFormat = true;
2556 }
2557
2558 serializeDataflashReadReply(dst, readAddress, readLength, useLegacyFormat, allowCompression);
2559 }
2560 #endif
2561
2562 static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src)
2563 {
2564 uint32_t i;
2565 uint8_t value;
2566 const unsigned int dataSize = sbufBytesRemaining(src);
2567 switch (cmdMSP) {
2568 case MSP_SELECT_SETTING:
2569 value = sbufReadU8(src);
2570 if ((value & RATEPROFILE_MASK) == 0) {
2571 if (!ARMING_FLAG(ARMED)) {
2572 if (value >= PID_PROFILE_COUNT) {
2573 value = 0;
2574 }
2575 changePidProfile(value);
2576 }
2577 } else {
2578 value = value & ~RATEPROFILE_MASK;
2579
2580 if (value >= CONTROL_RATE_PROFILE_COUNT) {
2581 value = 0;
2582 }
2583 changeControlRateProfile(value);
2584 }
2585 break;
2586
2587 case MSP_COPY_PROFILE:
2588 value = sbufReadU8(src); // 0 = pid profile, 1 = control rate profile
2589 uint8_t dstProfileIndex = sbufReadU8(src);
2590 uint8_t srcProfileIndex = sbufReadU8(src);
2591 if (value == 0) {
2592 pidCopyProfile(dstProfileIndex, srcProfileIndex);
2593 }
2594 else if (value == 1) {
2595 copyControlRateProfile(dstProfileIndex, srcProfileIndex);
2596 }
2597 break;
2598
2599 #if defined(USE_GPS) || defined(USE_MAG)
2600 case MSP_SET_HEADING:
2601 magHold = sbufReadU16(src);
2602 break;
2603 #endif
2604
2605 case MSP_SET_RAW_RC:
2606 #ifdef USE_RX_MSP
2607 {
2608 uint8_t channelCount = dataSize / sizeof(uint16_t);
2609 if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) {
2610 return MSP_RESULT_ERROR;
2611 } else {
2612 uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
2613 for (int i = 0; i < channelCount; i++) {
2614 frame[i] = sbufReadU16(src);
2615 }
2616 rxMspFrameReceive(frame, channelCount);
2617 }
2618 }
2619 #endif
2620 break;
2621 #if defined(USE_ACC)
2622 case MSP_SET_ACC_TRIM:
2623 accelerometerConfigMutable()->accelerometerTrims.values.pitch = sbufReadU16(src);
2624 accelerometerConfigMutable()->accelerometerTrims.values.roll = sbufReadU16(src);
2625
2626 break;
2627 #endif
2628 case MSP_SET_ARMING_CONFIG:
2629 armingConfigMutable()->auto_disarm_delay = sbufReadU8(src);
2630 sbufReadU8(src); // reserved
2631 if (sbufBytesRemaining(src)) {
2632 imuConfigMutable()->small_angle = sbufReadU8(src);
2633 }
2634 break;
2635
2636 case MSP_SET_PID_CONTROLLER:
2637 break;
2638
2639 case MSP_SET_PID:
2640 for (int i = 0; i < PID_ITEM_COUNT; i++) {
2641 currentPidProfile->pid[i].P = sbufReadU8(src);
2642 currentPidProfile->pid[i].I = sbufReadU8(src);
2643 currentPidProfile->pid[i].D = sbufReadU8(src);
2644 }
2645 pidInitConfig(currentPidProfile);
2646 break;
2647
2648 case MSP_SET_MODE_RANGE:
2649 i = sbufReadU8(src);
2650 if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
2651 modeActivationCondition_t *mac = modeActivationConditionsMutable(i);
2652 i = sbufReadU8(src);
2653 const box_t *box = findBoxByPermanentId(i);
2654 if (box) {
2655 mac->modeId = box->boxId;
2656 mac->auxChannelIndex = sbufReadU8(src);
2657 mac->range.startStep = sbufReadU8(src);
2658 mac->range.endStep = sbufReadU8(src);
2659 if (sbufBytesRemaining(src) != 0) {
2660 mac->modeLogic = sbufReadU8(src);
2661
2662 i = sbufReadU8(src);
2663 mac->linkedTo = findBoxByPermanentId(i)->boxId;
2664 }
2665 rcControlsInit();
2666 } else {
2667 return MSP_RESULT_ERROR;
2668 }
2669 } else {
2670 return MSP_RESULT_ERROR;
2671 }
2672 break;
2673
2674 case MSP_SET_ADJUSTMENT_RANGE:
2675 i = sbufReadU8(src);
2676 if (i < MAX_ADJUSTMENT_RANGE_COUNT) {
2677 adjustmentRange_t *adjRange = adjustmentRangesMutable(i);
2678 sbufReadU8(src); // was adjRange->adjustmentIndex
2679 adjRange->auxChannelIndex = sbufReadU8(src);
2680 adjRange->range.startStep = sbufReadU8(src);
2681 adjRange->range.endStep = sbufReadU8(src);
2682 adjRange->adjustmentConfig = sbufReadU8(src);
2683 adjRange->auxSwitchChannelIndex = sbufReadU8(src);
2684
2685 activeAdjustmentRangeReset();
2686 } else {
2687 return MSP_RESULT_ERROR;
2688 }
2689 break;
2690
2691 case MSP_SET_RC_TUNING:
2692 if (sbufBytesRemaining(src) >= 10) {
2693 value = sbufReadU8(src);
2694 if (currentControlRateProfile->rcRates[FD_PITCH] == currentControlRateProfile->rcRates[FD_ROLL]) {
2695 currentControlRateProfile->rcRates[FD_PITCH] = value;
2696 }
2697 currentControlRateProfile->rcRates[FD_ROLL] = value;
2698
2699 value = sbufReadU8(src);
2700 if (currentControlRateProfile->rcExpo[FD_PITCH] == currentControlRateProfile->rcExpo[FD_ROLL]) {
2701 currentControlRateProfile->rcExpo[FD_PITCH] = value;
2702 }
2703 currentControlRateProfile->rcExpo[FD_ROLL] = value;
2704
2705 for (int i = 0; i < 3; i++) {
2706 currentControlRateProfile->rates[i] = sbufReadU8(src);
2707 }
2708
2709 sbufReadU8(src); // tpa_rate is moved to PID profile
2710 currentControlRateProfile->thrMid8 = sbufReadU8(src);
2711 currentControlRateProfile->thrExpo8 = sbufReadU8(src);
2712 sbufReadU16(src); // tpa_breakpoint is moved to PID profile
2713
2714 if (sbufBytesRemaining(src) >= 1) {
2715 currentControlRateProfile->rcExpo[FD_YAW] = sbufReadU8(src);
2716 }
2717
2718 if (sbufBytesRemaining(src) >= 1) {
2719 currentControlRateProfile->rcRates[FD_YAW] = sbufReadU8(src);
2720 }
2721
2722 if (sbufBytesRemaining(src) >= 1) {
2723 currentControlRateProfile->rcRates[FD_PITCH] = sbufReadU8(src);
2724 }
2725
2726 if (sbufBytesRemaining(src) >= 1) {
2727 currentControlRateProfile->rcExpo[FD_PITCH] = sbufReadU8(src);
2728 }
2729
2730 // version 1.41
2731 if (sbufBytesRemaining(src) >= 2) {
2732 currentControlRateProfile->throttle_limit_type = sbufReadU8(src);
2733 currentControlRateProfile->throttle_limit_percent = sbufReadU8(src);
2734 }
2735
2736 // version 1.42
2737 if (sbufBytesRemaining(src) >= 6) {
2738 currentControlRateProfile->rate_limit[FD_ROLL] = sbufReadU16(src);
2739 currentControlRateProfile->rate_limit[FD_PITCH] = sbufReadU16(src);
2740 currentControlRateProfile->rate_limit[FD_YAW] = sbufReadU16(src);
2741 }
2742
2743 // version 1.43
2744 if (sbufBytesRemaining(src) >= 1) {
2745 currentControlRateProfile->rates_type = sbufReadU8(src);
2746 }
2747
2748 initRcProcessing();
2749 } else {
2750 return MSP_RESULT_ERROR;
2751 }
2752 break;
2753
2754 case MSP_SET_MOTOR_CONFIG:
2755 motorConfigMutable()->minthrottle = sbufReadU16(src);
2756 motorConfigMutable()->maxthrottle = sbufReadU16(src);
2757 motorConfigMutable()->mincommand = sbufReadU16(src);
2758
2759 // version 1.42
2760 if (sbufBytesRemaining(src) >= 2) {
2761 motorConfigMutable()->motorPoleCount = sbufReadU8(src);
2762 #if defined(USE_DSHOT_TELEMETRY)
2763 motorConfigMutable()->dev.useDshotTelemetry = sbufReadU8(src);
2764 #else
2765 sbufReadU8(src);
2766 #endif
2767 }
2768 break;
2769
2770 #ifdef USE_GPS
2771 case MSP_SET_GPS_CONFIG:
2772 gpsConfigMutable()->provider = sbufReadU8(src);
2773 gpsConfigMutable()->sbasMode = sbufReadU8(src);
2774 gpsConfigMutable()->autoConfig = sbufReadU8(src);
2775 gpsConfigMutable()->autoBaud = sbufReadU8(src);
2776 if (sbufBytesRemaining(src) >= 2) {
2777 // Added in API version 1.43
2778 gpsConfigMutable()->gps_set_home_point_once = sbufReadU8(src);
2779 gpsConfigMutable()->gps_ublox_use_galileo = sbufReadU8(src);
2780 }
2781 break;
2782
2783 #ifdef USE_GPS_RESCUE
2784 case MSP_SET_GPS_RESCUE:
2785 gpsRescueConfigMutable()->angle = sbufReadU16(src);
2786 gpsRescueConfigMutable()->initialAltitudeM = sbufReadU16(src);
2787 gpsRescueConfigMutable()->descentDistanceM = sbufReadU16(src);
2788 gpsRescueConfigMutable()->rescueGroundspeed = sbufReadU16(src);
2789 gpsRescueConfigMutable()->throttleMin = sbufReadU16(src);
2790 gpsRescueConfigMutable()->throttleMax = sbufReadU16(src);
2791 gpsRescueConfigMutable()->throttleHover = sbufReadU16(src);
2792 gpsRescueConfigMutable()->sanityChecks = sbufReadU8(src);
2793 gpsRescueConfigMutable()->minSats = sbufReadU8(src);
2794 if (sbufBytesRemaining(src) >= 6) {
2795 // Added in API version 1.43
2796 gpsRescueConfigMutable()->ascendRate = sbufReadU16(src);
2797 gpsRescueConfigMutable()->descendRate = sbufReadU16(src);
2798 gpsRescueConfigMutable()->allowArmingWithoutFix = sbufReadU8(src);
2799 gpsRescueConfigMutable()->altitudeMode = sbufReadU8(src);
2800 }
2801 if (sbufBytesRemaining(src) >= 2) {
2802 // Added in API version 1.44
2803 gpsRescueConfigMutable()->minRescueDth = sbufReadU16(src);
2804 }
2805 break;
2806
2807 case MSP_SET_GPS_RESCUE_PIDS:
2808 gpsRescueConfigMutable()->throttleP = sbufReadU16(src);
2809 gpsRescueConfigMutable()->throttleI = sbufReadU16(src);
2810 gpsRescueConfigMutable()->throttleD = sbufReadU16(src);
2811 gpsRescueConfigMutable()->velP = sbufReadU16(src);
2812 gpsRescueConfigMutable()->velI = sbufReadU16(src);
2813 gpsRescueConfigMutable()->velD = sbufReadU16(src);
2814 gpsRescueConfigMutable()->yawP = sbufReadU16(src);
2815 break;
2816 #endif
2817 #endif
2818
2819 case MSP_SET_MOTOR:
2820 for (int i = 0; i < getMotorCount(); i++) {
2821 motor_disarmed[i] = motorConvertFromExternal(sbufReadU16(src));
2822 }
2823 break;
2824
2825 case MSP_SET_SERVO_CONFIGURATION:
2826 #ifdef USE_SERVOS
2827 if (dataSize != 1 + 12) {
2828 return MSP_RESULT_ERROR;
2829 }
2830 i = sbufReadU8(src);
2831 if (i >= MAX_SUPPORTED_SERVOS) {
2832 return MSP_RESULT_ERROR;
2833 } else {
2834 servoParamsMutable(i)->min = sbufReadU16(src);
2835 servoParamsMutable(i)->max = sbufReadU16(src);
2836 servoParamsMutable(i)->middle = sbufReadU16(src);
2837 servoParamsMutable(i)->rate = sbufReadU8(src);
2838 servoParamsMutable(i)->forwardFromChannel = sbufReadU8(src);
2839 servoParamsMutable(i)->reversedSources = sbufReadU32(src);
2840 }
2841 #endif
2842 break;
2843
2844 case MSP_SET_SERVO_MIX_RULE:
2845 #ifdef USE_SERVOS
2846 i = sbufReadU8(src);
2847 if (i >= MAX_SERVO_RULES) {
2848 return MSP_RESULT_ERROR;
2849 } else {
2850 customServoMixersMutable(i)->targetChannel = sbufReadU8(src);
2851 customServoMixersMutable(i)->inputSource = sbufReadU8(src);
2852 customServoMixersMutable(i)->rate = sbufReadU8(src);
2853 customServoMixersMutable(i)->speed = sbufReadU8(src);
2854 customServoMixersMutable(i)->min = sbufReadU8(src);
2855 customServoMixersMutable(i)->max = sbufReadU8(src);
2856 customServoMixersMutable(i)->box = sbufReadU8(src);
2857 loadCustomServoMixer();
2858 }
2859 #endif
2860 break;
2861
2862 case MSP_SET_MOTOR_3D_CONFIG:
2863 flight3DConfigMutable()->deadband3d_low = sbufReadU16(src);
2864 flight3DConfigMutable()->deadband3d_high = sbufReadU16(src);
2865 flight3DConfigMutable()->neutral3d = sbufReadU16(src);
2866 break;
2867
2868 case MSP_SET_RC_DEADBAND:
2869 rcControlsConfigMutable()->deadband = sbufReadU8(src);
2870 rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
2871 rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src);
2872 flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src);
2873 break;
2874
2875 case MSP_SET_RESET_CURR_PID:
2876 resetPidProfile(currentPidProfile);
2877 break;
2878
2879 case MSP_SET_SENSOR_ALIGNMENT: {
2880 // maintain backwards compatibility for API < 1.41
2881 const uint8_t gyroAlignment = sbufReadU8(src);
2882 sbufReadU8(src); // discard deprecated acc_align
2883 #if defined(USE_MAG)
2884 compassConfigMutable()->mag_alignment = sbufReadU8(src);
2885 #else
2886 sbufReadU8(src);
2887 #endif
2888
2889 if (sbufBytesRemaining(src) >= 3) {
2890 // API >= 1.41 - support the gyro_to_use and alignment for gyros 1 & 2
2891 #ifdef USE_MULTI_GYRO
2892 gyroConfigMutable()->gyro_to_use = sbufReadU8(src);
2893 gyroDeviceConfigMutable(0)->alignment = sbufReadU8(src);
2894 gyroDeviceConfigMutable(1)->alignment = sbufReadU8(src);
2895 #else
2896 sbufReadU8(src); // unused gyro_to_use
2897 gyroDeviceConfigMutable(0)->alignment = sbufReadU8(src);
2898 sbufReadU8(src); // unused gyro_2_sensor_align
2899 #endif
2900 } else {
2901 // maintain backwards compatibility for API < 1.41
2902 #ifdef USE_MULTI_GYRO
2903 switch (gyroConfig()->gyro_to_use) {
2904 case GYRO_CONFIG_USE_GYRO_2:
2905 gyroDeviceConfigMutable(1)->alignment = gyroAlignment;
2906 break;
2907 case GYRO_CONFIG_USE_GYRO_BOTH:
2908 // For dual-gyro in "BOTH" mode we'll only update gyro 0
2909 default:
2910 gyroDeviceConfigMutable(0)->alignment = gyroAlignment;
2911 break;
2912 }
2913 #else
2914 gyroDeviceConfigMutable(0)->alignment = gyroAlignment;
2915 #endif
2916
2917 }
2918 break;
2919 }
2920
2921 case MSP_SET_ADVANCED_CONFIG:
2922 sbufReadU8(src); // was gyroConfigMutable()->gyro_sync_denom - removed in API 1.43
2923 pidConfigMutable()->pid_process_denom = sbufReadU8(src);
2924 motorConfigMutable()->dev.useUnsyncedPwm = sbufReadU8(src);
2925 motorConfigMutable()->dev.motorPwmProtocol = sbufReadU8(src);
2926 motorConfigMutable()->dev.motorPwmRate = sbufReadU16(src);
2927 if (sbufBytesRemaining(src) >= 2) {
2928 motorConfigMutable()->digitalIdleOffsetValue = sbufReadU16(src);
2929 }
2930 if (sbufBytesRemaining(src)) {
2931 sbufReadU8(src); // DEPRECATED: gyro_use_32khz
2932 }
2933 if (sbufBytesRemaining(src)) {
2934 motorConfigMutable()->dev.motorPwmInversion = sbufReadU8(src);
2935 }
2936 if (sbufBytesRemaining(src) >= 8) {
2937 gyroConfigMutable()->gyro_to_use = sbufReadU8(src);
2938 gyroConfigMutable()->gyro_high_fsr = sbufReadU8(src);
2939 gyroConfigMutable()->gyroMovementCalibrationThreshold = sbufReadU8(src);
2940 gyroConfigMutable()->gyroCalibrationDuration = sbufReadU16(src);
2941 gyroConfigMutable()->gyro_offset_yaw = sbufReadU16(src);
2942 gyroConfigMutable()->checkOverflow = sbufReadU8(src);
2943 }
2944 if (sbufBytesRemaining(src) >= 1) {
2945 //Added in MSP API 1.42
2946 systemConfigMutable()->debug_mode = sbufReadU8(src);
2947 }
2948
2949 validateAndFixGyroConfig();
2950
2951 break;
2952 case MSP_SET_FILTER_CONFIG:
2953 gyroConfigMutable()->gyro_lpf1_static_hz = sbufReadU8(src);
2954 currentPidProfile->dterm_lpf1_static_hz = sbufReadU16(src);
2955 currentPidProfile->yaw_lowpass_hz = sbufReadU16(src);
2956 if (sbufBytesRemaining(src) >= 8) {
2957 gyroConfigMutable()->gyro_soft_notch_hz_1 = sbufReadU16(src);
2958 gyroConfigMutable()->gyro_soft_notch_cutoff_1 = sbufReadU16(src);
2959 currentPidProfile->dterm_notch_hz = sbufReadU16(src);
2960 currentPidProfile->dterm_notch_cutoff = sbufReadU16(src);
2961 }
2962 if (sbufBytesRemaining(src) >= 4) {
2963 gyroConfigMutable()->gyro_soft_notch_hz_2 = sbufReadU16(src);
2964 gyroConfigMutable()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
2965 }
2966 if (sbufBytesRemaining(src) >= 1) {
2967 currentPidProfile->dterm_lpf1_type = sbufReadU8(src);
2968 }
2969 if (sbufBytesRemaining(src) >= 10) {
2970 gyroConfigMutable()->gyro_hardware_lpf = sbufReadU8(src);
2971 sbufReadU8(src); // DEPRECATED: gyro_32khz_hardware_lpf
2972 gyroConfigMutable()->gyro_lpf1_static_hz = sbufReadU16(src);
2973 gyroConfigMutable()->gyro_lpf2_static_hz = sbufReadU16(src);
2974 gyroConfigMutable()->gyro_lpf1_type = sbufReadU8(src);
2975 gyroConfigMutable()->gyro_lpf2_type = sbufReadU8(src);
2976 currentPidProfile->dterm_lpf2_static_hz = sbufReadU16(src);
2977 }
2978 if (sbufBytesRemaining(src) >= 9) {
2979 // Added in MSP API 1.41
2980 currentPidProfile->dterm_lpf2_type = sbufReadU8(src);
2981 #if defined(USE_DYN_LPF)
2982 gyroConfigMutable()->gyro_lpf1_dyn_min_hz = sbufReadU16(src);
2983 gyroConfigMutable()->gyro_lpf1_dyn_max_hz = sbufReadU16(src);
2984 currentPidProfile->dterm_lpf1_dyn_min_hz = sbufReadU16(src);
2985 currentPidProfile->dterm_lpf1_dyn_max_hz = sbufReadU16(src);
2986 #else
2987 sbufReadU16(src);
2988 sbufReadU16(src);
2989 sbufReadU16(src);
2990 sbufReadU16(src);
2991 #endif
2992 }
2993 if (sbufBytesRemaining(src) >= 8) {
2994 // Added in MSP API 1.42
2995 #if defined(USE_DYN_NOTCH_FILTER)
2996 sbufReadU8(src); // DEPRECATED 1.43: dyn_notch_range
2997 sbufReadU8(src); // DEPRECATED 1.44: dyn_notch_width_percent
2998 dynNotchConfigMutable()->dyn_notch_q = sbufReadU16(src);
2999 dynNotchConfigMutable()->dyn_notch_min_hz = sbufReadU16(src);
3000 #else
3001 sbufReadU8(src);
3002 sbufReadU8(src);
3003 sbufReadU16(src);
3004 sbufReadU16(src);
3005 #endif
3006 #if defined(USE_RPM_FILTER)
3007 rpmFilterConfigMutable()->rpm_filter_harmonics = sbufReadU8(src);
3008 rpmFilterConfigMutable()->rpm_filter_min_hz = sbufReadU8(src);
3009 #else
3010 sbufReadU8(src);
3011 sbufReadU8(src);
3012 #endif
3013 }
3014 if (sbufBytesRemaining(src) >= 2) {
3015 #if defined(USE_DYN_NOTCH_FILTER)
3016 // Added in MSP API 1.43
3017 dynNotchConfigMutable()->dyn_notch_max_hz = sbufReadU16(src);
3018 #else
3019 sbufReadU16(src);
3020 #endif
3021 }
3022 if (sbufBytesRemaining(src) >= 2) {
3023 // Added in MSP API 1.44
3024 #if defined(USE_DYN_LPF)
3025 currentPidProfile->dterm_lpf1_dyn_expo = sbufReadU8(src);
3026 #else
3027 sbufReadU8(src);
3028 #endif
3029 #if defined(USE_DYN_NOTCH_FILTER)
3030 dynNotchConfigMutable()->dyn_notch_count = sbufReadU8(src);
3031 #else
3032 sbufReadU8(src);
3033 #endif
3034 }
3035
3036 // reinitialize the gyro filters with the new values
3037 validateAndFixGyroConfig();
3038 gyroInitFilters();
3039 // reinitialize the PID filters with the new values
3040 pidInitFilters(currentPidProfile);
3041
3042 break;
3043 case MSP_SET_PID_ADVANCED:
3044 sbufReadU16(src);
3045 sbufReadU16(src);
3046 sbufReadU16(src); // was pidProfile.yaw_p_limit
3047 sbufReadU8(src); // reserved
3048 sbufReadU8(src); // was vbatPidCompensation
3049 #if defined(USE_FEEDFORWARD)
3050 currentPidProfile->feedforward_transition = sbufReadU8(src);
3051 #else
3052 sbufReadU8(src);
3053 #endif
3054 sbufReadU8(src); // was low byte of currentPidProfile->dtermSetpointWeight
3055 sbufReadU8(src); // reserved
3056 sbufReadU8(src); // reserved
3057 sbufReadU8(src); // reserved
3058 currentPidProfile->rateAccelLimit = sbufReadU16(src);
3059 currentPidProfile->yawRateAccelLimit = sbufReadU16(src);
3060 if (sbufBytesRemaining(src) >= 2) {
3061 currentPidProfile->levelAngleLimit = sbufReadU8(src);
3062 sbufReadU8(src); // was pidProfile.levelSensitivity
3063 }
3064 if (sbufBytesRemaining(src) >= 4) {
3065 sbufReadU16(src); // was currentPidProfile->itermThrottleThreshold
3066 currentPidProfile->anti_gravity_gain = sbufReadU16(src);
3067 }
3068 if (sbufBytesRemaining(src) >= 2) {
3069 sbufReadU16(src); // was currentPidProfile->dtermSetpointWeight
3070 }
3071 if (sbufBytesRemaining(src) >= 14) {
3072 // Added in MSP API 1.40
3073 currentPidProfile->iterm_rotation = sbufReadU8(src);
3074 sbufReadU8(src); // was currentPidProfile->smart_feedforward
3075 #if defined(USE_ITERM_RELAX)
3076 currentPidProfile->iterm_relax = sbufReadU8(src);
3077 currentPidProfile->iterm_relax_type = sbufReadU8(src);
3078 #else
3079 sbufReadU8(src);
3080 sbufReadU8(src);
3081 #endif
3082 #if defined(USE_ABSOLUTE_CONTROL)
3083 currentPidProfile->abs_control_gain = sbufReadU8(src);
3084 #else
3085 sbufReadU8(src);
3086 #endif
3087 #if defined(USE_THROTTLE_BOOST)
3088 currentPidProfile->throttle_boost = sbufReadU8(src);
3089 #else
3090 sbufReadU8(src);
3091 #endif
3092 #if defined(USE_ACRO_TRAINER)
3093 currentPidProfile->acro_trainer_angle_limit = sbufReadU8(src);
3094 #else
3095 sbufReadU8(src);
3096 #endif
3097 // PID controller feedforward terms
3098 currentPidProfile->pid[PID_ROLL].F = sbufReadU16(src);
3099 currentPidProfile->pid[PID_PITCH].F = sbufReadU16(src);
3100 currentPidProfile->pid[PID_YAW].F = sbufReadU16(src);
3101 sbufReadU8(src); // was currentPidProfile->antiGravityMode
3102 }
3103 if (sbufBytesRemaining(src) >= 7) {
3104 // Added in MSP API 1.41
3105 #if defined(USE_D_MIN)
3106 currentPidProfile->d_min[PID_ROLL] = sbufReadU8(src);
3107 currentPidProfile->d_min[PID_PITCH] = sbufReadU8(src);
3108 currentPidProfile->d_min[PID_YAW] = sbufReadU8(src);
3109 currentPidProfile->d_min_gain = sbufReadU8(src);
3110 currentPidProfile->d_min_advance = sbufReadU8(src);
3111 #else
3112 sbufReadU8(src);
3113 sbufReadU8(src);
3114 sbufReadU8(src);
3115 sbufReadU8(src);
3116 sbufReadU8(src);
3117 #endif
3118 #if defined(USE_INTEGRATED_YAW_CONTROL)
3119 currentPidProfile->use_integrated_yaw = sbufReadU8(src);
3120 currentPidProfile->integrated_yaw_relax = sbufReadU8(src);
3121 #else
3122 sbufReadU8(src);
3123 sbufReadU8(src);
3124 #endif
3125 }
3126 if(sbufBytesRemaining(src) >= 1) {
3127 // Added in MSP API 1.42
3128 #if defined(USE_ITERM_RELAX)
3129 currentPidProfile->iterm_relax_cutoff = sbufReadU8(src);
3130 #else
3131 sbufReadU8(src);
3132 #endif
3133 }
3134 if (sbufBytesRemaining(src) >= 3) {
3135 // Added in MSP API 1.43
3136 currentPidProfile->motor_output_limit = sbufReadU8(src);
3137 currentPidProfile->auto_profile_cell_count = sbufReadU8(src);
3138 #if defined(USE_DYN_IDLE)
3139 currentPidProfile->dyn_idle_min_rpm = sbufReadU8(src);
3140 #else
3141 sbufReadU8(src);
3142 #endif
3143 }
3144 if (sbufBytesRemaining(src) >= 7) {
3145 // Added in MSP API 1.44
3146 #if defined(USE_FEEDFORWARD)
3147 currentPidProfile->feedforward_averaging = sbufReadU8(src);
3148 currentPidProfile->feedforward_smooth_factor = sbufReadU8(src);
3149 currentPidProfile->feedforward_boost = sbufReadU8(src);
3150 currentPidProfile->feedforward_max_rate_limit = sbufReadU8(src);
3151 currentPidProfile->feedforward_jitter_factor = sbufReadU8(src);
3152 #else
3153 sbufReadU8(src);
3154 sbufReadU8(src);
3155 sbufReadU8(src);
3156 sbufReadU8(src);
3157 sbufReadU8(src);
3158 #endif
3159
3160 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
3161 currentPidProfile->vbat_sag_compensation = sbufReadU8(src);
3162 #else
3163 sbufReadU8(src);
3164 #endif
3165 #if defined(USE_THRUST_LINEARIZATION)
3166 currentPidProfile->thrustLinearization = sbufReadU8(src);
3167 #else
3168 sbufReadU8(src);
3169 #endif
3170 }
3171 if (sbufBytesRemaining(src) >= 3) {
3172 // Added in API 1.45
3173 value = sbufReadU8(src);
3174 currentPidProfile->tpa_rate = MIN(value, TPA_MAX);
3175 currentPidProfile->tpa_breakpoint = sbufReadU16(src);
3176 }
3177
3178 pidInitConfig(currentPidProfile);
3179 initEscEndpoints();
3180 mixerInitProfile();
3181
3182 break;
3183 case MSP_SET_SENSOR_CONFIG:
3184 #if defined(USE_ACC)
3185 accelerometerConfigMutable()->acc_hardware = sbufReadU8(src);
3186 #else
3187 sbufReadU8(src);
3188 #endif
3189 #if defined(USE_BARO)
3190 barometerConfigMutable()->baro_hardware = sbufReadU8(src);
3191 #else
3192 sbufReadU8(src);
3193 #endif
3194 #if defined(USE_MAG)
3195 compassConfigMutable()->mag_hardware = sbufReadU8(src);
3196 #else
3197 sbufReadU8(src);
3198 #endif
3199 break;
3200
3201 #ifdef USE_ACC
3202 case MSP_ACC_CALIBRATION:
3203 if (!ARMING_FLAG(ARMED))
3204 accStartCalibration();
3205 break;
3206 #endif
3207
3208 #if defined(USE_MAG)
3209 case MSP_MAG_CALIBRATION:
3210 if (!ARMING_FLAG(ARMED)) {
3211 compassStartCalibration();
3212 }
3213 #endif
3214
3215 break;
3216 case MSP_EEPROM_WRITE:
3217 if (ARMING_FLAG(ARMED)) {
3218 return MSP_RESULT_ERROR;
3219 }
3220
3221 // This is going to take some time and won't be done where real-time performance is needed so
3222 // ignore how long it takes to avoid confusing the scheduler
3223 schedulerIgnoreTaskStateTime();
3224
3225 #if defined(USE_MSP_OVER_TELEMETRY)
3226 if (featureIsEnabled(FEATURE_RX_SPI) && srcDesc == getMspTelemetryDescriptor()) {
3227 dispatchAdd(&writeReadEepromEntry, MSP_DISPATCH_DELAY_US);
3228 } else
3229 #endif
3230 {
3231 writeReadEeprom(NULL);
3232 }
3233
3234 break;
3235
3236 #ifdef USE_BLACKBOX
3237 case MSP_SET_BLACKBOX_CONFIG:
3238 // Don't allow config to be updated while Blackbox is logging
3239 if (blackboxMayEditConfig()) {
3240 blackboxConfigMutable()->device = sbufReadU8(src);
3241 const int rateNum = sbufReadU8(src); // was rate_num
3242 const int rateDenom = sbufReadU8(src); // was rate_denom
3243 uint16_t pRatio = 0;
3244 if (sbufBytesRemaining(src) >= 2) {
3245 // p_ratio specified, so use it directly
3246 pRatio = sbufReadU16(src);
3247 } else {
3248 // p_ratio not specified in MSP, so calculate it from old rateNum and rateDenom
3249 pRatio = blackboxCalculatePDenom(rateNum, rateDenom);
3250 }
3251
3252 if (sbufBytesRemaining(src) >= 1) {
3253 // sample_rate specified, so use it directly
3254 blackboxConfigMutable()->sample_rate = sbufReadU8(src);
3255 } else {
3256 // sample_rate not specified in MSP, so calculate it from old p_ratio
3257 blackboxConfigMutable()->sample_rate = blackboxCalculateSampleRate(pRatio);
3258 }
3259
3260 // Added in MSP API 1.45
3261 if (sbufBytesRemaining(src) >= 4) {
3262 blackboxConfigMutable()->fields_disabled_mask = sbufReadU32(src);
3263 }
3264 }
3265 break;
3266 #endif
3267
3268 #ifdef USE_VTX_COMMON
3269 case MSP_SET_VTX_CONFIG:
3270 {
3271 vtxDevice_t *vtxDevice = vtxCommonDevice();
3272 vtxDevType_e vtxType = VTXDEV_UNKNOWN;
3273 if (vtxDevice) {
3274 vtxType = vtxCommonGetDeviceType(vtxDevice);
3275 }
3276 uint16_t newFrequency = sbufReadU16(src);
3277 if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { // Value is band and channel
3278 const uint8_t newBand = (newFrequency / 8) + 1;
3279 const uint8_t newChannel = (newFrequency % 8) + 1;
3280 vtxSettingsConfigMutable()->band = newBand;
3281 vtxSettingsConfigMutable()->channel = newChannel;
3282 vtxSettingsConfigMutable()->freq = vtxCommonLookupFrequency(vtxDevice, newBand, newChannel);
3283 } else if (newFrequency <= VTX_SETTINGS_MAX_FREQUENCY_MHZ) { // Value is frequency in MHz
3284 vtxSettingsConfigMutable()->band = 0;
3285 vtxSettingsConfigMutable()->freq = newFrequency;
3286 }
3287
3288 if (sbufBytesRemaining(src) >= 2) {
3289 vtxSettingsConfigMutable()->power = sbufReadU8(src);
3290 const uint8_t newPitmode = sbufReadU8(src);
3291 if (vtxType != VTXDEV_UNKNOWN) {
3292 // Delegate pitmode to vtx directly
3293 unsigned vtxCurrentStatus;
3294 vtxCommonGetStatus(vtxDevice, &vtxCurrentStatus);
3295 if ((bool)(vtxCurrentStatus & VTX_STATUS_PIT_MODE) != (bool)newPitmode) {
3296 vtxCommonSetPitMode(vtxDevice, newPitmode);
3297 }
3298 }
3299 }
3300
3301 if (sbufBytesRemaining(src)) {
3302 vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src);
3303 }
3304
3305 // API version 1.42 - this parameter kept separate since clients may already be supplying
3306 if (sbufBytesRemaining(src) >= 2) {
3307 vtxSettingsConfigMutable()->pitModeFreq = sbufReadU16(src);
3308 }
3309
3310 // API version 1.42 - extensions for non-encoded versions of the band, channel or frequency
3311 if (sbufBytesRemaining(src) >= 4) {
3312 // Added standalone values for band, channel and frequency to move
3313 // away from the flawed encoded combined method originally implemented.
3314 uint8_t newBand = sbufReadU8(src);
3315 const uint8_t newChannel = sbufReadU8(src);
3316 uint16_t newFreq = sbufReadU16(src);
3317 if (newBand) {
3318 newFreq = vtxCommonLookupFrequency(vtxDevice, newBand, newChannel);
3319 }
3320 vtxSettingsConfigMutable()->band = newBand;
3321 vtxSettingsConfigMutable()->channel = newChannel;
3322 vtxSettingsConfigMutable()->freq = newFreq;
3323 }
3324
3325 // API version 1.42 - extensions for vtxtable support
3326 if (sbufBytesRemaining(src) >= 4) {
3327 #ifdef USE_VTX_TABLE
3328 const uint8_t newBandCount = sbufReadU8(src);
3329 const uint8_t newChannelCount = sbufReadU8(src);
3330 const uint8_t newPowerCount = sbufReadU8(src);
3331
3332 if ((newBandCount > VTX_TABLE_MAX_BANDS) ||
3333 (newChannelCount > VTX_TABLE_MAX_CHANNELS) ||
3334 (newPowerCount > VTX_TABLE_MAX_POWER_LEVELS)) {
3335 return MSP_RESULT_ERROR;
3336 }
3337 vtxTableConfigMutable()->bands = newBandCount;
3338 vtxTableConfigMutable()->channels = newChannelCount;
3339 vtxTableConfigMutable()->powerLevels = newPowerCount;
3340
3341 // boolean to determine whether the vtxtable should be cleared in
3342 // expectation that the detailed band/channel and power level messages
3343 // will follow to repopulate the tables
3344 if (sbufReadU8(src)) {
3345 for (int i = 0; i < VTX_TABLE_MAX_BANDS; i++) {
3346 vtxTableConfigClearBand(vtxTableConfigMutable(), i);
3347 vtxTableConfigClearChannels(vtxTableConfigMutable(), i, 0);
3348 }
3349 vtxTableConfigClearPowerLabels(vtxTableConfigMutable(), 0);
3350 vtxTableConfigClearPowerValues(vtxTableConfigMutable(), 0);
3351 }
3352 #else
3353 sbufReadU8(src);
3354 sbufReadU8(src);
3355 sbufReadU8(src);
3356 sbufReadU8(src);
3357 #endif
3358 }
3359 #ifdef USE_VTX_MSP
3360 setMspVtxDeviceStatusReady(srcDesc);
3361 #endif
3362 }
3363 break;
3364 #endif
3365
3366 #ifdef USE_VTX_TABLE
3367 case MSP_SET_VTXTABLE_BAND:
3368 {
3369 char bandName[VTX_TABLE_BAND_NAME_LENGTH + 1];
3370 memset(bandName, 0, VTX_TABLE_BAND_NAME_LENGTH + 1);
3371 uint16_t frequencies[VTX_TABLE_MAX_CHANNELS];
3372 const uint8_t band = sbufReadU8(src);
3373 const uint8_t bandNameLength = sbufReadU8(src);
3374 for (int i = 0; i < bandNameLength; i++) {
3375 const char nameChar = sbufReadU8(src);
3376 if (i < VTX_TABLE_BAND_NAME_LENGTH) {
3377 bandName[i] = toupper(nameChar);
3378 }
3379 }
3380 const char bandLetter = toupper(sbufReadU8(src));
3381 const bool isFactoryBand = (bool)sbufReadU8(src);
3382 const uint8_t channelCount = sbufReadU8(src);
3383 for (int i = 0; i < channelCount; i++) {
3384 const uint16_t frequency = sbufReadU16(src);
3385 if (i < vtxTableConfig()->channels) {
3386 frequencies[i] = frequency;
3387 }
3388 }
3389
3390 if (band > 0 && band <= vtxTableConfig()->bands) {
3391 vtxTableStrncpyWithPad(vtxTableConfigMutable()->bandNames[band - 1], bandName, VTX_TABLE_BAND_NAME_LENGTH);
3392 vtxTableConfigMutable()->bandLetters[band - 1] = bandLetter;
3393 vtxTableConfigMutable()->isFactoryBand[band - 1] = isFactoryBand;
3394 for (int i = 0; i < vtxTableConfig()->channels; i++) {
3395 vtxTableConfigMutable()->frequency[band - 1][i] = frequencies[i];
3396 }
3397 // If this is the currently selected band then reset the frequency
3398 if (band == vtxSettingsConfig()->band) {
3399 uint16_t newFreq = 0;
3400 if (vtxSettingsConfig()->channel > 0 && vtxSettingsConfig()->channel <= vtxTableConfig()->channels) {
3401 newFreq = frequencies[vtxSettingsConfig()->channel - 1];
3402 }
3403 vtxSettingsConfigMutable()->freq = newFreq;
3404 }
3405 vtxTableNeedsInit = true; // reinintialize vtxtable after eeprom write
3406 } else {
3407 return MSP_RESULT_ERROR;
3408 }
3409 #ifdef USE_VTX_MSP
3410 setMspVtxDeviceStatusReady(srcDesc);
3411 #endif
3412 }
3413 break;
3414
3415 case MSP_SET_VTXTABLE_POWERLEVEL:
3416 {
3417 char powerLevelLabel[VTX_TABLE_POWER_LABEL_LENGTH + 1];
3418 memset(powerLevelLabel, 0, VTX_TABLE_POWER_LABEL_LENGTH + 1);
3419 const uint8_t powerLevel = sbufReadU8(src);
3420 const uint16_t powerValue = sbufReadU16(src);
3421 const uint8_t powerLevelLabelLength = sbufReadU8(src);
3422 for (int i = 0; i < powerLevelLabelLength; i++) {
3423 const char labelChar = sbufReadU8(src);
3424 if (i < VTX_TABLE_POWER_LABEL_LENGTH) {
3425 powerLevelLabel[i] = toupper(labelChar);
3426 }
3427 }
3428
3429 if (powerLevel > 0 && powerLevel <= vtxTableConfig()->powerLevels) {
3430 vtxTableConfigMutable()->powerValues[powerLevel - 1] = powerValue;
3431 vtxTableStrncpyWithPad(vtxTableConfigMutable()->powerLabels[powerLevel - 1], powerLevelLabel, VTX_TABLE_POWER_LABEL_LENGTH);
3432 vtxTableNeedsInit = true; // reinintialize vtxtable after eeprom write
3433 } else {
3434 return MSP_RESULT_ERROR;
3435 }
3436 #ifdef USE_VTX_MSP
3437 setMspVtxDeviceStatusReady(srcDesc);
3438 #endif
3439 }
3440 break;
3441 #endif
3442
3443 case MSP2_SET_MOTOR_OUTPUT_REORDERING:
3444 {
3445 const uint8_t arraySize = sbufReadU8(src);
3446
3447 for (unsigned i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
3448 uint8_t value = i;
3449
3450 if (i < arraySize) {
3451 value = sbufReadU8(src);
3452 }
3453
3454 motorConfigMutable()->dev.motorOutputReordering[i] = value;
3455 }
3456 }
3457 break;
3458
3459 #ifdef USE_DSHOT
3460 case MSP2_SEND_DSHOT_COMMAND:
3461 {
3462 const bool armed = ARMING_FLAG(ARMED);
3463
3464 if (!armed) {
3465 const uint8_t commandType = sbufReadU8(src);
3466 const uint8_t motorIndex = sbufReadU8(src);
3467 const uint8_t commandCount = sbufReadU8(src);
3468
3469 if (DSHOT_CMD_TYPE_BLOCKING == commandType) {
3470 motorDisable();
3471 }
3472
3473 for (uint8_t i = 0; i < commandCount; i++) {
3474 const uint8_t commandIndex = sbufReadU8(src);
3475 dshotCommandWrite(motorIndex, getMotorCount(), commandIndex, commandType);
3476 }
3477
3478 if (DSHOT_CMD_TYPE_BLOCKING == commandType) {
3479 motorEnable();
3480 }
3481 }
3482 }
3483 break;
3484 #endif
3485
3486 #ifdef USE_SIMPLIFIED_TUNING
3487 // Added in MSP API 1.44
3488 case MSP_SET_SIMPLIFIED_TUNING:
3489 {
3490 readSimplifiedPids(currentPidProfile, src);
3491 readSimplifiedDtermFilters(currentPidProfile, src);
3492 readSimplifiedGyroFilters(gyroConfigMutable(), src);
3493 applySimplifiedTuning(currentPidProfile, gyroConfigMutable());
3494 }
3495 break;
3496 #endif
3497
3498 #ifdef USE_CAMERA_CONTROL
3499 case MSP_CAMERA_CONTROL:
3500 {
3501 if (ARMING_FLAG(ARMED)) {
3502 return MSP_RESULT_ERROR;
3503 }
3504
3505 const uint8_t key = sbufReadU8(src);
3506 cameraControlKeyPress(key, 0);
3507 }
3508 break;
3509 #endif
3510
3511 case MSP_SET_ARMING_DISABLED:
3512 {
3513 const uint8_t command = sbufReadU8(src);
3514 uint8_t disableRunawayTakeoff = 0;
3515 #ifndef USE_RUNAWAY_TAKEOFF
3516 UNUSED(disableRunawayTakeoff);
3517 #endif
3518 if (sbufBytesRemaining(src)) {
3519 disableRunawayTakeoff = sbufReadU8(src);
3520 }
3521 if (command) {
3522 mspArmingDisableByDescriptor(srcDesc);
3523 setArmingDisabled(ARMING_DISABLED_MSP);
3524 if (ARMING_FLAG(ARMED)) {
3525 disarm(DISARM_REASON_ARMING_DISABLED);
3526 }
3527 #ifdef USE_RUNAWAY_TAKEOFF
3528 runawayTakeoffTemporaryDisable(false);
3529 #endif
3530 } else {
3531 mspArmingEnableByDescriptor(srcDesc);
3532 if (mspIsMspArmingEnabled()) {
3533 unsetArmingDisabled(ARMING_DISABLED_MSP);
3534 #ifdef USE_RUNAWAY_TAKEOFF
3535 runawayTakeoffTemporaryDisable(disableRunawayTakeoff);
3536 #endif
3537 }
3538 }
3539 }
3540 break;
3541
3542 #ifdef USE_FLASHFS
3543 case MSP_DATAFLASH_ERASE:
3544 blackboxEraseAll();
3545
3546 break;
3547 #endif
3548
3549 #ifdef USE_GPS
3550 case MSP_SET_RAW_GPS:
3551 gpsSetFixState(sbufReadU8(src));
3552 gpsSol.numSat = sbufReadU8(src);
3553 gpsSol.llh.lat = sbufReadU32(src);
3554 gpsSol.llh.lon = sbufReadU32(src);
3555 gpsSol.llh.altCm = sbufReadU16(src) * 100; // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. Received MSP altitudes in 1m per lsb have to upscaled.
3556 gpsSol.groundSpeed = sbufReadU16(src);
3557 GPS_update |= GPS_MSP_UPDATE; // MSP data signalisation to GPS functions
3558 break;
3559 #endif // USE_GPS
3560 case MSP_SET_FEATURE_CONFIG:
3561 featureConfigReplace(sbufReadU32(src));
3562 break;
3563
3564 #ifdef USE_BEEPER
3565 case MSP_SET_BEEPER_CONFIG:
3566 beeperConfigMutable()->beeper_off_flags = sbufReadU32(src);
3567 if (sbufBytesRemaining(src) >= 1) {
3568 beeperConfigMutable()->dshotBeaconTone = sbufReadU8(src);
3569 }
3570 if (sbufBytesRemaining(src) >= 4) {
3571 beeperConfigMutable()->dshotBeaconOffFlags = sbufReadU32(src);
3572 }
3573 break;
3574 #endif
3575
3576 case MSP_SET_BOARD_ALIGNMENT_CONFIG:
3577 boardAlignmentMutable()->rollDegrees = sbufReadU16(src);
3578 boardAlignmentMutable()->pitchDegrees = sbufReadU16(src);
3579 boardAlignmentMutable()->yawDegrees = sbufReadU16(src);
3580 break;
3581
3582 case MSP_SET_MIXER_CONFIG:
3583 #ifndef USE_QUAD_MIXER_ONLY
3584 mixerConfigMutable()->mixerMode = sbufReadU8(src);
3585 #else
3586 sbufReadU8(src);
3587 #endif
3588 if (sbufBytesRemaining(src) >= 1) {
3589 mixerConfigMutable()->yaw_motors_reversed = sbufReadU8(src);
3590 }
3591 break;
3592
3593 case MSP_SET_RX_CONFIG:
3594 rxConfigMutable()->serialrx_provider = sbufReadU8(src);
3595 rxConfigMutable()->maxcheck = sbufReadU16(src);
3596 rxConfigMutable()->midrc = sbufReadU16(src);
3597 rxConfigMutable()->mincheck = sbufReadU16(src);
3598 rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src);
3599 if (sbufBytesRemaining(src) >= 4) {
3600 rxConfigMutable()->rx_min_usec = sbufReadU16(src);
3601 rxConfigMutable()->rx_max_usec = sbufReadU16(src);
3602 }
3603 if (sbufBytesRemaining(src) >= 4) {
3604 sbufReadU8(src); // not required in API 1.44, was rxConfigMutable()->rcInterpolation
3605 sbufReadU8(src); // not required in API 1.44, was rxConfigMutable()->rcInterpolationInterval
3606 rxConfigMutable()->airModeActivateThreshold = (sbufReadU16(src) - 1000) / 10;
3607 }
3608 if (sbufBytesRemaining(src) >= 6) {
3609 #ifdef USE_RX_SPI
3610 rxSpiConfigMutable()->rx_spi_protocol = sbufReadU8(src);
3611 rxSpiConfigMutable()->rx_spi_id = sbufReadU32(src);
3612 rxSpiConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(src);
3613 #else
3614 sbufReadU8(src);
3615 sbufReadU32(src);
3616 sbufReadU8(src);
3617 #endif
3618 }
3619 if (sbufBytesRemaining(src) >= 1) {
3620 rxConfigMutable()->fpvCamAngleDegrees = sbufReadU8(src);
3621 }
3622 if (sbufBytesRemaining(src) >= 6) {
3623 // Added in MSP API 1.40
3624 sbufReadU8(src); // not required in API 1.44, was rxConfigMutable()->rcSmoothingChannels
3625 #if defined(USE_RC_SMOOTHING_FILTER)
3626 sbufReadU8(src); // not required in API 1.44, was rc_smoothing_type
3627 configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_setpoint_cutoff, sbufReadU8(src));
3628 configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_feedforward_cutoff, sbufReadU8(src));
3629 sbufReadU8(src); // not required in API 1.44, was rc_smoothing_input_type
3630 sbufReadU8(src); // not required in API 1.44, was rc_smoothing_derivative_type
3631 #else
3632 sbufReadU8(src);
3633 sbufReadU8(src);
3634 sbufReadU8(src);
3635 sbufReadU8(src);
3636 sbufReadU8(src);
3637 #endif
3638 }
3639 if (sbufBytesRemaining(src) >= 1) {
3640 // Added in MSP API 1.40
3641 // Kept separate from the section above to work around missing Configurator support in version < 10.4.2
3642 #if defined(USE_USB_CDC_HID)
3643 usbDevConfigMutable()->type = sbufReadU8(src);
3644 #else
3645 sbufReadU8(src);
3646 #endif
3647 }
3648 if (sbufBytesRemaining(src) >= 1) {
3649 // Added in MSP API 1.42
3650 #if defined(USE_RC_SMOOTHING_FILTER)
3651 // Added extra validation/range constraint for rc_smoothing_auto_factor as a workaround for a bug in
3652 // the 10.6 configurator where it was possible to submit an invalid out-of-range value. We might be
3653 // able to remove the constraint at some point in the future once the affected versions are deprecated
3654 // enough that the risk is low.
3655 configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_auto_factor_rpy, constrain(sbufReadU8(src), RC_SMOOTHING_AUTO_FACTOR_MIN, RC_SMOOTHING_AUTO_FACTOR_MAX));
3656 #else
3657 sbufReadU8(src);
3658 #endif
3659 }
3660 if (sbufBytesRemaining(src) >= 1) {
3661 // Added in MSP API 1.44
3662 #if defined(USE_RC_SMOOTHING_FILTER)
3663 configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_mode, sbufReadU8(src));
3664 #else
3665 sbufReadU8(src);
3666 #endif
3667 }
3668 if (sbufBytesRemaining(src) >= 6) {
3669 // Added in MSP API 1.45
3670 #ifdef USE_RX_EXPRESSLRS
3671 sbufReadData(src, rxExpressLrsSpiConfigMutable()->UID, 6);
3672 #else
3673 uint8_t emptyUid[6];
3674 sbufReadData(src, emptyUid, 6);
3675 #endif
3676 }
3677 break;
3678 case MSP_SET_FAILSAFE_CONFIG:
3679 failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
3680 failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src);
3681 failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src);
3682 failsafeConfigMutable()->failsafe_switch_mode = sbufReadU8(src);
3683 failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src);
3684 failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src);
3685 break;
3686
3687 case MSP_SET_RXFAIL_CONFIG:
3688 i = sbufReadU8(src);
3689 if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) {
3690 rxFailsafeChannelConfigsMutable(i)->mode = sbufReadU8(src);
3691 rxFailsafeChannelConfigsMutable(i)->step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
3692 } else {
3693 return MSP_RESULT_ERROR;
3694 }
3695 break;
3696
3697 case MSP_SET_RSSI_CONFIG:
3698 rxConfigMutable()->rssi_channel = sbufReadU8(src);
3699 break;
3700
3701 case MSP_SET_RX_MAP:
3702 for (int i = 0; i < RX_MAPPABLE_CHANNEL_COUNT; i++) {
3703 rxConfigMutable()->rcmap[i] = sbufReadU8(src);
3704 }
3705 break;
3706
3707 case MSP_SET_CF_SERIAL_CONFIG:
3708 {
3709 uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4);
3710
3711 if (dataSize % portConfigSize != 0) {
3712 return MSP_RESULT_ERROR;
3713 }
3714
3715 uint8_t remainingPortsInPacket = dataSize / portConfigSize;
3716
3717 while (remainingPortsInPacket--) {
3718 uint8_t identifier = sbufReadU8(src);
3719
3720 serialPortConfig_t *portConfig = serialFindPortConfigurationMutable(identifier);
3721
3722 if (!portConfig) {
3723 return MSP_RESULT_ERROR;
3724 }
3725
3726 portConfig->identifier = identifier;
3727 portConfig->functionMask = sbufReadU16(src);
3728 portConfig->msp_baudrateIndex = sbufReadU8(src);
3729 portConfig->gps_baudrateIndex = sbufReadU8(src);
3730 portConfig->telemetry_baudrateIndex = sbufReadU8(src);
3731 portConfig->blackbox_baudrateIndex = sbufReadU8(src);
3732 }
3733 }
3734 break;
3735 case MSP2_COMMON_SET_SERIAL_CONFIG: {
3736 if (dataSize < 1) {
3737 return MSP_RESULT_ERROR;
3738 }
3739 unsigned count = sbufReadU8(src);
3740 unsigned portConfigSize = (dataSize - 1) / count;
3741 unsigned expectedPortSize = sizeof(uint8_t) + sizeof(uint32_t) + (sizeof(uint8_t) * 4);
3742 if (portConfigSize < expectedPortSize) {
3743 return MSP_RESULT_ERROR;
3744 }
3745 for (unsigned ii = 0; ii < count; ii++) {
3746 unsigned start = sbufBytesRemaining(src);
3747 uint8_t identifier = sbufReadU8(src);
3748 serialPortConfig_t *portConfig = serialFindPortConfigurationMutable(identifier);
3749
3750 if (!portConfig) {
3751 return MSP_RESULT_ERROR;
3752 }
3753
3754 portConfig->identifier = identifier;
3755 portConfig->functionMask = sbufReadU32(src);
3756 portConfig->msp_baudrateIndex = sbufReadU8(src);
3757 portConfig->gps_baudrateIndex = sbufReadU8(src);
3758 portConfig->telemetry_baudrateIndex = sbufReadU8(src);
3759 portConfig->blackbox_baudrateIndex = sbufReadU8(src);
3760 // Skip unknown bytes
3761 while (start - sbufBytesRemaining(src) < portConfigSize && sbufBytesRemaining(src)) {
3762 sbufReadU8(src);
3763 }
3764 }
3765 break;
3766 }
3767
3768 #ifdef USE_LED_STRIP_STATUS_MODE
3769 case MSP_SET_LED_COLORS:
3770 for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
3771 hsvColor_t *color = &ledStripStatusModeConfigMutable()->colors[i];
3772 color->h = sbufReadU16(src);
3773 color->s = sbufReadU8(src);
3774 color->v = sbufReadU8(src);
3775 }
3776 break;
3777 #endif
3778
3779 #ifdef USE_LED_STRIP
3780 case MSP_SET_LED_STRIP_CONFIG:
3781 {
3782 i = sbufReadU8(src);
3783 if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) {
3784 return MSP_RESULT_ERROR;
3785 }
3786 #ifdef USE_LED_STRIP_STATUS_MODE
3787 ledConfig_t *ledConfig = &ledStripStatusModeConfigMutable()->ledConfigs[i];
3788 *ledConfig = sbufReadU32(src);
3789 reevaluateLedConfig();
3790 #else
3791 sbufReadU32(src);
3792 #endif
3793 // API 1.41 - selected ledstrip_profile
3794 if (sbufBytesRemaining(src) >= 1) {
3795 ledStripConfigMutable()->ledstrip_profile = sbufReadU8(src);
3796 }
3797 }
3798 break;
3799 #endif
3800
3801 #ifdef USE_LED_STRIP_STATUS_MODE
3802 case MSP_SET_LED_STRIP_MODECOLOR:
3803 {
3804 ledModeIndex_e modeIdx = sbufReadU8(src);
3805 int funIdx = sbufReadU8(src);
3806 int color = sbufReadU8(src);
3807
3808 if (!setModeColor(modeIdx, funIdx, color)) {
3809 return MSP_RESULT_ERROR;
3810 }
3811 }
3812 break;
3813 #endif
3814
3815 #ifdef USE_RTC_TIME
3816 case MSP_SET_RTC:
3817 {
3818 // Use seconds and milliseconds to make senders
3819 // easier to implement. Generating a 64 bit value
3820 // might not be trivial in some platforms.
3821 int32_t secs = (int32_t)sbufReadU32(src);
3822 uint16_t millis = sbufReadU16(src);
3823 rtcTime_t t = rtcTimeMake(secs, millis);
3824 rtcSet(&t);
3825 }
3826
3827 break;
3828 #endif
3829
3830 case MSP_SET_TX_INFO:
3831 setRssiMsp(sbufReadU8(src));
3832
3833 break;
3834
3835 #if defined(USE_BOARD_INFO)
3836 case MSP_SET_BOARD_INFO:
3837 if (!boardInformationIsSet()) {
3838 uint8_t length = sbufReadU8(src);
3839 char boardName[MAX_BOARD_NAME_LENGTH + 1];
3840 sbufReadData(src, boardName, MIN(length, MAX_BOARD_NAME_LENGTH));
3841 if (length > MAX_BOARD_NAME_LENGTH) {
3842 sbufAdvance(src, length - MAX_BOARD_NAME_LENGTH);
3843 length = MAX_BOARD_NAME_LENGTH;
3844 }
3845 boardName[length] = '\0';
3846 length = sbufReadU8(src);
3847 char manufacturerId[MAX_MANUFACTURER_ID_LENGTH + 1];
3848 sbufReadData(src, manufacturerId, MIN(length, MAX_MANUFACTURER_ID_LENGTH));
3849 if (length > MAX_MANUFACTURER_ID_LENGTH) {
3850 sbufAdvance(src, length - MAX_MANUFACTURER_ID_LENGTH);
3851 length = MAX_MANUFACTURER_ID_LENGTH;
3852 }
3853 manufacturerId[length] = '\0';
3854
3855 setBoardName(boardName);
3856 setManufacturerId(manufacturerId);
3857 persistBoardInformation();
3858 } else {
3859 return MSP_RESULT_ERROR;
3860 }
3861
3862 break;
3863 #if defined(USE_SIGNATURE)
3864 case MSP_SET_SIGNATURE:
3865 if (!signatureIsSet()) {
3866 uint8_t signature[SIGNATURE_LENGTH];
3867 sbufReadData(src, signature, SIGNATURE_LENGTH);
3868 setSignature(signature);
3869 persistSignature();
3870 } else {
3871 return MSP_RESULT_ERROR;
3872 }
3873
3874 break;
3875 #endif
3876 #endif // USE_BOARD_INFO
3877 #if defined(USE_RX_BIND)
3878 case MSP2_BETAFLIGHT_BIND:
3879 if (!startRxBind()) {
3880 return MSP_RESULT_ERROR;
3881 }
3882
3883 break;
3884 #endif
3885
3886 case MSP2_SET_TEXT:
3887 {
3888 // type byte, then length byte followed by the actual characters
3889 const uint8_t textType = sbufReadU8(src);
3890
3891 char* textVar;
3892 const uint8_t textLength = MIN(MAX_NAME_LENGTH, sbufReadU8(src));
3893 switch (textType) {
3894 case MSP2TEXT_PILOT_NAME:
3895 textVar = pilotConfigMutable()->pilotName;
3896 break;
3897
3898 case MSP2TEXT_CRAFT_NAME:
3899 textVar = pilotConfigMutable()->craftName;
3900 break;
3901
3902 default:
3903 return MSP_RESULT_ERROR;
3904 }
3905
3906 memset(textVar, 0, strlen(textVar));
3907 for (unsigned int i = 0; i < textLength; i++) {
3908 textVar[i] = sbufReadU8(src);
3909 }
3910
3911 #ifdef USE_OSD
3912 if (textType == MSP2TEXT_PILOT_NAME || textType == MSP2TEXT_CRAFT_NAME) {
3913 osdAnalyzeActiveElements();
3914 }
3915 #endif
3916 }
3917 break;
3918
3919 default:
3920 // we do not know how to handle the (valid) message, indicate error MSP $M!
3921 return MSP_RESULT_ERROR;
3922 }
3923 return MSP_RESULT_ACK;
3924 }
3925
3926 static mspResult_e mspCommonProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
3927 {
3928 UNUSED(mspPostProcessFn);
3929 const unsigned int dataSize = sbufBytesRemaining(src);
3930 UNUSED(dataSize); // maybe unused due to compiler options
3931
3932 switch (cmdMSP) {
3933 #ifdef USE_TRANSPONDER
3934 case MSP_SET_TRANSPONDER_CONFIG: {
3935 // Backward compatibility to BFC 3.1.1 is lost for this message type
3936
3937 uint8_t provider = sbufReadU8(src);
3938 uint8_t bytesRemaining = dataSize - 1;
3939
3940 if (provider > TRANSPONDER_PROVIDER_COUNT) {
3941 return MSP_RESULT_ERROR;
3942 }
3943
3944 const uint8_t requirementIndex = provider - 1;
3945 const uint8_t transponderDataSize = transponderRequirements[requirementIndex].dataLength;
3946
3947 transponderConfigMutable()->provider = provider;
3948
3949 if (provider == TRANSPONDER_NONE) {
3950 break;
3951 }
3952
3953 if (bytesRemaining != transponderDataSize) {
3954 return MSP_RESULT_ERROR;
3955 }
3956
3957 if (provider != transponderConfig()->provider) {
3958 transponderStopRepeating();
3959 }
3960
3961 memset(transponderConfigMutable()->data, 0, sizeof(transponderConfig()->data));
3962
3963 for (unsigned int i = 0; i < transponderDataSize; i++) {
3964 transponderConfigMutable()->data[i] = sbufReadU8(src);
3965 }
3966 transponderUpdateData();
3967 break;
3968 }
3969 #endif
3970
3971 case MSP_SET_VOLTAGE_METER_CONFIG: {
3972 int8_t id = sbufReadU8(src);
3973
3974 //
3975 // find and configure an ADC voltage sensor
3976 //
3977 int8_t voltageSensorADCIndex;
3978 for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) {
3979 if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) {
3980 break;
3981 }
3982 }
3983
3984 if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) {
3985 voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src);
3986 voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src);
3987 voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src);
3988 } else {
3989 // if we had any other types of voltage sensor to configure, this is where we'd do it.
3990 sbufReadU8(src);
3991 sbufReadU8(src);
3992 sbufReadU8(src);
3993 }
3994 break;
3995 }
3996
3997 case MSP_SET_CURRENT_METER_CONFIG: {
3998 int id = sbufReadU8(src);
3999
4000 switch (id) {
4001 case CURRENT_METER_ID_BATTERY_1:
4002 currentSensorADCConfigMutable()->scale = sbufReadU16(src);
4003 currentSensorADCConfigMutable()->offset = sbufReadU16(src);
4004 break;
4005 #ifdef USE_VIRTUAL_CURRENT_METER
4006 case CURRENT_METER_ID_VIRTUAL_1:
4007 currentSensorVirtualConfigMutable()->scale = sbufReadU16(src);
4008 currentSensorVirtualConfigMutable()->offset = sbufReadU16(src);
4009 break;
4010 #endif
4011 default:
4012 sbufReadU16(src);
4013 sbufReadU16(src);
4014 break;
4015 }
4016 break;
4017 }
4018
4019 case MSP_SET_BATTERY_CONFIG:
4020 batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src) * 10; // vbatlevel_warn1 in MWC2.3 GUI
4021 batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src) * 10; // vbatlevel_warn2 in MWC2.3 GUI
4022 batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src) * 10; // vbatlevel when buzzer starts to alert
4023 batteryConfigMutable()->batteryCapacity = sbufReadU16(src);
4024 batteryConfigMutable()->voltageMeterSource = sbufReadU8(src);
4025 batteryConfigMutable()->currentMeterSource = sbufReadU8(src);
4026 if (sbufBytesRemaining(src) >= 6) {
4027 batteryConfigMutable()->vbatmincellvoltage = sbufReadU16(src);
4028 batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU16(src);
4029 batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU16(src);
4030 }
4031 break;
4032
4033 #if defined(USE_OSD)
4034 case MSP_SET_OSD_CONFIG:
4035 {
4036 const uint8_t addr = sbufReadU8(src);
4037
4038 if ((int8_t)addr == -1) {
4039 /* Set general OSD settings */
4040 #ifdef USE_MAX7456
4041 vcdProfileMutable()->video_system = sbufReadU8(src);
4042 #else
4043 sbufReadU8(src); // Skip video system
4044 #endif
4045 #if defined(USE_OSD)
4046 osdConfigMutable()->units = sbufReadU8(src);
4047
4048 // Alarms
4049 osdConfigMutable()->rssi_alarm = sbufReadU8(src);
4050 osdConfigMutable()->cap_alarm = sbufReadU16(src);
4051 sbufReadU16(src); // Skip unused (previously fly timer)
4052 osdConfigMutable()->alt_alarm = sbufReadU16(src);
4053
4054 if (sbufBytesRemaining(src) >= 2) {
4055 /* Enabled warnings */
4056 // API < 1.41 supports only the low 16 bits
4057 osdConfigMutable()->enabledWarnings = sbufReadU16(src);
4058 }
4059
4060 if (sbufBytesRemaining(src) >= 4) {
4061 // 32bit version of enabled warnings (API >= 1.41)
4062 osdConfigMutable()->enabledWarnings = sbufReadU32(src);
4063 }
4064
4065 if (sbufBytesRemaining(src) >= 1) {
4066 // API >= 1.41
4067 // selected OSD profile
4068 #ifdef USE_OSD_PROFILES
4069 changeOsdProfileIndex(sbufReadU8(src));
4070 #else
4071 sbufReadU8(src);
4072 #endif // USE_OSD_PROFILES
4073 }
4074
4075 if (sbufBytesRemaining(src) >= 1) {
4076 // API >= 1.41
4077 // OSD stick overlay mode
4078
4079 #ifdef USE_OSD_STICK_OVERLAY
4080 osdConfigMutable()->overlay_radio_mode = sbufReadU8(src);
4081 #else
4082 sbufReadU8(src);
4083 #endif // USE_OSD_STICK_OVERLAY
4084
4085 }
4086
4087 if (sbufBytesRemaining(src) >= 2) {
4088 // API >= 1.43
4089 // OSD camera frame element width/height
4090 osdConfigMutable()->camera_frame_width = sbufReadU8(src);
4091 osdConfigMutable()->camera_frame_height = sbufReadU8(src);
4092 }
4093 #endif
4094 } else if ((int8_t)addr == -2) {
4095 #if defined(USE_OSD)
4096 // Timers
4097 uint8_t index = sbufReadU8(src);
4098 if (index > OSD_TIMER_COUNT) {
4099 return MSP_RESULT_ERROR;
4100 }
4101 osdConfigMutable()->timers[index] = sbufReadU16(src);
4102 #endif
4103 return MSP_RESULT_ERROR;
4104 } else {
4105 #if defined(USE_OSD)
4106 const uint16_t value = sbufReadU16(src);
4107
4108 /* Get screen index, 0 is post flight statistics, 1 and above are in flight OSD screens */
4109 const uint8_t screen = (sbufBytesRemaining(src) >= 1) ? sbufReadU8(src) : 1;
4110
4111 if (screen == 0 && addr < OSD_STAT_COUNT) {
4112 /* Set statistic item enable */
4113 osdStatSetState(addr, (value != 0));
4114 } else if (addr < OSD_ITEM_COUNT) {
4115 /* Set element positions */
4116 osdElementConfigMutable()->item_pos[addr] = value;
4117 osdAnalyzeActiveElements();
4118 } else {
4119 return MSP_RESULT_ERROR;
4120 }
4121 #else
4122 return MSP_RESULT_ERROR;
4123 #endif
4124 }
4125 }
4126 break;
4127
4128 case MSP_OSD_CHAR_WRITE:
4129 {
4130 osdCharacter_t chr;
4131 size_t osdCharacterBytes;
4132 uint16_t addr;
4133 if (dataSize >= OSD_CHAR_VISIBLE_BYTES + 2) {
4134 if (dataSize >= OSD_CHAR_BYTES + 2) {
4135 // 16 bit address, full char with metadata
4136 addr = sbufReadU16(src);
4137 osdCharacterBytes = OSD_CHAR_BYTES;
4138 } else if (dataSize >= OSD_CHAR_BYTES + 1) {
4139 // 8 bit address, full char with metadata
4140 addr = sbufReadU8(src);
4141 osdCharacterBytes = OSD_CHAR_BYTES;
4142 } else {
4143 // 16 bit character address, only visible char bytes
4144 addr = sbufReadU16(src);
4145 osdCharacterBytes = OSD_CHAR_VISIBLE_BYTES;
4146 }
4147 } else {
4148 // 8 bit character address, only visible char bytes
4149 addr = sbufReadU8(src);
4150 osdCharacterBytes = OSD_CHAR_VISIBLE_BYTES;
4151 }
4152 for (unsigned ii = 0; ii < MIN(osdCharacterBytes, sizeof(chr.data)); ii++) {
4153 chr.data[ii] = sbufReadU8(src);
4154 }
4155 displayPort_t *osdDisplayPort = osdGetDisplayPort(NULL);
4156 if (!osdDisplayPort) {
4157 return MSP_RESULT_ERROR;
4158 }
4159
4160 if (!displayWriteFontCharacter(osdDisplayPort, addr, &chr)) {
4161 return MSP_RESULT_ERROR;
4162 }
4163 }
4164 break;
4165 #endif // OSD
4166
4167 default:
4168 return mspProcessInCommand(srcDesc, cmdMSP, src);
4169 }
4170 return MSP_RESULT_ACK;
4171 }
4172
4173 /*
4174 * Returns MSP_RESULT_ACK, MSP_RESULT_ERROR or MSP_RESULT_NO_REPLY
4175 */
4176 mspResult_e mspFcProcessCommand(mspDescriptor_t srcDesc, mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
4177 {
4178 int ret = MSP_RESULT_ACK;
4179 sbuf_t *dst = &reply->buf;
4180 sbuf_t *src = &cmd->buf;
4181 const int16_t cmdMSP = cmd->cmd;
4182 // initialize reply by default
4183 reply->cmd = cmd->cmd;
4184
4185 if (mspCommonProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
4186 ret = MSP_RESULT_ACK;
4187 } else if (mspProcessOutCommand(srcDesc, cmdMSP, dst)) {
4188 ret = MSP_RESULT_ACK;
4189 } else if ((ret = mspFcProcessOutCommandWithArg(srcDesc, cmdMSP, src, dst, mspPostProcessFn)) != MSP_RESULT_CMD_UNKNOWN) {
4190 /* ret */;
4191 } else if (cmdMSP == MSP_SET_PASSTHROUGH) {
4192 mspFcSetPassthroughCommand(dst, src, mspPostProcessFn);
4193 ret = MSP_RESULT_ACK;
4194 #ifdef USE_FLASHFS
4195 } else if (cmdMSP == MSP_DATAFLASH_READ) {
4196 mspFcDataFlashReadCommand(dst, src);
4197 ret = MSP_RESULT_ACK;
4198 #endif
4199 } else {
4200 ret = mspCommonProcessInCommand(srcDesc, cmdMSP, src, mspPostProcessFn);
4201 }
4202 reply->result = ret;
4203 return ret;
4204 }
4205
4206 void mspFcProcessReply(mspPacket_t *reply)
4207 {
4208 sbuf_t *src = &reply->buf;
4209 UNUSED(src); // potentially unused depending on compile options.
4210
4211 switch (reply->cmd) {
4212 case MSP_ANALOG:
4213 {
4214 uint8_t batteryVoltage = sbufReadU8(src);
4215 uint16_t mAhDrawn = sbufReadU16(src);
4216 uint16_t rssi = sbufReadU16(src);
4217 uint16_t amperage = sbufReadU16(src);
4218
4219 UNUSED(rssi);
4220 UNUSED(batteryVoltage);
4221 UNUSED(amperage);
4222 UNUSED(mAhDrawn);
4223
4224 #ifdef USE_MSP_CURRENT_METER
4225 currentMeterMSPSet(amperage, mAhDrawn);
4226 #endif
4227 }
4228 break;
4229 }
4230 }
4231
4232 void mspInit(void)
4233 {
4234 initActiveBoxIds();
4235 }