?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Add Rates type to Msp
[betaflight.git] / src / main / msp / msp.c
blob26c6f2c5eceff3ef3d788fa63aa193b396e6b7bf
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, getMcuTypeId());
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 // added in 1.43
1229 sbufWriteU8(dst, currentControlRateProfile->rates_type);
1230
1231 break;
1232
1233 case MSP_PID:
1234 for (int i = 0; i < PID_ITEM_COUNT; i++) {
1235 sbufWriteU8(dst, currentPidProfile->pid[i].P);
1236 sbufWriteU8(dst, currentPidProfile->pid[i].I);
1237 sbufWriteU8(dst, currentPidProfile->pid[i].D);
1238 }
1239 break;
1240
1241 case MSP_PIDNAMES:
1242 for (const char *c = pidNames; *c; c++) {
1243 sbufWriteU8(dst, *c);
1244 }
1245 break;
1246
1247 case MSP_PID_CONTROLLER:
1248 sbufWriteU8(dst, PID_CONTROLLER_BETAFLIGHT);
1249 break;
1250
1251 case MSP_MODE_RANGES:
1252 for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
1253 const modeActivationCondition_t *mac = modeActivationConditions(i);
1254 const box_t *box = findBoxByBoxId(mac->modeId);
1255 sbufWriteU8(dst, box->permanentId);
1256 sbufWriteU8(dst, mac->auxChannelIndex);
1257 sbufWriteU8(dst, mac->range.startStep);
1258 sbufWriteU8(dst, mac->range.endStep);
1259 }
1260 break;
1261
1262 case MSP_MODE_RANGES_EXTRA:
1263 sbufWriteU8(dst, MAX_MODE_ACTIVATION_CONDITION_COUNT); // prepend number of EXTRAs array elements
1264
1265 for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
1266 const modeActivationCondition_t *mac = modeActivationConditions(i);
1267 const box_t *box = findBoxByBoxId(mac->modeId);
1268 const box_t *linkedBox = findBoxByBoxId(mac->linkedTo);
1269 sbufWriteU8(dst, box->permanentId); // each element is aligned with MODE_RANGES by the permanentId
1270 sbufWriteU8(dst, mac->modeLogic);
1271 sbufWriteU8(dst, linkedBox->permanentId);
1272 }
1273 break;
1274
1275 case MSP_ADJUSTMENT_RANGES:
1276 for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
1277 const adjustmentRange_t *adjRange = adjustmentRanges(i);
1278 sbufWriteU8(dst, 0); // was adjRange->adjustmentIndex
1279 sbufWriteU8(dst, adjRange->auxChannelIndex);
1280 sbufWriteU8(dst, adjRange->range.startStep);
1281 sbufWriteU8(dst, adjRange->range.endStep);
1282 sbufWriteU8(dst, adjRange->adjustmentConfig);
1283 sbufWriteU8(dst, adjRange->auxSwitchChannelIndex);
1284 }
1285 break;
1286
1287 case MSP_MOTOR_CONFIG:
1288 sbufWriteU16(dst, motorConfig()->minthrottle);
1289 sbufWriteU16(dst, motorConfig()->maxthrottle);
1290 sbufWriteU16(dst, motorConfig()->mincommand);
1291
1292 // API 1.42
1293 sbufWriteU8(dst, getMotorCount());
1294 sbufWriteU8(dst, motorConfig()->motorPoleCount);
1295 #ifdef USE_DSHOT_TELEMETRY
1296 sbufWriteU8(dst, motorConfig()->dev.useDshotTelemetry);
1297 #else
1298 sbufWriteU8(dst, 0);
1299 #endif
1300
1301 #ifdef USE_ESC_SENSOR
1302 sbufWriteU8(dst, featureIsEnabled(FEATURE_ESC_SENSOR)); // ESC sensor available
1303 #else
1304 sbufWriteU8(dst, 0);
1305 #endif
1306 break;
1307
1308 #ifdef USE_MAG
1309 case MSP_COMPASS_CONFIG:
1310 sbufWriteU16(dst, compassConfig()->mag_declination / 10);
1311 break;
1312 #endif
1313
1314 #if defined(USE_ESC_SENSOR)
1315 // Deprecated in favor of MSP_MOTOR_TELEMETY as of API version 1.42
1316 case MSP_ESC_SENSOR_DATA:
1317 if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
1318 sbufWriteU8(dst, getMotorCount());
1319 for (int i = 0; i < getMotorCount(); i++) {
1320 const escSensorData_t *escData = getEscSensorData(i);
1321 sbufWriteU8(dst, escData->temperature);
1322 sbufWriteU16(dst, escData->rpm);
1323 }
1324 } else {
1325 unsupportedCommand = true;
1326 }
1327
1328 break;
1329 #endif
1330
1331 #ifdef USE_GPS
1332 case MSP_GPS_CONFIG:
1333 sbufWriteU8(dst, gpsConfig()->provider);
1334 sbufWriteU8(dst, gpsConfig()->sbasMode);
1335 sbufWriteU8(dst, gpsConfig()->autoConfig);
1336 sbufWriteU8(dst, gpsConfig()->autoBaud);
1337 // Added in API version 1.43
1338 sbufWriteU8(dst, gpsConfig()->gps_set_home_point_once);
1339 sbufWriteU8(dst, gpsConfig()->gps_ublox_use_galileo);
1340 break;
1341
1342 case MSP_RAW_GPS:
1343 sbufWriteU8(dst, STATE(GPS_FIX));
1344 sbufWriteU8(dst, gpsSol.numSat);
1345 sbufWriteU32(dst, gpsSol.llh.lat);
1346 sbufWriteU32(dst, gpsSol.llh.lon);
1347 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.
1348 sbufWriteU16(dst, gpsSol.groundSpeed);
1349 sbufWriteU16(dst, gpsSol.groundCourse);
1350 break;
1351
1352 case MSP_COMP_GPS:
1353 sbufWriteU16(dst, GPS_distanceToHome);
1354 sbufWriteU16(dst, GPS_directionToHome);
1355 sbufWriteU8(dst, GPS_update & 1);
1356 break;
1357
1358 case MSP_GPSSVINFO:
1359 sbufWriteU8(dst, GPS_numCh);
1360 for (int i = 0; i < GPS_numCh; i++) {
1361 sbufWriteU8(dst, GPS_svinfo_chn[i]);
1362 sbufWriteU8(dst, GPS_svinfo_svid[i]);
1363 sbufWriteU8(dst, GPS_svinfo_quality[i]);
1364 sbufWriteU8(dst, GPS_svinfo_cno[i]);
1365 }
1366 break;
1367
1368 #ifdef USE_GPS_RESCUE
1369 case MSP_GPS_RESCUE:
1370 sbufWriteU16(dst, gpsRescueConfig()->angle);
1371 sbufWriteU16(dst, gpsRescueConfig()->initialAltitudeM);
1372 sbufWriteU16(dst, gpsRescueConfig()->descentDistanceM);
1373 sbufWriteU16(dst, gpsRescueConfig()->rescueGroundspeed);
1374 sbufWriteU16(dst, gpsRescueConfig()->throttleMin);
1375 sbufWriteU16(dst, gpsRescueConfig()->throttleMax);
1376 sbufWriteU16(dst, gpsRescueConfig()->throttleHover);
1377 sbufWriteU8(dst, gpsRescueConfig()->sanityChecks);
1378 sbufWriteU8(dst, gpsRescueConfig()->minSats);
1379 // Added in API version 1.43
1380 sbufWriteU16(dst, gpsRescueConfig()->ascendRate);
1381 sbufWriteU16(dst, gpsRescueConfig()->descendRate);
1382 sbufWriteU8(dst, gpsRescueConfig()->allowArmingWithoutFix);
1383 sbufWriteU8(dst, gpsRescueConfig()->altitudeMode);
1384 break;
1385
1386 case MSP_GPS_RESCUE_PIDS:
1387 sbufWriteU16(dst, gpsRescueConfig()->throttleP);
1388 sbufWriteU16(dst, gpsRescueConfig()->throttleI);
1389 sbufWriteU16(dst, gpsRescueConfig()->throttleD);
1390 sbufWriteU16(dst, gpsRescueConfig()->velP);
1391 sbufWriteU16(dst, gpsRescueConfig()->velI);
1392 sbufWriteU16(dst, gpsRescueConfig()->velD);
1393 sbufWriteU16(dst, gpsRescueConfig()->yawP);
1394 break;
1395 #endif
1396 #endif
1397
1398 #if defined(USE_ACC)
1399 case MSP_ACC_TRIM:
1400 sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.pitch);
1401 sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.roll);
1402
1403 break;
1404 #endif
1405 case MSP_MIXER_CONFIG:
1406 sbufWriteU8(dst, mixerConfig()->mixerMode);
1407 sbufWriteU8(dst, mixerConfig()->yaw_motors_reversed);
1408 break;
1409
1410 case MSP_RX_CONFIG:
1411 sbufWriteU8(dst, rxConfig()->serialrx_provider);
1412 sbufWriteU16(dst, rxConfig()->maxcheck);
1413 sbufWriteU16(dst, rxConfig()->midrc);
1414 sbufWriteU16(dst, rxConfig()->mincheck);
1415 sbufWriteU8(dst, rxConfig()->spektrum_sat_bind);
1416 sbufWriteU16(dst, rxConfig()->rx_min_usec);
1417 sbufWriteU16(dst, rxConfig()->rx_max_usec);
1418 sbufWriteU8(dst, rxConfig()->rcInterpolation);
1419 sbufWriteU8(dst, rxConfig()->rcInterpolationInterval);
1420 sbufWriteU16(dst, rxConfig()->airModeActivateThreshold * 10 + 1000);
1421 #ifdef USE_RX_SPI
1422 sbufWriteU8(dst, rxSpiConfig()->rx_spi_protocol);
1423 sbufWriteU32(dst, rxSpiConfig()->rx_spi_id);
1424 sbufWriteU8(dst, rxSpiConfig()->rx_spi_rf_channel_count);
1425 #else
1426 sbufWriteU8(dst, 0);
1427 sbufWriteU32(dst, 0);
1428 sbufWriteU8(dst, 0);
1429 #endif
1430 sbufWriteU8(dst, rxConfig()->fpvCamAngleDegrees);
1431 sbufWriteU8(dst, rxConfig()->rcInterpolationChannels);
1432 #if defined(USE_RC_SMOOTHING_FILTER)
1433 sbufWriteU8(dst, rxConfig()->rc_smoothing_type);
1434 sbufWriteU8(dst, rxConfig()->rc_smoothing_input_cutoff);
1435 sbufWriteU8(dst, rxConfig()->rc_smoothing_derivative_cutoff);
1436 sbufWriteU8(dst, rxConfig()->rc_smoothing_input_type);
1437 sbufWriteU8(dst, rxConfig()->rc_smoothing_derivative_type);
1438 #else
1439 sbufWriteU8(dst, 0);
1440 sbufWriteU8(dst, 0);
1441 sbufWriteU8(dst, 0);
1442 sbufWriteU8(dst, 0);
1443 sbufWriteU8(dst, 0);
1444 #endif
1445 #if defined(USE_USB_CDC_HID)
1446 sbufWriteU8(dst, usbDevConfig()->type);
1447 #else
1448 sbufWriteU8(dst, 0);
1449 #endif
1450 // Added in MSP API 1.42
1451 #if defined(USE_RC_SMOOTHING_FILTER)
1452 sbufWriteU8(dst, rxConfig()->rc_smoothing_auto_factor);
1453 #else
1454 sbufWriteU8(dst, 0);
1455 #endif
1456 break;
1457 case MSP_FAILSAFE_CONFIG:
1458 sbufWriteU8(dst, failsafeConfig()->failsafe_delay);
1459 sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay);
1460 sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
1461 sbufWriteU8(dst, failsafeConfig()->failsafe_switch_mode);
1462 sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay);
1463 sbufWriteU8(dst, failsafeConfig()->failsafe_procedure);
1464 break;
1465
1466 case MSP_RXFAIL_CONFIG:
1467 for (int i = 0; i < rxRuntimeState.channelCount; i++) {
1468 sbufWriteU8(dst, rxFailsafeChannelConfigs(i)->mode);
1469 sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxFailsafeChannelConfigs(i)->step));
1470 }
1471 break;
1472
1473 case MSP_RSSI_CONFIG:
1474 sbufWriteU8(dst, rxConfig()->rssi_channel);
1475 break;
1476
1477 case MSP_RX_MAP:
1478 sbufWriteData(dst, rxConfig()->rcmap, RX_MAPPABLE_CHANNEL_COUNT);
1479 break;
1480
1481 case MSP_CF_SERIAL_CONFIG:
1482 for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
1483 if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
1484 continue;
1485 };
1486 sbufWriteU8(dst, serialConfig()->portConfigs[i].identifier);
1487 sbufWriteU16(dst, serialConfig()->portConfigs[i].functionMask);
1488 sbufWriteU8(dst, serialConfig()->portConfigs[i].msp_baudrateIndex);
1489 sbufWriteU8(dst, serialConfig()->portConfigs[i].gps_baudrateIndex);
1490 sbufWriteU8(dst, serialConfig()->portConfigs[i].telemetry_baudrateIndex);
1491 sbufWriteU8(dst, serialConfig()->portConfigs[i].blackbox_baudrateIndex);
1492 }
1493 break;
1494 case MSP2_COMMON_SERIAL_CONFIG: {
1495 uint8_t count = 0;
1496 for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
1497 if (serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
1498 count++;
1499 }
1500 }
1501 sbufWriteU8(dst, count);
1502 for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
1503 if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
1504 continue;
1505 };
1506 sbufWriteU8(dst, serialConfig()->portConfigs[i].identifier);
1507 sbufWriteU32(dst, serialConfig()->portConfigs[i].functionMask);
1508 sbufWriteU8(dst, serialConfig()->portConfigs[i].msp_baudrateIndex);
1509 sbufWriteU8(dst, serialConfig()->portConfigs[i].gps_baudrateIndex);
1510 sbufWriteU8(dst, serialConfig()->portConfigs[i].telemetry_baudrateIndex);
1511 sbufWriteU8(dst, serialConfig()->portConfigs[i].blackbox_baudrateIndex);
1512 }
1513 break;
1514 }
1515
1516 #ifdef USE_LED_STRIP_STATUS_MODE
1517 case MSP_LED_COLORS:
1518 for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
1519 const hsvColor_t *color = &ledStripStatusModeConfig()->colors[i];
1520 sbufWriteU16(dst, color->h);
1521 sbufWriteU8(dst, color->s);
1522 sbufWriteU8(dst, color->v);
1523 }
1524 break;
1525 #endif
1526
1527 #ifdef USE_LED_STRIP
1528 case MSP_LED_STRIP_CONFIG:
1529 for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
1530 #ifdef USE_LED_STRIP_STATUS_MODE
1531 const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[i];
1532 sbufWriteU32(dst, *ledConfig);
1533 #else
1534 sbufWriteU32(dst, 0);
1535 #endif
1536 }
1537
1538 // API 1.41 - add indicator for advanced profile support and the current profile selection
1539 // 0 = basic ledstrip available
1540 // 1 = advanced ledstrip available
1541 #ifdef USE_LED_STRIP_STATUS_MODE
1542 sbufWriteU8(dst, 1); // advanced ledstrip available
1543 #else
1544 sbufWriteU8(dst, 0); // only simple ledstrip available
1545 #endif
1546 sbufWriteU8(dst, ledStripConfig()->ledstrip_profile);
1547 break;
1548 #endif
1549
1550 #ifdef USE_LED_STRIP_STATUS_MODE
1551 case MSP_LED_STRIP_MODECOLOR:
1552 for (int i = 0; i < LED_MODE_COUNT; i++) {
1553 for (int j = 0; j < LED_DIRECTION_COUNT; j++) {
1554 sbufWriteU8(dst, i);
1555 sbufWriteU8(dst, j);
1556 sbufWriteU8(dst, ledStripStatusModeConfig()->modeColors[i].color[j]);
1557 }
1558 }
1559
1560 for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) {
1561 sbufWriteU8(dst, LED_MODE_COUNT);
1562 sbufWriteU8(dst, j);
1563 sbufWriteU8(dst, ledStripStatusModeConfig()->specialColors.color[j]);
1564 }
1565
1566 sbufWriteU8(dst, LED_AUX_CHANNEL);
1567 sbufWriteU8(dst, 0);
1568 sbufWriteU8(dst, ledStripStatusModeConfig()->ledstrip_aux_channel);
1569 break;
1570 #endif
1571
1572 case MSP_DATAFLASH_SUMMARY:
1573 serializeDataflashSummaryReply(dst);
1574 break;
1575
1576 case MSP_BLACKBOX_CONFIG:
1577 #ifdef USE_BLACKBOX
1578 sbufWriteU8(dst, 1); //Blackbox supported
1579 sbufWriteU8(dst, blackboxConfig()->device);
1580 sbufWriteU8(dst, 1); // Rate numerator, not used anymore
1581 sbufWriteU8(dst, blackboxGetRateDenom());
1582 sbufWriteU16(dst, blackboxConfig()->p_ratio);
1583 #else
1584 sbufWriteU8(dst, 0); // Blackbox not supported
1585 sbufWriteU8(dst, 0);
1586 sbufWriteU8(dst, 0);
1587 sbufWriteU8(dst, 0);
1588 sbufWriteU16(dst, 0);
1589 #endif
1590 break;
1591
1592 case MSP_SDCARD_SUMMARY:
1593 serializeSDCardSummaryReply(dst);
1594 break;
1595
1596 case MSP_MOTOR_3D_CONFIG:
1597 sbufWriteU16(dst, flight3DConfig()->deadband3d_low);
1598 sbufWriteU16(dst, flight3DConfig()->deadband3d_high);
1599 sbufWriteU16(dst, flight3DConfig()->neutral3d);
1600 break;
1601
1602 case MSP_RC_DEADBAND:
1603 sbufWriteU8(dst, rcControlsConfig()->deadband);
1604 sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
1605 sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband);
1606 sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle);
1607 break;
1608
1609
1610 case MSP_SENSOR_ALIGNMENT: {
1611 uint8_t gyroAlignment;
1612 #ifdef USE_MULTI_GYRO
1613 switch (gyroConfig()->gyro_to_use) {
1614 case GYRO_CONFIG_USE_GYRO_2:
1615 gyroAlignment = gyroDeviceConfig(1)->alignment;
1616 break;
1617 case GYRO_CONFIG_USE_GYRO_BOTH:
1618 // for dual-gyro in "BOTH" mode we only read/write gyro 0
1619 default:
1620 gyroAlignment = gyroDeviceConfig(0)->alignment;
1621 break;
1622 }
1623 #else
1624 gyroAlignment = gyroDeviceConfig(0)->alignment;
1625 #endif
1626 sbufWriteU8(dst, gyroAlignment);
1627 sbufWriteU8(dst, gyroAlignment); // Starting with 4.0 gyro and acc alignment are the same
1628 #if defined(USE_MAG)
1629 sbufWriteU8(dst, compassConfig()->mag_alignment);
1630 #else
1631 sbufWriteU8(dst, 0);
1632 #endif
1633
1634 // API 1.41 - Add multi-gyro indicator, selected gyro, and support for separate gyro 1 & 2 alignment
1635 sbufWriteU8(dst, getGyroDetectionFlags());
1636 #ifdef USE_MULTI_GYRO
1637 sbufWriteU8(dst, gyroConfig()->gyro_to_use);
1638 sbufWriteU8(dst, gyroDeviceConfig(0)->alignment);
1639 sbufWriteU8(dst, gyroDeviceConfig(1)->alignment);
1640 #else
1641 sbufWriteU8(dst, GYRO_CONFIG_USE_GYRO_1);
1642 sbufWriteU8(dst, gyroDeviceConfig(0)->alignment);
1643 sbufWriteU8(dst, ALIGN_DEFAULT);
1644 #endif
1645
1646 break;
1647 }
1648 case MSP_ADVANCED_CONFIG:
1649 sbufWriteU8(dst, 1); // was gyroConfig()->gyro_sync_denom - removed in API 1.43
1650 sbufWriteU8(dst, pidConfig()->pid_process_denom);
1651 sbufWriteU8(dst, motorConfig()->dev.useUnsyncedPwm);
1652 sbufWriteU8(dst, motorConfig()->dev.motorPwmProtocol);
1653 sbufWriteU16(dst, motorConfig()->dev.motorPwmRate);
1654 sbufWriteU16(dst, motorConfig()->digitalIdleOffsetValue);
1655 sbufWriteU8(dst, 0); // DEPRECATED: gyro_use_32kHz
1656 sbufWriteU8(dst, motorConfig()->dev.motorPwmInversion);
1657 sbufWriteU8(dst, gyroConfig()->gyro_to_use);
1658 sbufWriteU8(dst, gyroConfig()->gyro_high_fsr);
1659 sbufWriteU8(dst, gyroConfig()->gyroMovementCalibrationThreshold);
1660 sbufWriteU16(dst, gyroConfig()->gyroCalibrationDuration);
1661 sbufWriteU16(dst, gyroConfig()->gyro_offset_yaw);
1662 sbufWriteU8(dst, gyroConfig()->checkOverflow);
1663 //Added in MSP API 1.42
1664 sbufWriteU8(dst, systemConfig()->debug_mode);
1665 sbufWriteU8(dst, DEBUG_COUNT);
1666
1667 break;
1668 case MSP_FILTER_CONFIG :
1669 sbufWriteU8(dst, gyroConfig()->gyro_lowpass_hz);
1670 sbufWriteU16(dst, currentPidProfile->dterm_lowpass_hz);
1671 sbufWriteU16(dst, currentPidProfile->yaw_lowpass_hz);
1672 sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1);
1673 sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1);
1674 sbufWriteU16(dst, currentPidProfile->dterm_notch_hz);
1675 sbufWriteU16(dst, currentPidProfile->dterm_notch_cutoff);
1676 sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2);
1677 sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2);
1678 sbufWriteU8(dst, currentPidProfile->dterm_filter_type);
1679 sbufWriteU8(dst, gyroConfig()->gyro_hardware_lpf);
1680 sbufWriteU8(dst, 0); // DEPRECATED: gyro_32khz_hardware_lpf
1681 sbufWriteU16(dst, gyroConfig()->gyro_lowpass_hz);
1682 sbufWriteU16(dst, gyroConfig()->gyro_lowpass2_hz);
1683 sbufWriteU8(dst, gyroConfig()->gyro_lowpass_type);
1684 sbufWriteU8(dst, gyroConfig()->gyro_lowpass2_type);
1685 sbufWriteU16(dst, currentPidProfile->dterm_lowpass2_hz);
1686 // Added in MSP API 1.41
1687 sbufWriteU8(dst, currentPidProfile->dterm_filter2_type);
1688 #if defined(USE_DYN_LPF)
1689 sbufWriteU16(dst, gyroConfig()->dyn_lpf_gyro_min_hz);
1690 sbufWriteU16(dst, gyroConfig()->dyn_lpf_gyro_max_hz);
1691 sbufWriteU16(dst, currentPidProfile->dyn_lpf_dterm_min_hz);
1692 sbufWriteU16(dst, currentPidProfile->dyn_lpf_dterm_max_hz);
1693 #else
1694 sbufWriteU16(dst, 0);
1695 sbufWriteU16(dst, 0);
1696 sbufWriteU16(dst, 0);
1697 sbufWriteU16(dst, 0);
1698 #endif
1699 // Added in MSP API 1.42
1700 #if defined(USE_GYRO_DATA_ANALYSE)
1701 sbufWriteU8(dst, 0); // DEPRECATED 1.43: dyn_notch_range
1702 sbufWriteU8(dst, gyroConfig()->dyn_notch_width_percent);
1703 sbufWriteU16(dst, gyroConfig()->dyn_notch_q);
1704 sbufWriteU16(dst, gyroConfig()->dyn_notch_min_hz);
1705 #else
1706 sbufWriteU8(dst, 0);
1707 sbufWriteU8(dst, 0);
1708 sbufWriteU16(dst, 0);
1709 sbufWriteU16(dst, 0);
1710 #endif
1711 #if defined(USE_RPM_FILTER)
1712 sbufWriteU8(dst, rpmFilterConfig()->gyro_rpm_notch_harmonics);
1713 sbufWriteU8(dst, rpmFilterConfig()->gyro_rpm_notch_min);
1714 #else
1715 sbufWriteU8(dst, 0);
1716 sbufWriteU8(dst, 0);
1717 #endif
1718 #if defined(USE_GYRO_DATA_ANALYSE)
1719 // Added in MSP API 1.43
1720 sbufWriteU16(dst, gyroConfig()->dyn_notch_max_hz);
1721 #else
1722 sbufWriteU16(dst, 0);
1723 #endif
1724
1725 break;
1726 case MSP_PID_ADVANCED:
1727 sbufWriteU16(dst, 0);
1728 sbufWriteU16(dst, 0);
1729 sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit
1730 sbufWriteU8(dst, 0); // reserved
1731 sbufWriteU8(dst, currentPidProfile->vbatPidCompensation);
1732 sbufWriteU8(dst, currentPidProfile->feedForwardTransition);
1733 sbufWriteU8(dst, 0); // was low byte of currentPidProfile->dtermSetpointWeight
1734 sbufWriteU8(dst, 0); // reserved
1735 sbufWriteU8(dst, 0); // reserved
1736 sbufWriteU8(dst, 0); // reserved
1737 sbufWriteU16(dst, currentPidProfile->rateAccelLimit);
1738 sbufWriteU16(dst, currentPidProfile->yawRateAccelLimit);
1739 sbufWriteU8(dst, currentPidProfile->levelAngleLimit);
1740 sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity
1741 sbufWriteU16(dst, currentPidProfile->itermThrottleThreshold);
1742 sbufWriteU16(dst, currentPidProfile->itermAcceleratorGain);
1743 sbufWriteU16(dst, 0); // was currentPidProfile->dtermSetpointWeight
1744 sbufWriteU8(dst, currentPidProfile->iterm_rotation);
1745 sbufWriteU8(dst, 0); // was currentPidProfile->smart_feedforward
1746 #if defined(USE_ITERM_RELAX)
1747 sbufWriteU8(dst, currentPidProfile->iterm_relax);
1748 sbufWriteU8(dst, currentPidProfile->iterm_relax_type);
1749 #else
1750 sbufWriteU8(dst, 0);
1751 sbufWriteU8(dst, 0);
1752 #endif
1753 #if defined(USE_ABSOLUTE_CONTROL)
1754 sbufWriteU8(dst, currentPidProfile->abs_control_gain);
1755 #else
1756 sbufWriteU8(dst, 0);
1757 #endif
1758 #if defined(USE_THROTTLE_BOOST)
1759 sbufWriteU8(dst, currentPidProfile->throttle_boost);
1760 #else
1761 sbufWriteU8(dst, 0);
1762 #endif
1763 #if defined(USE_ACRO_TRAINER)
1764 sbufWriteU8(dst, currentPidProfile->acro_trainer_angle_limit);
1765 #else
1766 sbufWriteU8(dst, 0);
1767 #endif
1768 sbufWriteU16(dst, currentPidProfile->pid[PID_ROLL].F);
1769 sbufWriteU16(dst, currentPidProfile->pid[PID_PITCH].F);
1770 sbufWriteU16(dst, currentPidProfile->pid[PID_YAW].F);
1771
1772 sbufWriteU8(dst, currentPidProfile->antiGravityMode);
1773 #if defined(USE_D_MIN)
1774 sbufWriteU8(dst, currentPidProfile->d_min[PID_ROLL]);
1775 sbufWriteU8(dst, currentPidProfile->d_min[PID_PITCH]);
1776 sbufWriteU8(dst, currentPidProfile->d_min[PID_YAW]);
1777 sbufWriteU8(dst, currentPidProfile->d_min_gain);
1778 sbufWriteU8(dst, currentPidProfile->d_min_advance);
1779 #else
1780 sbufWriteU8(dst, 0);
1781 sbufWriteU8(dst, 0);
1782 sbufWriteU8(dst, 0);
1783 sbufWriteU8(dst, 0);
1784 sbufWriteU8(dst, 0);
1785 #endif
1786 #if defined(USE_INTEGRATED_YAW_CONTROL)
1787 sbufWriteU8(dst, currentPidProfile->use_integrated_yaw);
1788 sbufWriteU8(dst, currentPidProfile->integrated_yaw_relax);
1789 #else
1790 sbufWriteU8(dst, 0);
1791 sbufWriteU8(dst, 0);
1792 #endif
1793 #if defined(USE_ITERM_RELAX)
1794 // Added in MSP API 1.42
1795 sbufWriteU8(dst, currentPidProfile->iterm_relax_cutoff);
1796 #else
1797 sbufWriteU8(dst, 0);
1798 #endif
1799 // Added in MSP API 1.43
1800 sbufWriteU8(dst, currentPidProfile->motor_output_limit);
1801 sbufWriteU8(dst, currentPidProfile->auto_profile_cell_count);
1802
1803 break;
1804 case MSP_SENSOR_CONFIG:
1805 #if defined(USE_ACC)
1806 sbufWriteU8(dst, accelerometerConfig()->acc_hardware);
1807 #else
1808 sbufWriteU8(dst, 0);
1809 #endif
1810 #ifdef USE_BARO
1811 sbufWriteU8(dst, barometerConfig()->baro_hardware);
1812 #else
1813 sbufWriteU8(dst, BARO_NONE);
1814 #endif
1815 #ifdef USE_MAG
1816 sbufWriteU8(dst, compassConfig()->mag_hardware);
1817 #else
1818 sbufWriteU8(dst, MAG_NONE);
1819 #endif
1820 break;
1821
1822 #if defined(USE_VTX_COMMON)
1823 case MSP_VTX_CONFIG:
1824 {
1825 const vtxDevice_t *vtxDevice = vtxCommonDevice();
1826 unsigned vtxStatus = 0;
1827 vtxDevType_e vtxType = VTXDEV_UNKNOWN;
1828 uint8_t deviceIsReady = 0;
1829 if (vtxDevice) {
1830 vtxCommonGetStatus(vtxDevice, &vtxStatus);
1831 vtxType = vtxCommonGetDeviceType(vtxDevice);
1832 deviceIsReady = vtxCommonDeviceIsReady(vtxDevice) ? 1 : 0;
1833 }
1834 sbufWriteU8(dst, vtxType);
1835 sbufWriteU8(dst, vtxSettingsConfig()->band);
1836 sbufWriteU8(dst, vtxSettingsConfig()->channel);
1837 sbufWriteU8(dst, vtxSettingsConfig()->power);
1838 sbufWriteU8(dst, (vtxStatus & VTX_STATUS_PIT_MODE) ? 1 : 0);
1839 sbufWriteU16(dst, vtxSettingsConfig()->freq);
1840 sbufWriteU8(dst, deviceIsReady);
1841 sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm);
1842
1843 // API version 1.42
1844 sbufWriteU16(dst, vtxSettingsConfig()->pitModeFreq);
1845 #ifdef USE_VTX_TABLE
1846 sbufWriteU8(dst, 1); // vtxtable is available
1847 sbufWriteU8(dst, vtxTableConfig()->bands);
1848 sbufWriteU8(dst, vtxTableConfig()->channels);
1849 sbufWriteU8(dst, vtxTableConfig()->powerLevels);
1850 #else
1851 sbufWriteU8(dst, 0);
1852 sbufWriteU8(dst, 0);
1853 sbufWriteU8(dst, 0);
1854 sbufWriteU8(dst, 0);
1855 #endif
1856
1857 }
1858 break;
1859 #endif
1860
1861 case MSP_TX_INFO:
1862 sbufWriteU8(dst, rssiSource);
1863 uint8_t rtcDateTimeIsSet = 0;
1864 #ifdef USE_RTC_TIME
1865 dateTime_t dt;
1866 if (rtcGetDateTime(&dt)) {
1867 rtcDateTimeIsSet = 1;
1868 }
1869 #else
1870 rtcDateTimeIsSet = RTC_NOT_SUPPORTED;
1871 #endif
1872 sbufWriteU8(dst, rtcDateTimeIsSet);
1873
1874 break;
1875 #ifdef USE_RTC_TIME
1876 case MSP_RTC:
1877 {
1878 dateTime_t dt;
1879 if (rtcGetDateTime(&dt)) {
1880 sbufWriteU16(dst, dt.year);
1881 sbufWriteU8(dst, dt.month);
1882 sbufWriteU8(dst, dt.day);
1883 sbufWriteU8(dst, dt.hours);
1884 sbufWriteU8(dst, dt.minutes);
1885 sbufWriteU8(dst, dt.seconds);
1886 sbufWriteU16(dst, dt.millis);
1887 }
1888 }
1889
1890 break;
1891 #endif
1892 default:
1893 unsupportedCommand = true;
1894 }
1895 return !unsupportedCommand;
1896 }
1897
1898 static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
1899 {
1900
1901 switch (cmdMSP) {
1902 case MSP_BOXNAMES:
1903 {
1904 const int page = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
1905 serializeBoxReply(dst, page, &serializeBoxNameFn);
1906 }
1907 break;
1908 case MSP_BOXIDS:
1909 {
1910 const int page = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
1911 serializeBoxReply(dst, page, &serializeBoxPermanentIdFn);
1912 }
1913 break;
1914 case MSP_REBOOT:
1915 if (sbufBytesRemaining(src)) {
1916 rebootMode = sbufReadU8(src);
1917
1918 if (rebootMode >= MSP_REBOOT_COUNT
1919 #if !defined(USE_USB_MSC)
1920 || rebootMode == MSP_REBOOT_MSC || rebootMode == MSP_REBOOT_MSC_UTC
1921 #endif
1922 ) {
1923 return MSP_RESULT_ERROR;
1924 }
1925 } else {
1926 rebootMode = MSP_REBOOT_FIRMWARE;
1927 }
1928
1929 sbufWriteU8(dst, rebootMode);
1930
1931 #if defined(USE_USB_MSC)
1932 if (rebootMode == MSP_REBOOT_MSC) {
1933 if (mscCheckFilesystemReady()) {
1934 sbufWriteU8(dst, 1);
1935 } else {
1936 sbufWriteU8(dst, 0);
1937
1938 return MSP_RESULT_ACK;
1939 }
1940 }
1941 #endif
1942
1943 if (mspPostProcessFn) {
1944 *mspPostProcessFn = mspRebootFn;
1945 }
1946
1947 break;
1948 case MSP_MULTIPLE_MSP:
1949 {
1950 uint8_t maxMSPs = 0;
1951 if (sbufBytesRemaining(src) == 0) {
1952 return MSP_RESULT_ERROR;
1953 }
1954 int bytesRemaining = sbufBytesRemaining(dst) - 1; // need to keep one byte for checksum
1955 mspPacket_t packetIn, packetOut;
1956 sbufInit(&packetIn.buf, src->end, src->end);
1957 uint8_t* resetInputPtr = src->ptr;
1958 while (sbufBytesRemaining(src) && bytesRemaining > 0) {
1959 uint8_t newMSP = sbufReadU8(src);
1960 sbufInit(&packetOut.buf, dst->ptr, dst->end);
1961 packetIn.cmd = newMSP;
1962 mspFcProcessCommand(srcDesc, &packetIn, &packetOut, NULL);
1963 uint8_t mspSize = sbufPtr(&packetOut.buf) - dst->ptr;
1964 mspSize++; // need to add length information for each MSP
1965 bytesRemaining -= mspSize;
1966 if (bytesRemaining >= 0) {
1967 maxMSPs++;
1968 }
1969 }
1970 src->ptr = resetInputPtr;
1971 sbufInit(&packetOut.buf, dst->ptr, dst->end);
1972 for (int i = 0; i < maxMSPs; i++) {
1973 uint8_t* sizePtr = sbufPtr(&packetOut.buf);
1974 sbufWriteU8(&packetOut.buf, 0); // dummy
1975 packetIn.cmd = sbufReadU8(src);
1976 mspFcProcessCommand(srcDesc, &packetIn, &packetOut, NULL);
1977 (*sizePtr) = sbufPtr(&packetOut.buf) - (sizePtr + 1);
1978 }
1979 dst->ptr = packetOut.buf.ptr;
1980 }
1981 break;
1982
1983 #ifdef USE_VTX_TABLE
1984 case MSP_VTXTABLE_BAND:
1985 {
1986 const uint8_t band = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
1987 if (band > 0 && band <= VTX_TABLE_MAX_BANDS) {
1988 sbufWriteU8(dst, band); // band number (same as request)
1989 sbufWriteU8(dst, VTX_TABLE_BAND_NAME_LENGTH); // band name length
1990 for (int i = 0; i < VTX_TABLE_BAND_NAME_LENGTH; i++) { // band name bytes
1991 sbufWriteU8(dst, vtxTableConfig()->bandNames[band - 1][i]);
1992 }
1993 sbufWriteU8(dst, vtxTableConfig()->bandLetters[band - 1]); // band letter
1994 sbufWriteU8(dst, vtxTableConfig()->isFactoryBand[band - 1]); // CUSTOM = 0; FACTORY = 1
1995 sbufWriteU8(dst, vtxTableConfig()->channels); // number of channel frequencies to follow
1996 for (int i = 0; i < vtxTableConfig()->channels; i++) { // the frequency for each channel
1997 sbufWriteU16(dst, vtxTableConfig()->frequency[band - 1][i]);
1998 }
1999 } else {
2000 return MSP_RESULT_ERROR;
2001 }
2002 }
2003 break;
2004
2005 case MSP_VTXTABLE_POWERLEVEL:
2006 {
2007 const uint8_t powerLevel = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
2008 if (powerLevel > 0 && powerLevel <= VTX_TABLE_MAX_POWER_LEVELS) {
2009 sbufWriteU8(dst, powerLevel); // powerLevel number (same as request)
2010 sbufWriteU16(dst, vtxTableConfig()->powerValues[powerLevel - 1]);
2011 sbufWriteU8(dst, VTX_TABLE_POWER_LABEL_LENGTH); // powerLevel label length
2012 for (int i = 0; i < VTX_TABLE_POWER_LABEL_LENGTH; i++) { // powerlevel label bytes
2013 sbufWriteU8(dst, vtxTableConfig()->powerLabels[powerLevel - 1][i]);
2014 }
2015 } else {
2016 return MSP_RESULT_ERROR;
2017 }
2018 }
2019 break;
2020 #endif // USE_VTX_TABLE
2021
2022 case MSP_RESET_CONF:
2023 {
2024 #if defined(USE_CUSTOM_DEFAULTS)
2025 defaultsType_e defaultsType = DEFAULTS_TYPE_CUSTOM;
2026 #endif
2027 if (sbufBytesRemaining(src) >= 1) {
2028 // Added in MSP API 1.42
2029 #if defined(USE_CUSTOM_DEFAULTS)
2030 defaultsType = sbufReadU8(src);
2031 #else
2032 sbufReadU8(src);
2033 #endif
2034 }
2035
2036 bool success = false;
2037 if (!ARMING_FLAG(ARMED)) {
2038 #if defined(USE_CUSTOM_DEFAULTS)
2039 success = resetEEPROM(defaultsType == DEFAULTS_TYPE_CUSTOM);
2040 #else
2041 success = resetEEPROM(false);
2042 #endif
2043
2044 if (success && mspPostProcessFn) {
2045 rebootMode = MSP_REBOOT_FIRMWARE;
2046 *mspPostProcessFn = mspRebootFn;
2047 }
2048 }
2049
2050 // Added in API version 1.42
2051 sbufWriteU8(dst, success);
2052 }
2053
2054 break;
2055 default:
2056 return MSP_RESULT_CMD_UNKNOWN;
2057 }
2058 return MSP_RESULT_ACK;
2059 }
2060
2061 #ifdef USE_FLASHFS
2062 static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
2063 {
2064 const unsigned int dataSize = sbufBytesRemaining(src);
2065 const uint32_t readAddress = sbufReadU32(src);
2066 uint16_t readLength;
2067 bool allowCompression = false;
2068 bool useLegacyFormat;
2069 if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
2070 readLength = sbufReadU16(src);
2071 if (sbufBytesRemaining(src)) {
2072 allowCompression = sbufReadU8(src);
2073 }
2074 useLegacyFormat = false;
2075 } else {
2076 readLength = 128;
2077 useLegacyFormat = true;
2078 }
2079
2080 serializeDataflashReadReply(dst, readAddress, readLength, useLegacyFormat, allowCompression);
2081 }
2082 #endif
2083
2084 static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src)
2085 {
2086 uint32_t i;
2087 uint8_t value;
2088 const unsigned int dataSize = sbufBytesRemaining(src);
2089 switch (cmdMSP) {
2090 case MSP_SELECT_SETTING:
2091 value = sbufReadU8(src);
2092 if ((value & RATEPROFILE_MASK) == 0) {
2093 if (!ARMING_FLAG(ARMED)) {
2094 if (value >= PID_PROFILE_COUNT) {
2095 value = 0;
2096 }
2097 changePidProfile(value);
2098 }
2099 } else {
2100 value = value & ~RATEPROFILE_MASK;
2101
2102 if (value >= CONTROL_RATE_PROFILE_COUNT) {
2103 value = 0;
2104 }
2105 changeControlRateProfile(value);
2106 }
2107 break;
2108
2109 case MSP_COPY_PROFILE:
2110 value = sbufReadU8(src); // 0 = pid profile, 1 = control rate profile
2111 uint8_t dstProfileIndex = sbufReadU8(src);
2112 uint8_t srcProfileIndex = sbufReadU8(src);
2113 if (value == 0) {
2114 pidCopyProfile(dstProfileIndex, srcProfileIndex);
2115 }
2116 else if (value == 1) {
2117 copyControlRateProfile(dstProfileIndex, srcProfileIndex);
2118 }
2119 break;
2120
2121 #if defined(USE_GPS) || defined(USE_MAG)
2122 case MSP_SET_HEADING:
2123 magHold = sbufReadU16(src);
2124 break;
2125 #endif
2126
2127 case MSP_SET_RAW_RC:
2128 #ifdef USE_RX_MSP
2129 {
2130 uint8_t channelCount = dataSize / sizeof(uint16_t);
2131 if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) {
2132 return MSP_RESULT_ERROR;
2133 } else {
2134 uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
2135 for (int i = 0; i < channelCount; i++) {
2136 frame[i] = sbufReadU16(src);
2137 }
2138 rxMspFrameReceive(frame, channelCount);
2139 }
2140 }
2141 #endif
2142 break;
2143 #if defined(USE_ACC)
2144 case MSP_SET_ACC_TRIM:
2145 accelerometerConfigMutable()->accelerometerTrims.values.pitch = sbufReadU16(src);
2146 accelerometerConfigMutable()->accelerometerTrims.values.roll = sbufReadU16(src);
2147
2148 break;
2149 #endif
2150 case MSP_SET_ARMING_CONFIG:
2151 armingConfigMutable()->auto_disarm_delay = sbufReadU8(src);
2152 sbufReadU8(src); // reserved
2153 if (sbufBytesRemaining(src)) {
2154 imuConfigMutable()->small_angle = sbufReadU8(src);
2155 }
2156 break;
2157
2158 case MSP_SET_PID_CONTROLLER:
2159 break;
2160
2161 case MSP_SET_PID:
2162 for (int i = 0; i < PID_ITEM_COUNT; i++) {
2163 currentPidProfile->pid[i].P = sbufReadU8(src);
2164 currentPidProfile->pid[i].I = sbufReadU8(src);
2165 currentPidProfile->pid[i].D = sbufReadU8(src);
2166 }
2167 pidInitConfig(currentPidProfile);
2168 break;
2169
2170 case MSP_SET_MODE_RANGE:
2171 i = sbufReadU8(src);
2172 if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
2173 modeActivationCondition_t *mac = modeActivationConditionsMutable(i);
2174 i = sbufReadU8(src);
2175 const box_t *box = findBoxByPermanentId(i);
2176 if (box) {
2177 mac->modeId = box->boxId;
2178 mac->auxChannelIndex = sbufReadU8(src);
2179 mac->range.startStep = sbufReadU8(src);
2180 mac->range.endStep = sbufReadU8(src);
2181 if (sbufBytesRemaining(src) != 0) {
2182 mac->modeLogic = sbufReadU8(src);
2183
2184 i = sbufReadU8(src);
2185 mac->linkedTo = findBoxByPermanentId(i)->boxId;
2186 }
2187 rcControlsInit();
2188 } else {
2189 return MSP_RESULT_ERROR;
2190 }
2191 } else {
2192 return MSP_RESULT_ERROR;
2193 }
2194 break;
2195
2196 case MSP_SET_ADJUSTMENT_RANGE:
2197 i = sbufReadU8(src);
2198 if (i < MAX_ADJUSTMENT_RANGE_COUNT) {
2199 adjustmentRange_t *adjRange = adjustmentRangesMutable(i);
2200 sbufReadU8(src); // was adjRange->adjustmentIndex
2201 adjRange->auxChannelIndex = sbufReadU8(src);
2202 adjRange->range.startStep = sbufReadU8(src);
2203 adjRange->range.endStep = sbufReadU8(src);
2204 adjRange->adjustmentConfig = sbufReadU8(src);
2205 adjRange->auxSwitchChannelIndex = sbufReadU8(src);
2206
2207 activeAdjustmentRangeReset();
2208 } else {
2209 return MSP_RESULT_ERROR;
2210 }
2211 break;
2212
2213 case MSP_SET_RC_TUNING:
2214 if (sbufBytesRemaining(src) >= 10) {
2215 value = sbufReadU8(src);
2216 if (currentControlRateProfile->rcRates[FD_PITCH] == currentControlRateProfile->rcRates[FD_ROLL]) {
2217 currentControlRateProfile->rcRates[FD_PITCH] = value;
2218 }
2219 currentControlRateProfile->rcRates[FD_ROLL] = value;
2220
2221 value = sbufReadU8(src);
2222 if (currentControlRateProfile->rcExpo[FD_PITCH] == currentControlRateProfile->rcExpo[FD_ROLL]) {
2223 currentControlRateProfile->rcExpo[FD_PITCH] = value;
2224 }
2225 currentControlRateProfile->rcExpo[FD_ROLL] = value;
2226
2227 for (int i = 0; i < 3; i++) {
2228 currentControlRateProfile->rates[i] = sbufReadU8(src);
2229 }
2230
2231 value = sbufReadU8(src);
2232 currentControlRateProfile->dynThrPID = MIN(value, CONTROL_RATE_CONFIG_TPA_MAX);
2233 currentControlRateProfile->thrMid8 = sbufReadU8(src);
2234 currentControlRateProfile->thrExpo8 = sbufReadU8(src);
2235 currentControlRateProfile->tpa_breakpoint = sbufReadU16(src);
2236
2237 if (sbufBytesRemaining(src) >= 1) {
2238 currentControlRateProfile->rcExpo[FD_YAW] = sbufReadU8(src);
2239 }
2240
2241 if (sbufBytesRemaining(src) >= 1) {
2242 currentControlRateProfile->rcRates[FD_YAW] = sbufReadU8(src);
2243 }
2244
2245 if (sbufBytesRemaining(src) >= 1) {
2246 currentControlRateProfile->rcRates[FD_PITCH] = sbufReadU8(src);
2247 }
2248
2249 if (sbufBytesRemaining(src) >= 1) {
2250 currentControlRateProfile->rcExpo[FD_PITCH] = sbufReadU8(src);
2251 }
2252
2253 // version 1.41
2254 if (sbufBytesRemaining(src) >= 2) {
2255 currentControlRateProfile->throttle_limit_type = sbufReadU8(src);
2256 currentControlRateProfile->throttle_limit_percent = sbufReadU8(src);
2257 }
2258
2259 // version 1.42
2260 if (sbufBytesRemaining(src) >= 6) {
2261 currentControlRateProfile->rate_limit[FD_ROLL] = sbufReadU16(src);
2262 currentControlRateProfile->rate_limit[FD_PITCH] = sbufReadU16(src);
2263 currentControlRateProfile->rate_limit[FD_YAW] = sbufReadU16(src);
2264 }
2265
2266 // version 1.43
2267 if (sbufBytesRemaining(src) >= 1) {
2268 currentControlRateProfile->rates_type = sbufReadU8(src);
2269 }
2270
2271 initRcProcessing();
2272 } else {
2273 return MSP_RESULT_ERROR;
2274 }
2275 break;
2276
2277 case MSP_SET_MOTOR_CONFIG:
2278 motorConfigMutable()->minthrottle = sbufReadU16(src);
2279 motorConfigMutable()->maxthrottle = sbufReadU16(src);
2280 motorConfigMutable()->mincommand = sbufReadU16(src);
2281
2282 // version 1.42
2283 if (sbufBytesRemaining(src) >= 2) {
2284 motorConfigMutable()->motorPoleCount = sbufReadU8(src);
2285 #if defined(USE_DSHOT_TELEMETRY)
2286 motorConfigMutable()->dev.useDshotTelemetry = sbufReadU8(src);
2287 #else
2288 sbufReadU8(src);
2289 #endif
2290 }
2291 break;
2292
2293 #ifdef USE_GPS
2294 case MSP_SET_GPS_CONFIG:
2295 gpsConfigMutable()->provider = sbufReadU8(src);
2296 gpsConfigMutable()->sbasMode = sbufReadU8(src);
2297 gpsConfigMutable()->autoConfig = sbufReadU8(src);
2298 gpsConfigMutable()->autoBaud = sbufReadU8(src);
2299 if (sbufBytesRemaining(src) >= 2) {
2300 // Added in API version 1.43
2301 gpsConfigMutable()->gps_set_home_point_once = sbufReadU8(src);
2302 gpsConfigMutable()->gps_ublox_use_galileo = sbufReadU8(src);
2303 }
2304 break;
2305
2306 #ifdef USE_GPS_RESCUE
2307 case MSP_SET_GPS_RESCUE:
2308 gpsRescueConfigMutable()->angle = sbufReadU16(src);
2309 gpsRescueConfigMutable()->initialAltitudeM = sbufReadU16(src);
2310 gpsRescueConfigMutable()->descentDistanceM = sbufReadU16(src);
2311 gpsRescueConfigMutable()->rescueGroundspeed = sbufReadU16(src);
2312 gpsRescueConfigMutable()->throttleMin = sbufReadU16(src);
2313 gpsRescueConfigMutable()->throttleMax = sbufReadU16(src);
2314 gpsRescueConfigMutable()->throttleHover = sbufReadU16(src);
2315 gpsRescueConfigMutable()->sanityChecks = sbufReadU8(src);
2316 gpsRescueConfigMutable()->minSats = sbufReadU8(src);
2317 if (sbufBytesRemaining(src) >= 6) {
2318 // Added in API version 1.43
2319 gpsRescueConfigMutable()->ascendRate = sbufReadU16(src);
2320 gpsRescueConfigMutable()->descendRate = sbufReadU16(src);
2321 gpsRescueConfigMutable()->allowArmingWithoutFix = sbufReadU8(src);
2322 gpsRescueConfigMutable()->altitudeMode = sbufReadU8(src);
2323 }
2324 break;
2325
2326 case MSP_SET_GPS_RESCUE_PIDS:
2327 gpsRescueConfigMutable()->throttleP = sbufReadU16(src);
2328 gpsRescueConfigMutable()->throttleI = sbufReadU16(src);
2329 gpsRescueConfigMutable()->throttleD = sbufReadU16(src);
2330 gpsRescueConfigMutable()->velP = sbufReadU16(src);
2331 gpsRescueConfigMutable()->velI = sbufReadU16(src);
2332 gpsRescueConfigMutable()->velD = sbufReadU16(src);
2333 gpsRescueConfigMutable()->yawP = sbufReadU16(src);
2334 break;
2335 #endif
2336 #endif
2337
2338 #ifdef USE_MAG
2339 case MSP_SET_COMPASS_CONFIG:
2340 compassConfigMutable()->mag_declination = sbufReadU16(src) * 10;
2341 break;
2342 #endif
2343
2344 case MSP_SET_MOTOR:
2345 for (int i = 0; i < getMotorCount(); i++) {
2346 motor_disarmed[i] = motorConvertFromExternal(sbufReadU16(src));
2347 }
2348 break;
2349
2350 case MSP_SET_SERVO_CONFIGURATION:
2351 #ifdef USE_SERVOS
2352 if (dataSize != 1 + 12) {
2353 return MSP_RESULT_ERROR;
2354 }
2355 i = sbufReadU8(src);
2356 if (i >= MAX_SUPPORTED_SERVOS) {
2357 return MSP_RESULT_ERROR;
2358 } else {
2359 servoParamsMutable(i)->min = sbufReadU16(src);
2360 servoParamsMutable(i)->max = sbufReadU16(src);
2361 servoParamsMutable(i)->middle = sbufReadU16(src);
2362 servoParamsMutable(i)->rate = sbufReadU8(src);
2363 servoParamsMutable(i)->forwardFromChannel = sbufReadU8(src);
2364 servoParamsMutable(i)->reversedSources = sbufReadU32(src);
2365 }
2366 #endif
2367 break;
2368
2369 case MSP_SET_SERVO_MIX_RULE:
2370 #ifdef USE_SERVOS
2371 i = sbufReadU8(src);
2372 if (i >= MAX_SERVO_RULES) {
2373 return MSP_RESULT_ERROR;
2374 } else {
2375 customServoMixersMutable(i)->targetChannel = sbufReadU8(src);
2376 customServoMixersMutable(i)->inputSource = sbufReadU8(src);
2377 customServoMixersMutable(i)->rate = sbufReadU8(src);
2378 customServoMixersMutable(i)->speed = sbufReadU8(src);
2379 customServoMixersMutable(i)->min = sbufReadU8(src);
2380 customServoMixersMutable(i)->max = sbufReadU8(src);
2381 customServoMixersMutable(i)->box = sbufReadU8(src);
2382 loadCustomServoMixer();
2383 }
2384 #endif
2385 break;
2386
2387 case MSP_SET_MOTOR_3D_CONFIG:
2388 flight3DConfigMutable()->deadband3d_low = sbufReadU16(src);
2389 flight3DConfigMutable()->deadband3d_high = sbufReadU16(src);
2390 flight3DConfigMutable()->neutral3d = sbufReadU16(src);
2391 break;
2392
2393 case MSP_SET_RC_DEADBAND:
2394 rcControlsConfigMutable()->deadband = sbufReadU8(src);
2395 rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
2396 rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src);
2397 flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src);
2398 break;
2399
2400 case MSP_SET_RESET_CURR_PID:
2401 resetPidProfile(currentPidProfile);
2402 break;
2403
2404 case MSP_SET_SENSOR_ALIGNMENT: {
2405 // maintain backwards compatibility for API < 1.41
2406 const uint8_t gyroAlignment = sbufReadU8(src);
2407 sbufReadU8(src); // discard deprecated acc_align
2408 #if defined(USE_MAG)
2409 compassConfigMutable()->mag_alignment = sbufReadU8(src);
2410 #else
2411 sbufReadU8(src);
2412 #endif
2413
2414 if (sbufBytesRemaining(src) >= 3) {
2415 // API >= 1.41 - support the gyro_to_use and alignment for gyros 1 & 2
2416 #ifdef USE_MULTI_GYRO
2417 gyroConfigMutable()->gyro_to_use = sbufReadU8(src);
2418 gyroDeviceConfigMutable(0)->alignment = sbufReadU8(src);
2419 gyroDeviceConfigMutable(1)->alignment = sbufReadU8(src);
2420 #else
2421 sbufReadU8(src); // unused gyro_to_use
2422 gyroDeviceConfigMutable(0)->alignment = sbufReadU8(src);
2423 sbufReadU8(src); // unused gyro_2_sensor_align
2424 #endif
2425 } else {
2426 // maintain backwards compatibility for API < 1.41
2427 #ifdef USE_MULTI_GYRO
2428 switch (gyroConfig()->gyro_to_use) {
2429 case GYRO_CONFIG_USE_GYRO_2:
2430 gyroDeviceConfigMutable(1)->alignment = gyroAlignment;
2431 break;
2432 case GYRO_CONFIG_USE_GYRO_BOTH:
2433 // For dual-gyro in "BOTH" mode we'll only update gyro 0
2434 default:
2435 gyroDeviceConfigMutable(0)->alignment = gyroAlignment;
2436 break;
2437 }
2438 #else
2439 gyroDeviceConfigMutable(0)->alignment = gyroAlignment;
2440 #endif
2441
2442 }
2443 break;
2444 }
2445
2446 case MSP_SET_ADVANCED_CONFIG:
2447 sbufReadU8(src); // was gyroConfigMutable()->gyro_sync_denom - removed in API 1.43
2448 pidConfigMutable()->pid_process_denom = sbufReadU8(src);
2449 motorConfigMutable()->dev.useUnsyncedPwm = sbufReadU8(src);
2450 #ifdef USE_DSHOT
2451 motorConfigMutable()->dev.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
2452 #else
2453 motorConfigMutable()->dev.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
2454 #endif
2455 motorConfigMutable()->dev.motorPwmRate = sbufReadU16(src);
2456 if (sbufBytesRemaining(src) >= 2) {
2457 motorConfigMutable()->digitalIdleOffsetValue = sbufReadU16(src);
2458 }
2459 if (sbufBytesRemaining(src)) {
2460 sbufReadU8(src); // DEPRECATED: gyro_use_32khz
2461 }
2462 if (sbufBytesRemaining(src)) {
2463 motorConfigMutable()->dev.motorPwmInversion = sbufReadU8(src);
2464 }
2465 if (sbufBytesRemaining(src) >= 8) {
2466 gyroConfigMutable()->gyro_to_use = sbufReadU8(src);
2467 gyroConfigMutable()->gyro_high_fsr = sbufReadU8(src);
2468 gyroConfigMutable()->gyroMovementCalibrationThreshold = sbufReadU8(src);
2469 gyroConfigMutable()->gyroCalibrationDuration = sbufReadU16(src);
2470 gyroConfigMutable()->gyro_offset_yaw = sbufReadU16(src);
2471 gyroConfigMutable()->checkOverflow = sbufReadU8(src);
2472 }
2473 if (sbufBytesRemaining(src) >= 1) {
2474 //Added in MSP API 1.42
2475 systemConfigMutable()->debug_mode = sbufReadU8(src);
2476 }
2477
2478 validateAndFixGyroConfig();
2479
2480 break;
2481 case MSP_SET_FILTER_CONFIG:
2482 gyroConfigMutable()->gyro_lowpass_hz = sbufReadU8(src);
2483 currentPidProfile->dterm_lowpass_hz = sbufReadU16(src);
2484 currentPidProfile->yaw_lowpass_hz = sbufReadU16(src);
2485 if (sbufBytesRemaining(src) >= 8) {
2486 gyroConfigMutable()->gyro_soft_notch_hz_1 = sbufReadU16(src);
2487 gyroConfigMutable()->gyro_soft_notch_cutoff_1 = sbufReadU16(src);
2488 currentPidProfile->dterm_notch_hz = sbufReadU16(src);
2489 currentPidProfile->dterm_notch_cutoff = sbufReadU16(src);
2490 }
2491 if (sbufBytesRemaining(src) >= 4) {
2492 gyroConfigMutable()->gyro_soft_notch_hz_2 = sbufReadU16(src);
2493 gyroConfigMutable()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
2494 }
2495 if (sbufBytesRemaining(src) >= 1) {
2496 currentPidProfile->dterm_filter_type = sbufReadU8(src);
2497 }
2498 if (sbufBytesRemaining(src) >= 10) {
2499 gyroConfigMutable()->gyro_hardware_lpf = sbufReadU8(src);
2500 sbufReadU8(src); // DEPRECATED: gyro_32khz_hardware_lpf
2501 gyroConfigMutable()->gyro_lowpass_hz = sbufReadU16(src);
2502 gyroConfigMutable()->gyro_lowpass2_hz = sbufReadU16(src);
2503 gyroConfigMutable()->gyro_lowpass_type = sbufReadU8(src);
2504 gyroConfigMutable()->gyro_lowpass2_type = sbufReadU8(src);
2505 currentPidProfile->dterm_lowpass2_hz = sbufReadU16(src);
2506 }
2507 if (sbufBytesRemaining(src) >= 9) {
2508 // Added in MSP API 1.41
2509 currentPidProfile->dterm_filter2_type = sbufReadU8(src);
2510 #if defined(USE_DYN_LPF)
2511 gyroConfigMutable()->dyn_lpf_gyro_min_hz = sbufReadU16(src);
2512 gyroConfigMutable()->dyn_lpf_gyro_max_hz = sbufReadU16(src);
2513 currentPidProfile->dyn_lpf_dterm_min_hz = sbufReadU16(src);
2514 currentPidProfile->dyn_lpf_dterm_max_hz = sbufReadU16(src);
2515 #else
2516 sbufReadU16(src);
2517 sbufReadU16(src);
2518 sbufReadU16(src);
2519 sbufReadU16(src);
2520 #endif
2521 }
2522 if (sbufBytesRemaining(src) >= 8) {
2523 // Added in MSP API 1.42
2524 #if defined(USE_GYRO_DATA_ANALYSE)
2525 sbufReadU8(src); // DEPRECATED: dyn_notch_range
2526 gyroConfigMutable()->dyn_notch_width_percent = sbufReadU8(src);
2527 gyroConfigMutable()->dyn_notch_q = sbufReadU16(src);
2528 gyroConfigMutable()->dyn_notch_min_hz = sbufReadU16(src);
2529 #else
2530 sbufReadU8(src);
2531 sbufReadU8(src);
2532 sbufReadU16(src);
2533 sbufReadU16(src);
2534 #endif
2535 #if defined(USE_RPM_FILTER)
2536 rpmFilterConfigMutable()->gyro_rpm_notch_harmonics = sbufReadU8(src);
2537 rpmFilterConfigMutable()->gyro_rpm_notch_min = sbufReadU8(src);
2538 #else
2539 sbufReadU8(src);
2540 sbufReadU8(src);
2541 #endif
2542 }
2543 if (sbufBytesRemaining(src) >= 1) {
2544 #if defined(USE_GYRO_DATA_ANALYSE)
2545 // Added in MSP API 1.43
2546 gyroConfigMutable()->dyn_notch_max_hz = sbufReadU16(src);
2547 #else
2548 sbufReadU16(src);
2549 #endif
2550 }
2551
2552
2553 // reinitialize the gyro filters with the new values
2554 validateAndFixGyroConfig();
2555 gyroInitFilters();
2556 // reinitialize the PID filters with the new values
2557 pidInitFilters(currentPidProfile);
2558
2559 break;
2560 case MSP_SET_PID_ADVANCED:
2561 sbufReadU16(src);
2562 sbufReadU16(src);
2563 sbufReadU16(src); // was pidProfile.yaw_p_limit
2564 sbufReadU8(src); // reserved
2565 currentPidProfile->vbatPidCompensation = sbufReadU8(src);
2566 currentPidProfile->feedForwardTransition = sbufReadU8(src);
2567 sbufReadU8(src); // was low byte of currentPidProfile->dtermSetpointWeight
2568 sbufReadU8(src); // reserved
2569 sbufReadU8(src); // reserved
2570 sbufReadU8(src); // reserved
2571 currentPidProfile->rateAccelLimit = sbufReadU16(src);
2572 currentPidProfile->yawRateAccelLimit = sbufReadU16(src);
2573 if (sbufBytesRemaining(src) >= 2) {
2574 currentPidProfile->levelAngleLimit = sbufReadU8(src);
2575 sbufReadU8(src); // was pidProfile.levelSensitivity
2576 }
2577 if (sbufBytesRemaining(src) >= 4) {
2578 currentPidProfile->itermThrottleThreshold = sbufReadU16(src);
2579 currentPidProfile->itermAcceleratorGain = sbufReadU16(src);
2580 }
2581 if (sbufBytesRemaining(src) >= 2) {
2582 sbufReadU16(src); // was currentPidProfile->dtermSetpointWeight
2583 }
2584 if (sbufBytesRemaining(src) >= 14) {
2585 // Added in MSP API 1.40
2586 currentPidProfile->iterm_rotation = sbufReadU8(src);
2587 sbufReadU8(src); // was currentPidProfile->smart_feedforward
2588 #if defined(USE_ITERM_RELAX)
2589 currentPidProfile->iterm_relax = sbufReadU8(src);
2590 currentPidProfile->iterm_relax_type = sbufReadU8(src);
2591 #else
2592 sbufReadU8(src);
2593 sbufReadU8(src);
2594 #endif
2595 #if defined(USE_ABSOLUTE_CONTROL)
2596 currentPidProfile->abs_control_gain = sbufReadU8(src);
2597 #else
2598 sbufReadU8(src);
2599 #endif
2600 #if defined(USE_THROTTLE_BOOST)
2601 currentPidProfile->throttle_boost = sbufReadU8(src);
2602 #else
2603 sbufReadU8(src);
2604 #endif
2605 #if defined(USE_ACRO_TRAINER)
2606 currentPidProfile->acro_trainer_angle_limit = sbufReadU8(src);
2607 #else
2608 sbufReadU8(src);
2609 #endif
2610 // PID controller feedforward terms
2611 currentPidProfile->pid[PID_ROLL].F = sbufReadU16(src);
2612 currentPidProfile->pid[PID_PITCH].F = sbufReadU16(src);
2613 currentPidProfile->pid[PID_YAW].F = sbufReadU16(src);
2614
2615 currentPidProfile->antiGravityMode = sbufReadU8(src);
2616 }
2617 if (sbufBytesRemaining(src) >= 7) {
2618 // Added in MSP API 1.41
2619 #if defined(USE_D_MIN)
2620 currentPidProfile->d_min[PID_ROLL] = sbufReadU8(src);
2621 currentPidProfile->d_min[PID_PITCH] = sbufReadU8(src);
2622 currentPidProfile->d_min[PID_YAW] = sbufReadU8(src);
2623 currentPidProfile->d_min_gain = sbufReadU8(src);
2624 currentPidProfile->d_min_advance = sbufReadU8(src);
2625 #else
2626 sbufReadU8(src);
2627 sbufReadU8(src);
2628 sbufReadU8(src);
2629 sbufReadU8(src);
2630 sbufReadU8(src);
2631 #endif
2632 #if defined(USE_INTEGRATED_YAW_CONTROL)
2633 currentPidProfile->use_integrated_yaw = sbufReadU8(src);
2634 currentPidProfile->integrated_yaw_relax = sbufReadU8(src);
2635 #else
2636 sbufReadU8(src);
2637 sbufReadU8(src);
2638 #endif
2639 }
2640 if(sbufBytesRemaining(src) >= 1) {
2641 // Added in MSP API 1.42
2642 #if defined(USE_ITERM_RELAX)
2643 currentPidProfile->iterm_relax_cutoff = sbufReadU8(src);
2644 #else
2645 sbufReadU8(src);
2646 #endif
2647 }
2648 if(sbufBytesRemaining(src) >= 2) {
2649 // Added in MSP API 1.43
2650 currentPidProfile->motor_output_limit = sbufReadU8(src);
2651 currentPidProfile->auto_profile_cell_count = sbufReadU8(src);
2652 }
2653 pidInitConfig(currentPidProfile);
2654
2655 break;
2656 case MSP_SET_SENSOR_CONFIG:
2657 #if defined(USE_ACC)
2658 accelerometerConfigMutable()->acc_hardware = sbufReadU8(src);
2659 #else
2660 sbufReadU8(src);
2661 #endif
2662 #if defined(USE_BARO)
2663 barometerConfigMutable()->baro_hardware = sbufReadU8(src);
2664 #else
2665 sbufReadU8(src);
2666 #endif
2667 #if defined(USE_MAG)
2668 compassConfigMutable()->mag_hardware = sbufReadU8(src);
2669 #else
2670 sbufReadU8(src);
2671 #endif
2672 break;
2673
2674 #ifdef USE_ACC
2675 case MSP_ACC_CALIBRATION:
2676 if (!ARMING_FLAG(ARMED))
2677 accStartCalibration();
2678 break;
2679 #endif
2680
2681 #if defined(USE_MAG)
2682 case MSP_MAG_CALIBRATION:
2683 if (!ARMING_FLAG(ARMED)) {
2684 compassStartCalibration();
2685 }
2686 #endif
2687
2688 break;
2689 case MSP_EEPROM_WRITE:
2690 if (ARMING_FLAG(ARMED)) {
2691 return MSP_RESULT_ERROR;
2692 }
2693
2694 writeEEPROM();
2695 readEEPROM();
2696
2697 #ifdef USE_VTX_TABLE
2698 if (vtxTableNeedsInit) {
2699 vtxTableNeedsInit = false;
2700 vtxTableInit(); // Reinitialize and refresh the in-memory copies
2701 }
2702 #endif
2703
2704 break;
2705
2706 #ifdef USE_BLACKBOX
2707 case MSP_SET_BLACKBOX_CONFIG:
2708 // Don't allow config to be updated while Blackbox is logging
2709 if (blackboxMayEditConfig()) {
2710 blackboxConfigMutable()->device = sbufReadU8(src);
2711 const int rateNum = sbufReadU8(src); // was rate_num
2712 const int rateDenom = sbufReadU8(src); // was rate_denom
2713 if (sbufBytesRemaining(src) >= 2) {
2714 // p_ratio specified, so use it directly
2715 blackboxConfigMutable()->p_ratio = sbufReadU16(src);
2716 } else {
2717 // p_ratio not specified in MSP, so calculate it from old rateNum and rateDenom
2718 blackboxConfigMutable()->p_ratio = blackboxCalculatePDenom(rateNum, rateDenom);
2719 }
2720 }
2721 break;
2722 #endif
2723
2724 #ifdef USE_VTX_COMMON
2725 case MSP_SET_VTX_CONFIG:
2726 {
2727 vtxDevice_t *vtxDevice = vtxCommonDevice();
2728 vtxDevType_e vtxType = VTXDEV_UNKNOWN;
2729 if (vtxDevice) {
2730 vtxType = vtxCommonGetDeviceType(vtxDevice);
2731 }
2732 uint16_t newFrequency = sbufReadU16(src);
2733 if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { // Value is band and channel
2734 const uint8_t newBand = (newFrequency / 8) + 1;
2735 const uint8_t newChannel = (newFrequency % 8) + 1;
2736 vtxSettingsConfigMutable()->band = newBand;
2737 vtxSettingsConfigMutable()->channel = newChannel;
2738 vtxSettingsConfigMutable()->freq = vtxCommonLookupFrequency(vtxDevice, newBand, newChannel);
2739 } else if (newFrequency <= VTX_SETTINGS_MAX_FREQUENCY_MHZ) { // Value is frequency in MHz
2740 vtxSettingsConfigMutable()->band = 0;
2741 vtxSettingsConfigMutable()->freq = newFrequency;
2742 }
2743
2744 if (sbufBytesRemaining(src) >= 2) {
2745 vtxSettingsConfigMutable()->power = sbufReadU8(src);
2746 const uint8_t newPitmode = sbufReadU8(src);
2747 if (vtxType != VTXDEV_UNKNOWN) {
2748 // Delegate pitmode to vtx directly
2749 unsigned vtxCurrentStatus;
2750 vtxCommonGetStatus(vtxDevice, &vtxCurrentStatus);
2751 if ((bool)(vtxCurrentStatus & VTX_STATUS_PIT_MODE) != (bool)newPitmode) {
2752 vtxCommonSetPitMode(vtxDevice, newPitmode);
2753 }
2754 }
2755 }
2756
2757 if (sbufBytesRemaining(src)) {
2758 vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src);
2759 }
2760
2761 // API version 1.42 - this parameter kept separate since clients may already be supplying
2762 if (sbufBytesRemaining(src) >= 2) {
2763 vtxSettingsConfigMutable()->pitModeFreq = sbufReadU16(src);
2764 }
2765
2766 // API version 1.42 - extensions for non-encoded versions of the band, channel or frequency
2767 if (sbufBytesRemaining(src) >= 4) {
2768 // Added standalone values for band, channel and frequency to move
2769 // away from the flawed encoded combined method originally implemented.
2770 uint8_t newBand = sbufReadU8(src);
2771 const uint8_t newChannel = sbufReadU8(src);
2772 uint16_t newFreq = sbufReadU16(src);
2773 if (newBand) {
2774 newFreq = vtxCommonLookupFrequency(vtxDevice, newBand, newChannel);
2775 }
2776 vtxSettingsConfigMutable()->band = newBand;
2777 vtxSettingsConfigMutable()->channel = newChannel;
2778 vtxSettingsConfigMutable()->freq = newFreq;
2779 }
2780
2781 // API version 1.42 - extensions for vtxtable support
2782 if (sbufBytesRemaining(src) >= 4) {
2783 #ifdef USE_VTX_TABLE
2784 const uint8_t newBandCount = sbufReadU8(src);
2785 const uint8_t newChannelCount = sbufReadU8(src);
2786 const uint8_t newPowerCount = sbufReadU8(src);
2787
2788 if ((newBandCount > VTX_TABLE_MAX_BANDS) ||
2789 (newChannelCount > VTX_TABLE_MAX_CHANNELS) ||
2790 (newPowerCount > VTX_TABLE_MAX_POWER_LEVELS)) {
2791 return MSP_RESULT_ERROR;
2792 }
2793 vtxTableConfigMutable()->bands = newBandCount;
2794 vtxTableConfigMutable()->channels = newChannelCount;
2795 vtxTableConfigMutable()->powerLevels = newPowerCount;
2796
2797 // boolean to determine whether the vtxtable should be cleared in
2798 // expectation that the detailed band/channel and power level messages
2799 // will follow to repopulate the tables
2800 if (sbufReadU8(src)) {
2801 for (int i = 0; i < VTX_TABLE_MAX_BANDS; i++) {
2802 vtxTableConfigClearBand(vtxTableConfigMutable(), i);
2803 vtxTableConfigClearChannels(vtxTableConfigMutable(), i, 0);
2804 }
2805 vtxTableConfigClearPowerLabels(vtxTableConfigMutable(), 0);
2806 vtxTableConfigClearPowerValues(vtxTableConfigMutable(), 0);
2807 }
2808 #else
2809 sbufReadU8(src);
2810 sbufReadU8(src);
2811 sbufReadU8(src);
2812 sbufReadU8(src);
2813 #endif
2814 }
2815 }
2816 break;
2817 #endif
2818
2819 #ifdef USE_VTX_TABLE
2820 case MSP_SET_VTXTABLE_BAND:
2821 {
2822 char bandName[VTX_TABLE_BAND_NAME_LENGTH + 1];
2823 memset(bandName, 0, VTX_TABLE_BAND_NAME_LENGTH + 1);
2824 uint16_t frequencies[VTX_TABLE_MAX_CHANNELS];
2825 const uint8_t band = sbufReadU8(src);
2826 const uint8_t bandNameLength = sbufReadU8(src);
2827 for (int i = 0; i < bandNameLength; i++) {
2828 const char nameChar = sbufReadU8(src);
2829 if (i < VTX_TABLE_BAND_NAME_LENGTH) {
2830 bandName[i] = toupper(nameChar);
2831 }
2832 }
2833 const char bandLetter = toupper(sbufReadU8(src));
2834 const bool isFactoryBand = (bool)sbufReadU8(src);
2835 const uint8_t channelCount = sbufReadU8(src);
2836 for (int i = 0; i < channelCount; i++) {
2837 const uint16_t frequency = sbufReadU16(src);
2838 if (i < vtxTableConfig()->channels) {
2839 frequencies[i] = frequency;
2840 }
2841 }
2842
2843 if (band > 0 && band <= vtxTableConfig()->bands) {
2844 vtxTableStrncpyWithPad(vtxTableConfigMutable()->bandNames[band - 1], bandName, VTX_TABLE_BAND_NAME_LENGTH);
2845 vtxTableConfigMutable()->bandLetters[band - 1] = bandLetter;
2846 vtxTableConfigMutable()->isFactoryBand[band - 1] = isFactoryBand;
2847 for (int i = 0; i < vtxTableConfig()->channels; i++) {
2848 vtxTableConfigMutable()->frequency[band - 1][i] = frequencies[i];
2849 }
2850 // If this is the currently selected band then reset the frequency
2851 if (band == vtxSettingsConfig()->band) {
2852 uint16_t newFreq = 0;
2853 if (vtxSettingsConfig()->channel > 0 && vtxSettingsConfig()->channel <= vtxTableConfig()->channels) {
2854 newFreq = frequencies[vtxSettingsConfig()->channel - 1];
2855 }
2856 vtxSettingsConfigMutable()->freq = newFreq;
2857 }
2858 vtxTableNeedsInit = true; // reinintialize vtxtable after eeprom write
2859 } else {
2860 return MSP_RESULT_ERROR;
2861 }
2862 }
2863 break;
2864
2865 case MSP_SET_VTXTABLE_POWERLEVEL:
2866 {
2867 char powerLevelLabel[VTX_TABLE_POWER_LABEL_LENGTH + 1];
2868 memset(powerLevelLabel, 0, VTX_TABLE_POWER_LABEL_LENGTH + 1);
2869 const uint8_t powerLevel = sbufReadU8(src);
2870 const uint16_t powerValue = sbufReadU16(src);
2871 const uint8_t powerLevelLabelLength = sbufReadU8(src);
2872 for (int i = 0; i < powerLevelLabelLength; i++) {
2873 const char labelChar = sbufReadU8(src);
2874 if (i < VTX_TABLE_POWER_LABEL_LENGTH) {
2875 powerLevelLabel[i] = toupper(labelChar);
2876 }
2877 }
2878
2879 if (powerLevel > 0 && powerLevel <= vtxTableConfig()->powerLevels) {
2880 vtxTableConfigMutable()->powerValues[powerLevel - 1] = powerValue;
2881 vtxTableStrncpyWithPad(vtxTableConfigMutable()->powerLabels[powerLevel - 1], powerLevelLabel, VTX_TABLE_POWER_LABEL_LENGTH);
2882 vtxTableNeedsInit = true; // reinintialize vtxtable after eeprom write
2883 } else {
2884 return MSP_RESULT_ERROR;
2885 }
2886 }
2887 break;
2888 #endif
2889
2890 #ifdef USE_CAMERA_CONTROL
2891 case MSP_CAMERA_CONTROL:
2892 {
2893 if (ARMING_FLAG(ARMED)) {
2894 return MSP_RESULT_ERROR;
2895 }
2896
2897 const uint8_t key = sbufReadU8(src);
2898 cameraControlKeyPress(key, 0);
2899 }
2900 break;
2901 #endif
2902
2903 case MSP_SET_ARMING_DISABLED:
2904 {
2905 const uint8_t command = sbufReadU8(src);
2906 uint8_t disableRunawayTakeoff = 0;
2907 #ifndef USE_RUNAWAY_TAKEOFF
2908 UNUSED(disableRunawayTakeoff);
2909 #endif
2910 if (sbufBytesRemaining(src)) {
2911 disableRunawayTakeoff = sbufReadU8(src);
2912 }
2913 if (command) {
2914 mspArmingDisableByDescriptor(srcDesc);
2915 setArmingDisabled(ARMING_DISABLED_MSP);
2916 if (ARMING_FLAG(ARMED)) {
2917 disarm(DISARM_REASON_ARMING_DISABLED);
2918 }
2919 #ifdef USE_RUNAWAY_TAKEOFF
2920 runawayTakeoffTemporaryDisable(false);
2921 #endif
2922 } else {
2923 mspArmingEnableByDescriptor(srcDesc);
2924 if (mspIsMspArmingEnabled()) {
2925 unsetArmingDisabled(ARMING_DISABLED_MSP);
2926 #ifdef USE_RUNAWAY_TAKEOFF
2927 runawayTakeoffTemporaryDisable(disableRunawayTakeoff);
2928 #endif
2929 }
2930 }
2931 }
2932 break;
2933
2934 #ifdef USE_FLASHFS
2935 case MSP_DATAFLASH_ERASE:
2936 flashfsEraseCompletely();
2937
2938 break;
2939 #endif
2940
2941 #ifdef USE_GPS
2942 case MSP_SET_RAW_GPS:
2943 if (sbufReadU8(src)) {
2944 ENABLE_STATE(GPS_FIX);
2945 } else {
2946 DISABLE_STATE(GPS_FIX);
2947 }
2948 gpsSol.numSat = sbufReadU8(src);
2949 gpsSol.llh.lat = sbufReadU32(src);
2950 gpsSol.llh.lon = sbufReadU32(src);
2951 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.
2952 gpsSol.groundSpeed = sbufReadU16(src);
2953 GPS_update |= GPS_MSP_UPDATE; // MSP data signalisation to GPS functions
2954 break;
2955 #endif // USE_GPS
2956 case MSP_SET_FEATURE_CONFIG:
2957 featureConfigReplace(sbufReadU32(src));
2958 break;
2959
2960 #ifdef USE_BEEPER
2961 case MSP_SET_BEEPER_CONFIG:
2962 beeperConfigMutable()->beeper_off_flags = sbufReadU32(src);
2963 if (sbufBytesRemaining(src) >= 1) {
2964 beeperConfigMutable()->dshotBeaconTone = sbufReadU8(src);
2965 }
2966 if (sbufBytesRemaining(src) >= 4) {
2967 beeperConfigMutable()->dshotBeaconOffFlags = sbufReadU32(src);
2968 }
2969 break;
2970 #endif
2971
2972 case MSP_SET_BOARD_ALIGNMENT_CONFIG:
2973 boardAlignmentMutable()->rollDegrees = sbufReadU16(src);
2974 boardAlignmentMutable()->pitchDegrees = sbufReadU16(src);
2975 boardAlignmentMutable()->yawDegrees = sbufReadU16(src);
2976 break;
2977
2978 case MSP_SET_MIXER_CONFIG:
2979 #ifndef USE_QUAD_MIXER_ONLY
2980 mixerConfigMutable()->mixerMode = sbufReadU8(src);
2981 #else
2982 sbufReadU8(src);
2983 #endif
2984 if (sbufBytesRemaining(src) >= 1) {
2985 mixerConfigMutable()->yaw_motors_reversed = sbufReadU8(src);
2986 }
2987 break;
2988
2989 case MSP_SET_RX_CONFIG:
2990 rxConfigMutable()->serialrx_provider = sbufReadU8(src);
2991 rxConfigMutable()->maxcheck = sbufReadU16(src);
2992 rxConfigMutable()->midrc = sbufReadU16(src);
2993 rxConfigMutable()->mincheck = sbufReadU16(src);
2994 rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src);
2995 if (sbufBytesRemaining(src) >= 4) {
2996 rxConfigMutable()->rx_min_usec = sbufReadU16(src);
2997 rxConfigMutable()->rx_max_usec = sbufReadU16(src);
2998 }
2999 if (sbufBytesRemaining(src) >= 4) {
3000 rxConfigMutable()->rcInterpolation = sbufReadU8(src);
3001 rxConfigMutable()->rcInterpolationInterval = sbufReadU8(src);
3002 rxConfigMutable()->airModeActivateThreshold = (sbufReadU16(src) - 1000) / 10;
3003 }
3004 if (sbufBytesRemaining(src) >= 6) {
3005 #ifdef USE_RX_SPI
3006 rxSpiConfigMutable()->rx_spi_protocol = sbufReadU8(src);
3007 rxSpiConfigMutable()->rx_spi_id = sbufReadU32(src);
3008 rxSpiConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(src);
3009 #else
3010 sbufReadU8(src);
3011 sbufReadU32(src);
3012 sbufReadU8(src);
3013 #endif
3014 }
3015 if (sbufBytesRemaining(src) >= 1) {
3016 rxConfigMutable()->fpvCamAngleDegrees = sbufReadU8(src);
3017 }
3018 if (sbufBytesRemaining(src) >= 6) {
3019 // Added in MSP API 1.40
3020 rxConfigMutable()->rcInterpolationChannels = sbufReadU8(src);
3021 #if defined(USE_RC_SMOOTHING_FILTER)
3022 configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_type, sbufReadU8(src));
3023 configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_input_cutoff, sbufReadU8(src));
3024 configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_derivative_cutoff, sbufReadU8(src));
3025 configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_input_type, sbufReadU8(src));
3026 configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_derivative_type, sbufReadU8(src));
3027 #else
3028 sbufReadU8(src);
3029 sbufReadU8(src);
3030 sbufReadU8(src);
3031 sbufReadU8(src);
3032 sbufReadU8(src);
3033 #endif
3034 }
3035 if (sbufBytesRemaining(src) >= 1) {
3036 // Added in MSP API 1.40
3037 // Kept separate from the section above to work around missing Configurator support in version < 10.4.2
3038 #if defined(USE_USB_CDC_HID)
3039 usbDevConfigMutable()->type = sbufReadU8(src);
3040 #else
3041 sbufReadU8(src);
3042 #endif
3043 }
3044 if (sbufBytesRemaining(src) >= 1) {
3045 // Added in MSP API 1.42
3046 #if defined(USE_RC_SMOOTHING_FILTER)
3047 configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_auto_factor, sbufReadU8(src));
3048 #else
3049 sbufReadU8(src);
3050 #endif
3051 }
3052
3053 break;
3054 case MSP_SET_FAILSAFE_CONFIG:
3055 failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
3056 failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src);
3057 failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src);
3058 failsafeConfigMutable()->failsafe_switch_mode = sbufReadU8(src);
3059 failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src);
3060 failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src);
3061 break;
3062
3063 case MSP_SET_RXFAIL_CONFIG:
3064 i = sbufReadU8(src);
3065 if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) {
3066 rxFailsafeChannelConfigsMutable(i)->mode = sbufReadU8(src);
3067 rxFailsafeChannelConfigsMutable(i)->step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
3068 } else {
3069 return MSP_RESULT_ERROR;
3070 }
3071 break;
3072
3073 case MSP_SET_RSSI_CONFIG:
3074 rxConfigMutable()->rssi_channel = sbufReadU8(src);
3075 break;
3076
3077 case MSP_SET_RX_MAP:
3078 for (int i = 0; i < RX_MAPPABLE_CHANNEL_COUNT; i++) {
3079 rxConfigMutable()->rcmap[i] = sbufReadU8(src);
3080 }
3081 break;
3082
3083 case MSP_SET_CF_SERIAL_CONFIG:
3084 {
3085 uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4);
3086
3087 if (dataSize % portConfigSize != 0) {
3088 return MSP_RESULT_ERROR;
3089 }
3090
3091 uint8_t remainingPortsInPacket = dataSize / portConfigSize;
3092
3093 while (remainingPortsInPacket--) {
3094 uint8_t identifier = sbufReadU8(src);
3095
3096 serialPortConfig_t *portConfig = serialFindPortConfigurationMutable(identifier);
3097
3098 if (!portConfig) {
3099 return MSP_RESULT_ERROR;
3100 }
3101
3102 portConfig->identifier = identifier;
3103 portConfig->functionMask = sbufReadU16(src);
3104 portConfig->msp_baudrateIndex = sbufReadU8(src);
3105 portConfig->gps_baudrateIndex = sbufReadU8(src);
3106 portConfig->telemetry_baudrateIndex = sbufReadU8(src);
3107 portConfig->blackbox_baudrateIndex = sbufReadU8(src);
3108 }
3109 }
3110 break;
3111 case MSP2_COMMON_SET_SERIAL_CONFIG: {
3112 if (dataSize < 1) {
3113 return MSP_RESULT_ERROR;
3114 }
3115 unsigned count = sbufReadU8(src);
3116 unsigned portConfigSize = (dataSize - 1) / count;
3117 unsigned expectedPortSize = sizeof(uint8_t) + sizeof(uint32_t) + (sizeof(uint8_t) * 4);
3118 if (portConfigSize < expectedPortSize) {
3119 return MSP_RESULT_ERROR;
3120 }
3121 for (unsigned ii = 0; ii < count; ii++) {
3122 unsigned start = sbufBytesRemaining(src);
3123 uint8_t identifier = sbufReadU8(src);
3124 serialPortConfig_t *portConfig = serialFindPortConfigurationMutable(identifier);
3125
3126 if (!portConfig) {
3127 return MSP_RESULT_ERROR;
3128 }
3129
3130 portConfig->identifier = identifier;
3131 portConfig->functionMask = sbufReadU32(src);
3132 portConfig->msp_baudrateIndex = sbufReadU8(src);
3133 portConfig->gps_baudrateIndex = sbufReadU8(src);
3134 portConfig->telemetry_baudrateIndex = sbufReadU8(src);
3135 portConfig->blackbox_baudrateIndex = sbufReadU8(src);
3136 // Skip unknown bytes
3137 while (start - sbufBytesRemaining(src) < portConfigSize && sbufBytesRemaining(src)) {
3138 sbufReadU8(src);
3139 }
3140 }
3141 break;
3142 }
3143
3144 #ifdef USE_LED_STRIP_STATUS_MODE
3145 case MSP_SET_LED_COLORS:
3146 for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
3147 hsvColor_t *color = &ledStripStatusModeConfigMutable()->colors[i];
3148 color->h = sbufReadU16(src);
3149 color->s = sbufReadU8(src);
3150 color->v = sbufReadU8(src);
3151 }
3152 break;
3153 #endif
3154
3155 #ifdef USE_LED_STRIP
3156 case MSP_SET_LED_STRIP_CONFIG:
3157 {
3158 i = sbufReadU8(src);
3159 if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) {
3160 return MSP_RESULT_ERROR;
3161 }
3162 #ifdef USE_LED_STRIP_STATUS_MODE
3163 ledConfig_t *ledConfig = &ledStripStatusModeConfigMutable()->ledConfigs[i];
3164 *ledConfig = sbufReadU32(src);
3165 reevaluateLedConfig();
3166 #else
3167 sbufReadU32(src);
3168 #endif
3169 // API 1.41 - selected ledstrip_profile
3170 if (sbufBytesRemaining(src) >= 1) {
3171 ledStripConfigMutable()->ledstrip_profile = sbufReadU8(src);
3172 }
3173 }
3174 break;
3175 #endif
3176
3177 #ifdef USE_LED_STRIP_STATUS_MODE
3178 case MSP_SET_LED_STRIP_MODECOLOR:
3179 {
3180 ledModeIndex_e modeIdx = sbufReadU8(src);
3181 int funIdx = sbufReadU8(src);
3182 int color = sbufReadU8(src);
3183
3184 if (!setModeColor(modeIdx, funIdx, color)) {
3185 return MSP_RESULT_ERROR;
3186 }
3187 }
3188 break;
3189 #endif
3190
3191 case MSP_SET_NAME:
3192 memset(pilotConfigMutable()->name, 0, ARRAYLEN(pilotConfig()->name));
3193 for (unsigned int i = 0; i < MIN(MAX_NAME_LENGTH, dataSize); i++) {
3194 pilotConfigMutable()->name[i] = sbufReadU8(src);
3195 }
3196 break;
3197
3198 #ifdef USE_RTC_TIME
3199 case MSP_SET_RTC:
3200 {
3201 // Use seconds and milliseconds to make senders
3202 // easier to implement. Generating a 64 bit value
3203 // might not be trivial in some platforms.
3204 int32_t secs = (int32_t)sbufReadU32(src);
3205 uint16_t millis = sbufReadU16(src);
3206 rtcTime_t t = rtcTimeMake(secs, millis);
3207 rtcSet(&t);
3208 }
3209
3210 break;
3211 #endif
3212
3213 case MSP_SET_TX_INFO:
3214 setRssiMsp(sbufReadU8(src));
3215
3216 break;
3217
3218 #if defined(USE_BOARD_INFO)
3219 case MSP_SET_BOARD_INFO:
3220 if (!boardInformationIsSet()) {
3221 uint8_t length = sbufReadU8(src);
3222 char boardName[MAX_BOARD_NAME_LENGTH + 1];
3223 sbufReadData(src, boardName, MIN(length, MAX_BOARD_NAME_LENGTH));
3224 if (length > MAX_BOARD_NAME_LENGTH) {
3225 sbufAdvance(src, length - MAX_BOARD_NAME_LENGTH);
3226 }
3227 boardName[length] = '\0';
3228 length = sbufReadU8(src);
3229 char manufacturerId[MAX_MANUFACTURER_ID_LENGTH + 1];
3230 sbufReadData(src, manufacturerId, MIN(length, MAX_MANUFACTURER_ID_LENGTH));
3231 if (length > MAX_MANUFACTURER_ID_LENGTH) {
3232 sbufAdvance(src, length - MAX_MANUFACTURER_ID_LENGTH);
3233 }
3234 manufacturerId[length] = '\0';
3235
3236 setBoardName(boardName);
3237 setManufacturerId(manufacturerId);
3238 persistBoardInformation();
3239 } else {
3240 return MSP_RESULT_ERROR;
3241 }
3242
3243 break;
3244 #if defined(USE_SIGNATURE)
3245 case MSP_SET_SIGNATURE:
3246 if (!signatureIsSet()) {
3247 uint8_t signature[SIGNATURE_LENGTH];
3248 sbufReadData(src, signature, SIGNATURE_LENGTH);
3249 setSignature(signature);
3250 persistSignature();
3251 } else {
3252 return MSP_RESULT_ERROR;
3253 }
3254
3255 break;
3256 #endif
3257 #endif // USE_BOARD_INFO
3258 #if defined(USE_RX_BIND)
3259 case MSP2_BETAFLIGHT_BIND:
3260 if (!startRxBind()) {
3261 return MSP_RESULT_ERROR;
3262 }
3263
3264 break;
3265 #endif
3266 default:
3267 // we do not know how to handle the (valid) message, indicate error MSP $M!
3268 return MSP_RESULT_ERROR;
3269 }
3270 return MSP_RESULT_ACK;
3271 }
3272
3273 static mspResult_e mspCommonProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
3274 {
3275 UNUSED(mspPostProcessFn);
3276 const unsigned int dataSize = sbufBytesRemaining(src);
3277 UNUSED(dataSize); // maybe unused due to compiler options
3278
3279 switch (cmdMSP) {
3280 #ifdef USE_TRANSPONDER
3281 case MSP_SET_TRANSPONDER_CONFIG: {
3282 // Backward compatibility to BFC 3.1.1 is lost for this message type
3283
3284 uint8_t provider = sbufReadU8(src);
3285 uint8_t bytesRemaining = dataSize - 1;
3286
3287 if (provider > TRANSPONDER_PROVIDER_COUNT) {
3288 return MSP_RESULT_ERROR;
3289 }
3290
3291 const uint8_t requirementIndex = provider - 1;
3292 const uint8_t transponderDataSize = transponderRequirements[requirementIndex].dataLength;
3293
3294 transponderConfigMutable()->provider = provider;
3295
3296 if (provider == TRANSPONDER_NONE) {
3297 break;
3298 }
3299
3300 if (bytesRemaining != transponderDataSize) {
3301 return MSP_RESULT_ERROR;
3302 }
3303
3304 if (provider != transponderConfig()->provider) {
3305 transponderStopRepeating();
3306 }
3307
3308 memset(transponderConfigMutable()->data, 0, sizeof(transponderConfig()->data));
3309
3310 for (unsigned int i = 0; i < transponderDataSize; i++) {
3311 transponderConfigMutable()->data[i] = sbufReadU8(src);
3312 }
3313 transponderUpdateData();
3314 break;
3315 }
3316 #endif
3317
3318 case MSP_SET_VOLTAGE_METER_CONFIG: {
3319 int8_t id = sbufReadU8(src);
3320
3321 //
3322 // find and configure an ADC voltage sensor
3323 //
3324 int8_t voltageSensorADCIndex;
3325 for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) {
3326 if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) {
3327 break;
3328 }
3329 }
3330
3331 if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) {
3332 voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src);
3333 voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src);
3334 voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src);
3335 } else {
3336 // if we had any other types of voltage sensor to configure, this is where we'd do it.
3337 sbufReadU8(src);
3338 sbufReadU8(src);
3339 sbufReadU8(src);
3340 }
3341 break;
3342 }
3343
3344 case MSP_SET_CURRENT_METER_CONFIG: {
3345 int id = sbufReadU8(src);
3346
3347 switch (id) {
3348 case CURRENT_METER_ID_BATTERY_1:
3349 currentSensorADCConfigMutable()->scale = sbufReadU16(src);
3350 currentSensorADCConfigMutable()->offset = sbufReadU16(src);
3351 break;
3352 #ifdef USE_VIRTUAL_CURRENT_METER
3353 case CURRENT_METER_ID_VIRTUAL_1:
3354 currentSensorVirtualConfigMutable()->scale = sbufReadU16(src);
3355 currentSensorVirtualConfigMutable()->offset = sbufReadU16(src);
3356 break;
3357 #endif
3358 default:
3359 sbufReadU16(src);
3360 sbufReadU16(src);
3361 break;
3362 }
3363 break;
3364 }
3365
3366 case MSP_SET_BATTERY_CONFIG:
3367 batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src) * 10; // vbatlevel_warn1 in MWC2.3 GUI
3368 batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src) * 10; // vbatlevel_warn2 in MWC2.3 GUI
3369 batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src) * 10; // vbatlevel when buzzer starts to alert
3370 batteryConfigMutable()->batteryCapacity = sbufReadU16(src);
3371 batteryConfigMutable()->voltageMeterSource = sbufReadU8(src);
3372 batteryConfigMutable()->currentMeterSource = sbufReadU8(src);
3373 if (sbufBytesRemaining(src) >= 6) {
3374 batteryConfigMutable()->vbatmincellvoltage = sbufReadU16(src);
3375 batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU16(src);
3376 batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU16(src);
3377 }
3378 break;
3379
3380 #if defined(USE_OSD)
3381 case MSP_SET_OSD_CONFIG:
3382 {
3383 const uint8_t addr = sbufReadU8(src);
3384
3385 if ((int8_t)addr == -1) {
3386 /* Set general OSD settings */
3387 #ifdef USE_MAX7456
3388 vcdProfileMutable()->video_system = sbufReadU8(src);
3389 #else
3390 sbufReadU8(src); // Skip video system
3391 #endif
3392 #if defined(USE_OSD)
3393 osdConfigMutable()->units = sbufReadU8(src);
3394
3395 // Alarms
3396 osdConfigMutable()->rssi_alarm = sbufReadU8(src);
3397 osdConfigMutable()->cap_alarm = sbufReadU16(src);
3398 sbufReadU16(src); // Skip unused (previously fly timer)
3399 osdConfigMutable()->alt_alarm = sbufReadU16(src);
3400
3401 if (sbufBytesRemaining(src) >= 2) {
3402 /* Enabled warnings */
3403 // API < 1.41 supports only the low 16 bits
3404 osdConfigMutable()->enabledWarnings = sbufReadU16(src);
3405 }
3406
3407 if (sbufBytesRemaining(src) >= 4) {
3408 // 32bit version of enabled warnings (API >= 1.41)
3409 osdConfigMutable()->enabledWarnings = sbufReadU32(src);
3410 }
3411
3412 if (sbufBytesRemaining(src) >= 1) {
3413 // API >= 1.41
3414 // selected OSD profile
3415 #ifdef USE_OSD_PROFILES
3416 changeOsdProfileIndex(sbufReadU8(src));
3417 #else
3418 sbufReadU8(src);
3419 #endif // USE_OSD_PROFILES
3420 }
3421
3422 if (sbufBytesRemaining(src) >= 1) {
3423 // API >= 1.41
3424 // OSD stick overlay mode
3425
3426 #ifdef USE_OSD_STICK_OVERLAY
3427 osdConfigMutable()->overlay_radio_mode = sbufReadU8(src);
3428 #else
3429 sbufReadU8(src);
3430 #endif // USE_OSD_STICK_OVERLAY
3431
3432 }
3433
3434 if (sbufBytesRemaining(src) >= 2) {
3435 // API >= 1.43
3436 // OSD camera frame element width/height
3437 osdConfigMutable()->camera_frame_width = sbufReadU8(src);
3438 osdConfigMutable()->camera_frame_height = sbufReadU8(src);
3439 }
3440 #endif
3441 } else if ((int8_t)addr == -2) {
3442 #if defined(USE_OSD)
3443 // Timers
3444 uint8_t index = sbufReadU8(src);
3445 if (index > OSD_TIMER_COUNT) {
3446 return MSP_RESULT_ERROR;
3447 }
3448 osdConfigMutable()->timers[index] = sbufReadU16(src);
3449 #endif
3450 return MSP_RESULT_ERROR;
3451 } else {
3452 #if defined(USE_OSD)
3453 const uint16_t value = sbufReadU16(src);
3454
3455 /* Get screen index, 0 is post flight statistics, 1 and above are in flight OSD screens */
3456 const uint8_t screen = (sbufBytesRemaining(src) >= 1) ? sbufReadU8(src) : 1;
3457
3458 if (screen == 0 && addr < OSD_STAT_COUNT) {
3459 /* Set statistic item enable */
3460 osdStatSetState(addr, (value != 0));
3461 } else if (addr < OSD_ITEM_COUNT) {
3462 /* Set element positions */
3463 osdElementConfigMutable()->item_pos[addr] = value;
3464 osdAnalyzeActiveElements();
3465 } else {
3466 return MSP_RESULT_ERROR;
3467 }
3468 #else
3469 return MSP_RESULT_ERROR;
3470 #endif
3471 }
3472 }
3473 break;
3474
3475 case MSP_OSD_CHAR_WRITE:
3476 {
3477 osdCharacter_t chr;
3478 size_t osdCharacterBytes;
3479 uint16_t addr;
3480 if (dataSize >= OSD_CHAR_VISIBLE_BYTES + 2) {
3481 if (dataSize >= OSD_CHAR_BYTES + 2) {
3482 // 16 bit address, full char with metadata
3483 addr = sbufReadU16(src);
3484 osdCharacterBytes = OSD_CHAR_BYTES;
3485 } else if (dataSize >= OSD_CHAR_BYTES + 1) {
3486 // 8 bit address, full char with metadata
3487 addr = sbufReadU8(src);
3488 osdCharacterBytes = OSD_CHAR_BYTES;
3489 } else {
3490 // 16 bit character address, only visible char bytes
3491 addr = sbufReadU16(src);
3492 osdCharacterBytes = OSD_CHAR_VISIBLE_BYTES;
3493 }
3494 } else {
3495 // 8 bit character address, only visible char bytes
3496 addr = sbufReadU8(src);
3497 osdCharacterBytes = OSD_CHAR_VISIBLE_BYTES;
3498 }
3499 for (unsigned ii = 0; ii < MIN(osdCharacterBytes, sizeof(chr.data)); ii++) {
3500 chr.data[ii] = sbufReadU8(src);
3501 }
3502 displayPort_t *osdDisplayPort = osdGetDisplayPort(NULL);
3503 if (!osdDisplayPort) {
3504 return MSP_RESULT_ERROR;
3505 }
3506
3507 if (!displayWriteFontCharacter(osdDisplayPort, addr, &chr)) {
3508 return MSP_RESULT_ERROR;
3509 }
3510 }
3511 break;
3512 #endif // OSD
3513
3514 default:
3515 return mspProcessInCommand(srcDesc, cmdMSP, src);
3516 }
3517 return MSP_RESULT_ACK;
3518 }
3519
3520 /*
3521 * Returns MSP_RESULT_ACK, MSP_RESULT_ERROR or MSP_RESULT_NO_REPLY
3522 */
3523 mspResult_e mspFcProcessCommand(mspDescriptor_t srcDesc, mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
3524 {
3525 int ret = MSP_RESULT_ACK;
3526 sbuf_t *dst = &reply->buf;
3527 sbuf_t *src = &cmd->buf;
3528 const int16_t cmdMSP = cmd->cmd;
3529 // initialize reply by default
3530 reply->cmd = cmd->cmd;
3531
3532 if (mspCommonProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
3533 ret = MSP_RESULT_ACK;
3534 } else if (mspProcessOutCommand(cmdMSP, dst)) {
3535 ret = MSP_RESULT_ACK;
3536 } else if ((ret = mspFcProcessOutCommandWithArg(srcDesc, cmdMSP, src, dst, mspPostProcessFn)) != MSP_RESULT_CMD_UNKNOWN) {
3537 /* ret */;
3538 } else if (cmdMSP == MSP_SET_PASSTHROUGH) {
3539 mspFcSetPassthroughCommand(dst, src, mspPostProcessFn);
3540 ret = MSP_RESULT_ACK;
3541 #ifdef USE_FLASHFS
3542 } else if (cmdMSP == MSP_DATAFLASH_READ) {
3543 mspFcDataFlashReadCommand(dst, src);
3544 ret = MSP_RESULT_ACK;
3545 #endif
3546 } else {
3547 ret = mspCommonProcessInCommand(srcDesc, cmdMSP, src, mspPostProcessFn);
3548 }
3549 reply->result = ret;
3550 return ret;
3551 }
3552
3553 void mspFcProcessReply(mspPacket_t *reply)
3554 {
3555 sbuf_t *src = &reply->buf;
3556 UNUSED(src); // potentially unused depending on compile options.
3557
3558 switch (reply->cmd) {
3559 case MSP_ANALOG:
3560 {
3561 uint8_t batteryVoltage = sbufReadU8(src);
3562 uint16_t mAhDrawn = sbufReadU16(src);
3563 uint16_t rssi = sbufReadU16(src);
3564 uint16_t amperage = sbufReadU16(src);
3565
3566 UNUSED(rssi);
3567 UNUSED(batteryVoltage);
3568 UNUSED(amperage);
3569 UNUSED(mAhDrawn);
3570
3571 #ifdef USE_MSP_CURRENT_METER
3572 currentMeterMSPSet(amperage, mAhDrawn);
3573 #endif
3574 }
3575 break;
3576 }
3577 }
3578
3579 void mspInit(void)
3580 {
3581 initActiveBoxIds();
3582 }