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