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