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