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