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