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