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