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