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