?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Merge pull request #6363 from mikeller/add_rc_smoothing_msp
[betaflight.git] / src / main / interface / msp.c
blobb938205688b2d9fb934255a3651b4bc7eb3be54e
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
27 #include "platform.h"
28
29 #include "blackbox/blackbox.h"
30
31 #include "build/build_config.h"
32 #include "build/debug.h"
33 #include "build/version.h"
34
35 #include "common/axis.h"
36 #include "common/bitarray.h"
37 #include "common/color.h"
38 #include "common/maths.h"
39 #include "common/streambuf.h"
40 #include "common/huffman.h"
41
42 #include "config/config_eeprom.h"
43 #include "config/feature.h"
44
45 #include "drivers/accgyro/accgyro.h"
46 #include "drivers/bus_i2c.h"
47 #include "drivers/camera_control.h"
48 #include "drivers/compass/compass.h"
49 #include "drivers/flash.h"
50 #include "drivers/io.h"
51 #include "drivers/max7456.h"
52 #include "drivers/pwm_output.h"
53 #include "drivers/sdcard.h"
54 #include "drivers/serial.h"
55 #include "drivers/serial_escserial.h"
56 #include "drivers/system.h"
57 #include "drivers/transponder_ir.h"
58 #include "drivers/usb_msc.h"
59 #include "drivers/vtx_common.h"
60
61 #include "fc/board_info.h"
62 #include "fc/config.h"
63 #include "fc/controlrate_profile.h"
64 #include "fc/fc_core.h"
65 #include "fc/fc_rc.h"
66 #include "fc/rc_adjustments.h"
67 #include "fc/rc_controls.h"
68 #include "fc/rc_modes.h"
69 #include "fc/runtime_config.h"
70
71 #include "flight/position.h"
72 #include "flight/failsafe.h"
73 #include "flight/imu.h"
74 #include "flight/mixer.h"
75 #include "flight/pid.h"
76 #include "flight/servos.h"
77
78 #include "interface/msp.h"
79 #include "interface/msp_box.h"
80 #include "interface/msp_protocol.h"
81
82 #include "io/asyncfatfs/asyncfatfs.h"
83 #include "io/beeper.h"
84 #include "io/flashfs.h"
85 #include "io/gimbal.h"
86 #include "io/gps.h"
87 #include "io/ledstrip.h"
88 #include "io/motors.h"
89 #include "io/osd.h"
90 #include "io/osd_slave.h"
91 #include "io/serial.h"
92 #include "io/serial_4way.h"
93 #include "io/servos.h"
94 #include "io/transponder_ir.h"
95 #include "io/usb_msc.h"
96 #include "io/vtx_control.h"
97 #include "io/vtx.h"
98 #include "io/vtx_string.h"
99
100 #include "msp/msp_serial.h"
101
102 #include "pg/beeper.h"
103 #include "pg/board.h"
104 #include "pg/pg.h"
105 #include "pg/pg_ids.h"
106 #include "pg/rx.h"
107 #include "pg/rx_spi.h"
108 #include "pg/vcd.h"
109
110 #include "rx/rx.h"
111 #include "rx/msp.h"
112
113 #include "scheduler/scheduler.h"
114
115 #include "sensors/battery.h"
116
117 #include "sensors/acceleration.h"
118 #include "sensors/barometer.h"
119 #include "sensors/boardalignment.h"
120 #include "sensors/esc_sensor.h"
121 #include "sensors/compass.h"
122 #include "sensors/gyro.h"
123 #include "sensors/rangefinder.h"
124 #include "sensors/sensors.h"
125
126 #include "telemetry/telemetry.h"
127
128 #ifdef USE_HARDWARE_REVISION_DETECTION
129 #include "hardware_revision.h"
130 #endif
131
132 static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
133
134 enum {
135 MSP_REBOOT_FIRMWARE = 0,
136 MSP_REBOOT_BOOTLOADER,
137 MSP_REBOOT_MSC,
138 MSP_REBOOT_COUNT,
139 };
140
141 static uint8_t rebootMode;
142
143 #ifndef USE_OSD_SLAVE
144
145 static const char pidnames[] =
146 "ROLL;"
147 "PITCH;"
148 "YAW;"
149 "ALT;"
150 "Pos;"
151 "PosR;"
152 "NavR;"
153 "LEVEL;"
154 "MAG;"
155 "VEL;";
156
157 typedef enum {
158 MSP_SDCARD_STATE_NOT_PRESENT = 0,
159 MSP_SDCARD_STATE_FATAL = 1,
160 MSP_SDCARD_STATE_CARD_INIT = 2,
161 MSP_SDCARD_STATE_FS_INIT = 3,
162 MSP_SDCARD_STATE_READY = 4
163 } mspSDCardState_e;
164
165 typedef enum {
166 MSP_SDCARD_FLAG_SUPPORTTED = 1
167 } mspSDCardFlags_e;
168
169 typedef enum {
170 MSP_FLASHFS_FLAG_READY = 1,
171 MSP_FLASHFS_FLAG_SUPPORTED = 2
172 } mspFlashFsFlags_e;
173
174 #define RATEPROFILE_MASK (1 << 7)
175 #endif //USE_OSD_SLAVE
176
177 #define RTC_NOT_SUPPORTED 0xff
178
179 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
180 #define ESC_4WAY 0xff
181
182 uint8_t escMode;
183 uint8_t escPortIndex;
184
185 #ifdef USE_ESCSERIAL
186 static void mspEscPassthroughFn(serialPort_t *serialPort)
187 {
188 escEnablePassthrough(serialPort, escPortIndex, escMode);
189 }
190 #endif
191
192 static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
193 {
194 const unsigned int dataSize = sbufBytesRemaining(src);
195 if (dataSize == 0) {
196 // Legacy format
197
198 escMode = ESC_4WAY;
199 } else {
200 escMode = sbufReadU8(src);
201 escPortIndex = sbufReadU8(src);
202 }
203
204 switch (escMode) {
205 case ESC_4WAY:
206 // get channel number
207 // switch all motor lines HI
208 // reply with the count of ESC found
209 sbufWriteU8(dst, esc4wayInit());
210
211 if (mspPostProcessFn) {
212 *mspPostProcessFn = esc4wayProcess;
213 }
214 break;
215
216 #ifdef USE_ESCSERIAL
217 case PROTOCOL_SIMONK:
218 case PROTOCOL_BLHELI:
219 case PROTOCOL_KISS:
220 case PROTOCOL_KISSALL:
221 case PROTOCOL_CASTLE:
222 if (escPortIndex < getMotorCount() || (escMode == PROTOCOL_KISS && escPortIndex == ALL_MOTORS)) {
223 sbufWriteU8(dst, 1);
224
225 if (mspPostProcessFn) {
226 *mspPostProcessFn = mspEscPassthroughFn;
227 }
228
229 break;
230 }
231 FALLTHROUGH;
232 #endif
233 default:
234 sbufWriteU8(dst, 0);
235 }
236 }
237 #endif //USE_SERIAL_4WAY_BLHELI_INTERFACE
238
239 static void mspRebootFn(serialPort_t *serialPort)
240 {
241 UNUSED(serialPort);
242
243 #ifndef USE_OSD_SLAVE
244 stopPwmAllMotors();
245 #endif
246
247 switch (rebootMode) {
248 case MSP_REBOOT_FIRMWARE:
249 systemReset();
250
251 break;
252 case MSP_REBOOT_BOOTLOADER:
253 systemResetToBootloader();
254
255 break;
256 #if defined(USE_USB_MSC)
257 case MSP_REBOOT_MSC:
258 systemResetToMsc();
259
260 break;
261 #endif
262 default:
263
264 break;
265 }
266
267 // control should never return here.
268 while (true) ;
269 }
270
271 #ifndef USE_OSD_SLAVE
272 static void serializeSDCardSummaryReply(sbuf_t *dst)
273 {
274 #ifdef USE_SDCARD
275 uint8_t flags = MSP_SDCARD_FLAG_SUPPORTTED;
276 uint8_t state = 0;
277
278 sbufWriteU8(dst, flags);
279
280 // Merge the card and filesystem states together
281 if (!sdcard_isInserted()) {
282 state = MSP_SDCARD_STATE_NOT_PRESENT;
283 } else if (!sdcard_isFunctional()) {
284 state = MSP_SDCARD_STATE_FATAL;
285 } else {
286 switch (afatfs_getFilesystemState()) {
287 case AFATFS_FILESYSTEM_STATE_READY:
288 state = MSP_SDCARD_STATE_READY;
289 break;
290
291 case AFATFS_FILESYSTEM_STATE_INITIALIZATION:
292 if (sdcard_isInitialized()) {
293 state = MSP_SDCARD_STATE_FS_INIT;
294 } else {
295 state = MSP_SDCARD_STATE_CARD_INIT;
296 }
297 break;
298
299 case AFATFS_FILESYSTEM_STATE_FATAL:
300 case AFATFS_FILESYSTEM_STATE_UNKNOWN:
301 default:
302 state = MSP_SDCARD_STATE_FATAL;
303 break;
304 }
305 }
306
307 sbufWriteU8(dst, state);
308 sbufWriteU8(dst, afatfs_getLastError());
309 // Write free space and total space in kilobytes
310 sbufWriteU32(dst, afatfs_getContiguousFreeSpace() / 1024);
311 sbufWriteU32(dst, sdcard_getMetadata()->numBlocks / 2); // Block size is half a kilobyte
312 #else
313 sbufWriteU8(dst, 0);
314 sbufWriteU8(dst, 0);
315 sbufWriteU8(dst, 0);
316 sbufWriteU32(dst, 0);
317 sbufWriteU32(dst, 0);
318 #endif
319 }
320
321 static void serializeDataflashSummaryReply(sbuf_t *dst)
322 {
323 #ifdef USE_FLASHFS
324 if (flashfsIsSupported()) {
325 uint8_t flags = MSP_FLASHFS_FLAG_SUPPORTED;
326 flags |= (flashfsIsReady() ? MSP_FLASHFS_FLAG_READY : 0);
327 const flashGeometry_t *geometry = flashfsGetGeometry();
328 sbufWriteU8(dst, flags);
329 sbufWriteU32(dst, geometry->sectors);
330 sbufWriteU32(dst, geometry->totalSize);
331 sbufWriteU32(dst, flashfsGetOffset()); // Effectively the current number of bytes stored on the volume
332 } else
333 #endif
334
335 // FlashFS is not configured or valid device is not detected
336 {
337 sbufWriteU8(dst, 0);
338 sbufWriteU32(dst, 0);
339 sbufWriteU32(dst, 0);
340 sbufWriteU32(dst, 0);
341 }
342 }
343
344 #ifdef USE_FLASHFS
345 enum compressionType_e {
346 NO_COMPRESSION,
347 HUFFMAN
348 };
349
350 static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uint16_t size, bool useLegacyFormat, bool allowCompression)
351 {
352 BUILD_BUG_ON(MSP_PORT_DATAFLASH_INFO_SIZE < 16);
353
354 uint16_t readLen = size;
355 const int bytesRemainingInBuf = sbufBytesRemaining(dst) - MSP_PORT_DATAFLASH_INFO_SIZE;
356 if (readLen > bytesRemainingInBuf) {
357 readLen = bytesRemainingInBuf;
358 }
359 // size will be lower than that requested if we reach end of volume
360 const uint32_t flashfsSize = flashfsGetSize();
361 if (readLen > flashfsSize - address) {
362 // truncate the request
363 readLen = flashfsSize - address;
364 }
365 sbufWriteU32(dst, address);
366
367 // legacy format does not support compression
368 #ifdef USE_HUFFMAN
369 const uint8_t compressionMethod = (!allowCompression || useLegacyFormat) ? NO_COMPRESSION : HUFFMAN;
370 #else
371 const uint8_t compressionMethod = NO_COMPRESSION;
372 UNUSED(allowCompression);
373 #endif
374
375 if (compressionMethod == NO_COMPRESSION) {
376 if (!useLegacyFormat) {
377 // new format supports variable read lengths
378 sbufWriteU16(dst, readLen);
379 sbufWriteU8(dst, 0); // placeholder for compression format
380 }
381
382 const int bytesRead = flashfsReadAbs(address, sbufPtr(dst), readLen);
383
384 sbufAdvance(dst, bytesRead);
385
386 if (useLegacyFormat) {
387 // pad the buffer with zeros
388 for (int i = bytesRead; i < size; i++) {
389 sbufWriteU8(dst, 0);
390 }
391 }
392 } else {
393 #ifdef USE_HUFFMAN
394 // compress in 256-byte chunks
395 const uint16_t READ_BUFFER_SIZE = 256;
396 uint8_t readBuffer[READ_BUFFER_SIZE];
397
398 huffmanState_t state = {
399 .bytesWritten = 0,
400 .outByte = sbufPtr(dst) + sizeof(uint16_t) + sizeof(uint8_t) + HUFFMAN_INFO_SIZE,
401 .outBufLen = readLen,
402 .outBit = 0x80,
403 };
404 *state.outByte = 0;
405
406 uint16_t bytesReadTotal = 0;
407 // read until output buffer overflows or flash is exhausted
408 while (state.bytesWritten < state.outBufLen && address + bytesReadTotal < flashfsSize) {
409 const int bytesRead = flashfsReadAbs(address + bytesReadTotal, readBuffer,
410 MIN(sizeof(readBuffer), flashfsSize - address - bytesReadTotal));
411
412 const int status = huffmanEncodeBufStreaming(&state, readBuffer, bytesRead, huffmanTable);
413 if (status == -1) {
414 // overflow
415 break;
416 }
417
418 bytesReadTotal += bytesRead;
419 }
420
421 if (state.outBit != 0x80) {
422 ++state.bytesWritten;
423 }
424
425 // header
426 sbufWriteU16(dst, HUFFMAN_INFO_SIZE + state.bytesWritten);
427 sbufWriteU8(dst, compressionMethod);
428 // payload
429 sbufWriteU16(dst, bytesReadTotal);
430 sbufAdvance(dst, state.bytesWritten);
431 #endif
432 }
433 }
434 #endif // USE_FLASHFS
435 #endif // USE_OSD_SLAVE
436
437 /*
438 * Returns true if the command was processd, false otherwise.
439 * May set mspPostProcessFunc to a function to be called once the command has been processed
440 */
441 static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
442 {
443 UNUSED(mspPostProcessFn);
444
445 switch (cmdMSP) {
446 case MSP_API_VERSION:
447 sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
448 sbufWriteU8(dst, API_VERSION_MAJOR);
449 sbufWriteU8(dst, API_VERSION_MINOR);
450 break;
451
452 case MSP_FC_VARIANT:
453 sbufWriteData(dst, flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
454 break;
455
456 case MSP_FC_VERSION:
457 sbufWriteU8(dst, FC_VERSION_MAJOR);
458 sbufWriteU8(dst, FC_VERSION_MINOR);
459 sbufWriteU8(dst, FC_VERSION_PATCH_LEVEL);
460 break;
461
462 case MSP_BOARD_INFO:
463 {
464 sbufWriteData(dst, systemConfig()->boardIdentifier, BOARD_IDENTIFIER_LENGTH);
465 #ifdef USE_HARDWARE_REVISION_DETECTION
466 sbufWriteU16(dst, hardwareRevision);
467 #else
468 sbufWriteU16(dst, 0); // No other build targets currently have hardware revision detection.
469 #endif
470 #ifdef USE_OSD_SLAVE
471 sbufWriteU8(dst, 1); // 1 == OSD
472 #else
473 #if defined(USE_OSD) && defined(USE_MAX7456)
474 sbufWriteU8(dst, 2); // 2 == FC with OSD
475 #else
476 sbufWriteU8(dst, 0); // 0 == FC
477 #endif
478 #endif
479 // Board communication capabilities (uint8)
480 // Bit 0: 1 iff the board has VCP
481 // Bit 1: 1 iff the board supports software serial
482 uint8_t commCapabilities = 0;
483 #ifdef USE_VCP
484 commCapabilities |= 1 << 0;
485 #endif
486 #if defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)
487 commCapabilities |= 1 << 1;
488 #endif
489 sbufWriteU8(dst, commCapabilities);
490
491 // Target name with explicit length
492 sbufWriteU8(dst, strlen(targetName));
493 sbufWriteData(dst, targetName, strlen(targetName));
494
495 #if defined(USE_BOARD_INFO)
496 // Board name with explicit length
497 char *value = getBoardName();
498 sbufWriteU8(dst, strlen(value));
499 sbufWriteString(dst, value);
500
501 // Manufacturer id with explicit length
502 value = getManufacturerId();
503 sbufWriteU8(dst, strlen(value));
504 sbufWriteString(dst, value);
505
506 #if defined(USE_SIGNATURE)
507 // Signature
508 sbufWriteData(dst, getSignature(), SIGNATURE_LENGTH);
509 #endif
510 #endif // USE_BOARD_INFO
511
512 break;
513 }
514
515 case MSP_BUILD_INFO:
516 sbufWriteData(dst, buildDate, BUILD_DATE_LENGTH);
517 sbufWriteData(dst, buildTime, BUILD_TIME_LENGTH);
518 sbufWriteData(dst, shortGitRevision, GIT_SHORT_REVISION_LENGTH);
519 break;
520
521 case MSP_ANALOG:
522 sbufWriteU8(dst, (uint8_t)constrain(getBatteryVoltage(), 0, 255));
523 sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
524 #ifdef USE_OSD_SLAVE
525 sbufWriteU16(dst, 0); // rssi
526 #else
527 sbufWriteU16(dst, getRssi());
528 #endif
529 sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send current in 0.01 A steps, range is -320A to 320A
530 break;
531
532 case MSP_DEBUG:
533 // output some useful QA statistics
534 // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock]
535
536 for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) {
537 sbufWriteU16(dst, debug[i]); // 4 variables are here for general monitoring purpose
538 }
539 break;
540
541 case MSP_UID:
542 sbufWriteU32(dst, U_ID_0);
543 sbufWriteU32(dst, U_ID_1);
544 sbufWriteU32(dst, U_ID_2);
545 break;
546
547 case MSP_FEATURE_CONFIG:
548 sbufWriteU32(dst, featureMask());
549 break;
550
551 #ifdef USE_BEEPER
552 case MSP_BEEPER_CONFIG:
553 sbufWriteU32(dst, beeperConfig()->beeper_off_flags);
554 sbufWriteU8(dst, beeperConfig()->dshotBeaconTone);
555 sbufWriteU32(dst, beeperConfig()->dshotBeaconOffFlags);
556 break;
557 #endif
558
559 case MSP_BATTERY_STATE: {
560 // battery characteristics
561 sbufWriteU8(dst, (uint8_t)constrain(getBatteryCellCount(), 0, 255)); // 0 indicates battery not detected.
562 sbufWriteU16(dst, batteryConfig()->batteryCapacity); // in mAh
563
564 // battery state
565 sbufWriteU8(dst, (uint8_t)constrain(getBatteryVoltage(), 0, 255)); // in 0.1V steps
566 sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
567 sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send current in 0.01 A steps, range is -320A to 320A
568
569 // battery alerts
570 sbufWriteU8(dst, (uint8_t)getBatteryState());
571 break;
572 }
573
574 case MSP_VOLTAGE_METERS: {
575 // write out id and voltage meter values, once for each meter we support
576 uint8_t count = supportedVoltageMeterCount;
577 #ifdef USE_ESC_SENSOR
578 count -= VOLTAGE_METER_ID_ESC_COUNT - getMotorCount();
579 #endif
580
581 for (int i = 0; i < count; i++) {
582
583 voltageMeter_t meter;
584 uint8_t id = (uint8_t)voltageMeterIds[i];
585 voltageMeterRead(id, &meter);
586
587 sbufWriteU8(dst, id);
588 sbufWriteU8(dst, (uint8_t)constrain(meter.filtered, 0, 255));
589 }
590 break;
591 }
592
593 case MSP_CURRENT_METERS: {
594 // write out id and current meter values, once for each meter we support
595 uint8_t count = supportedCurrentMeterCount;
596 #ifdef USE_ESC_SENSOR
597 count -= VOLTAGE_METER_ID_ESC_COUNT - getMotorCount();
598 #endif
599 for (int i = 0; i < count; i++) {
600
601 currentMeter_t meter;
602 uint8_t id = (uint8_t)currentMeterIds[i];
603 currentMeterRead(id, &meter);
604
605 sbufWriteU8(dst, id);
606 sbufWriteU16(dst, (uint16_t)constrain(meter.mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
607 sbufWriteU16(dst, (uint16_t)constrain(meter.amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps (mA). Negative range is truncated to zero
608 }
609 break;
610 }
611
612 case MSP_VOLTAGE_METER_CONFIG:
613 // by using a sensor type and a sub-frame length it's possible to configure any type of voltage meter,
614 // e.g. an i2c/spi/can sensor or any sensor not built directly into the FC such as ESC/RX/SPort/SBus that has
615 // different configuration requirements.
616 BUILD_BUG_ON(VOLTAGE_SENSOR_ADC_VBAT != 0); // VOLTAGE_SENSOR_ADC_VBAT should be the first index,
617 sbufWriteU8(dst, MAX_VOLTAGE_SENSOR_ADC); // voltage meters in payload
618 for (int i = VOLTAGE_SENSOR_ADC_VBAT; i < MAX_VOLTAGE_SENSOR_ADC; i++) {
619 const uint8_t adcSensorSubframeLength = 1 + 1 + 1 + 1 + 1; // length of id, type, vbatscale, vbatresdivval, vbatresdivmultipler, in bytes
620 sbufWriteU8(dst, adcSensorSubframeLength); // ADC sensor sub-frame length
621
622 sbufWriteU8(dst, voltageMeterADCtoIDMap[i]); // id of the sensor
623 sbufWriteU8(dst, VOLTAGE_SENSOR_TYPE_ADC_RESISTOR_DIVIDER); // indicate the type of sensor that the next part of the payload is for
624
625 sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatscale);
626 sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivval);
627 sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivmultiplier);
628 }
629 // if we had any other voltage sensors, this is where we would output any needed configuration
630 break;
631
632 case MSP_CURRENT_METER_CONFIG: {
633 // the ADC and VIRTUAL sensors have the same configuration requirements, however this API reflects
634 // that this situation may change and allows us to support configuration of any current sensor with
635 // specialist configuration requirements.
636
637 int currentMeterCount = 1;
638
639 #ifdef USE_VIRTUAL_CURRENT_METER
640 currentMeterCount++;
641 #endif
642 sbufWriteU8(dst, currentMeterCount);
643
644 const uint8_t adcSensorSubframeLength = 1 + 1 + 2 + 2; // length of id, type, scale, offset, in bytes
645 sbufWriteU8(dst, adcSensorSubframeLength);
646 sbufWriteU8(dst, CURRENT_METER_ID_BATTERY_1); // the id of the meter
647 sbufWriteU8(dst, CURRENT_SENSOR_ADC); // indicate the type of sensor that the next part of the payload is for
648 sbufWriteU16(dst, currentSensorADCConfig()->scale);
649 sbufWriteU16(dst, currentSensorADCConfig()->offset);
650
651 #ifdef USE_VIRTUAL_CURRENT_METER
652 const int8_t virtualSensorSubframeLength = 1 + 1 + 2 + 2; // length of id, type, scale, offset, in bytes
653 sbufWriteU8(dst, virtualSensorSubframeLength);
654 sbufWriteU8(dst, CURRENT_METER_ID_VIRTUAL_1); // the id of the meter
655 sbufWriteU8(dst, CURRENT_SENSOR_VIRTUAL); // indicate the type of sensor that the next part of the payload is for
656 sbufWriteU16(dst, currentSensorVirtualConfig()->scale);
657 sbufWriteU16(dst, currentSensorVirtualConfig()->offset);
658 #endif
659
660 // if we had any other current sensors, this is where we would output any needed configuration
661 break;
662 }
663
664 case MSP_BATTERY_CONFIG:
665 sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage);
666 sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage);
667 sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage);
668 sbufWriteU16(dst, batteryConfig()->batteryCapacity);
669 sbufWriteU8(dst, batteryConfig()->voltageMeterSource);
670 sbufWriteU8(dst, batteryConfig()->currentMeterSource);
671 break;
672
673 case MSP_TRANSPONDER_CONFIG: {
674 #ifdef USE_TRANSPONDER
675 // Backward compatibility to BFC 3.1.1 is lost for this message type
676 sbufWriteU8(dst, TRANSPONDER_PROVIDER_COUNT);
677 for (unsigned int i = 0; i < TRANSPONDER_PROVIDER_COUNT; i++) {
678 sbufWriteU8(dst, transponderRequirements[i].provider);
679 sbufWriteU8(dst, transponderRequirements[i].dataLength);
680 }
681
682 uint8_t provider = transponderConfig()->provider;
683 sbufWriteU8(dst, provider);
684
685 if (provider) {
686 uint8_t requirementIndex = provider - 1;
687 uint8_t providerDataLength = transponderRequirements[requirementIndex].dataLength;
688
689 for (unsigned int i = 0; i < providerDataLength; i++) {
690 sbufWriteU8(dst, transponderConfig()->data[i]);
691 }
692 }
693 #else
694 sbufWriteU8(dst, 0); // no providers
695 #endif
696 break;
697 }
698
699 case MSP_OSD_CONFIG: {
700 #define OSD_FLAGS_OSD_FEATURE (1 << 0)
701 #define OSD_FLAGS_OSD_SLAVE (1 << 1)
702 #define OSD_FLAGS_RESERVED_1 (1 << 2)
703 #define OSD_FLAGS_RESERVED_2 (1 << 3)
704 #define OSD_FLAGS_OSD_HARDWARE_MAX_7456 (1 << 4)
705
706 uint8_t osdFlags = 0;
707 #if defined(USE_OSD)
708 osdFlags |= OSD_FLAGS_OSD_FEATURE;
709 #endif
710 #if defined(USE_OSD_SLAVE)
711 osdFlags |= OSD_FLAGS_OSD_SLAVE;
712 #endif
713 #ifdef USE_MAX7456
714 osdFlags |= OSD_FLAGS_OSD_HARDWARE_MAX_7456;
715 #endif
716
717 sbufWriteU8(dst, osdFlags);
718
719 #ifdef USE_MAX7456
720 // send video system (AUTO/PAL/NTSC)
721 sbufWriteU8(dst, vcdProfile()->video_system);
722 #else
723 sbufWriteU8(dst, 0);
724 #endif
725
726 #ifdef USE_OSD
727 // OSD specific, not applicable to OSD slaves.
728
729 // Configuration
730 sbufWriteU8(dst, osdConfig()->units);
731
732 // Alarms
733 sbufWriteU8(dst, osdConfig()->rssi_alarm);
734 sbufWriteU16(dst, osdConfig()->cap_alarm);
735
736 // Reuse old timer alarm (U16) as OSD_ITEM_COUNT
737 sbufWriteU8(dst, 0);
738 sbufWriteU8(dst, OSD_ITEM_COUNT);
739
740 sbufWriteU16(dst, osdConfig()->alt_alarm);
741
742 // Element position and visibility
743 for (int i = 0; i < OSD_ITEM_COUNT; i++) {
744 sbufWriteU16(dst, osdConfig()->item_pos[i]);
745 }
746
747 // Post flight statistics
748 sbufWriteU8(dst, OSD_STAT_COUNT);
749 for (int i = 0; i < OSD_STAT_COUNT; i++ ) {
750 sbufWriteU8(dst, osdStatGetState(i));
751 }
752
753 // Timers
754 sbufWriteU8(dst, OSD_TIMER_COUNT);
755 for (int i = 0; i < OSD_TIMER_COUNT; i++) {
756 sbufWriteU16(dst, osdConfig()->timers[i]);
757 }
758
759 // Enabled warnings
760 sbufWriteU16(dst, osdConfig()->enabledWarnings);
761 #endif
762 break;
763 }
764
765 default:
766 return false;
767 }
768 return true;
769 }
770
771 #ifdef USE_OSD_SLAVE
772 static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
773 {
774 switch (cmdMSP) {
775 case MSP_STATUS_EX:
776 case MSP_STATUS:
777 sbufWriteU16(dst, getTaskDeltaTime(TASK_SERIAL));
778 #ifdef USE_I2C
779 sbufWriteU16(dst, i2cGetErrorCounter());
780 #else
781 sbufWriteU16(dst, 0);
782 #endif
783 sbufWriteU16(dst, 0); // sensors
784 sbufWriteU32(dst, 0); // flight modes
785 sbufWriteU8(dst, 0); // profile
786 sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
787 if (cmdMSP == MSP_STATUS_EX) {
788 sbufWriteU8(dst, 1); // max profiles
789 sbufWriteU8(dst, 0); // control rate profile
790 } else {
791 sbufWriteU16(dst, 0); // gyro cycle time
792 }
793 break;
794
795 default:
796 return false;
797 }
798 return true;
799 }
800
801 #else
802
803 static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
804 {
805 bool unsupportedCommand = false;
806
807 switch (cmdMSP) {
808 case MSP_STATUS_EX:
809 case MSP_STATUS:
810 {
811 boxBitmask_t flightModeFlags;
812 const int flagBits = packFlightModeFlags(&flightModeFlags);
813
814 sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID));
815 #ifdef USE_I2C
816 sbufWriteU16(dst, i2cGetErrorCounter());
817 #else
818 sbufWriteU16(dst, 0);
819 #endif
820 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);
821 sbufWriteData(dst, &flightModeFlags, 4); // unconditional part of flags, first 32 bits
822 sbufWriteU8(dst, getCurrentPidProfileIndex());
823 sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
824 if (cmdMSP == MSP_STATUS_EX) {
825 sbufWriteU8(dst, MAX_PROFILE_COUNT);
826 sbufWriteU8(dst, getCurrentControlRateProfileIndex());
827 } else { // MSP_STATUS
828 sbufWriteU16(dst, 0); // gyro cycle time
829 }
830
831 // write flightModeFlags header. Lowest 4 bits contain number of bytes that follow
832 // header is emited even when all bits fit into 32 bits to allow future extension
833 int byteCount = (flagBits - 32 + 7) / 8; // 32 already stored, round up
834 byteCount = constrain(byteCount, 0, 15); // limit to 16 bytes (128 bits)
835 sbufWriteU8(dst, byteCount);
836 sbufWriteData(dst, ((uint8_t*)&flightModeFlags) + 4, byteCount);
837
838 // Write arming disable flags
839 // 1 byte, flag count
840 sbufWriteU8(dst, ARMING_DISABLE_FLAGS_COUNT);
841 // 4 bytes, flags
842 const uint32_t armingDisableFlags = getArmingDisableFlags();
843 sbufWriteU32(dst, armingDisableFlags);
844 }
845 break;
846
847 case MSP_RAW_IMU:
848 {
849 // Hack scale due to choice of units for sensor data in multiwii
850
851 uint8_t scale;
852
853 if (acc.dev.acc_1G > 512*4) {
854 scale = 8;
855 } else if (acc.dev.acc_1G > 512*2) {
856 scale = 4;
857 } else if (acc.dev.acc_1G >= 512) {
858 scale = 2;
859 } else {
860 scale = 1;
861 }
862
863 for (int i = 0; i < 3; i++) {
864 sbufWriteU16(dst, lrintf(acc.accADC[i] / scale));
865 }
866 for (int i = 0; i < 3; i++) {
867 sbufWriteU16(dst, gyroRateDps(i));
868 }
869 for (int i = 0; i < 3; i++) {
870 sbufWriteU16(dst, lrintf(mag.magADC[i]));
871 }
872 }
873 break;
874
875 case MSP_NAME:
876 {
877 const int nameLen = strlen(pilotConfig()->name);
878 for (int i = 0; i < nameLen; i++) {
879 sbufWriteU8(dst, pilotConfig()->name[i]);
880 }
881 }
882 break;
883
884 #ifdef USE_SERVOS
885 case MSP_SERVO:
886 sbufWriteData(dst, &servo, MAX_SUPPORTED_SERVOS * 2);
887 break;
888 case MSP_SERVO_CONFIGURATIONS:
889 for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
890 sbufWriteU16(dst, servoParams(i)->min);
891 sbufWriteU16(dst, servoParams(i)->max);
892 sbufWriteU16(dst, servoParams(i)->middle);
893 sbufWriteU8(dst, servoParams(i)->rate);
894 sbufWriteU8(dst, servoParams(i)->forwardFromChannel);
895 sbufWriteU32(dst, servoParams(i)->reversedSources);
896 }
897 break;
898
899 case MSP_SERVO_MIX_RULES:
900 for (int i = 0; i < MAX_SERVO_RULES; i++) {
901 sbufWriteU8(dst, customServoMixers(i)->targetChannel);
902 sbufWriteU8(dst, customServoMixers(i)->inputSource);
903 sbufWriteU8(dst, customServoMixers(i)->rate);
904 sbufWriteU8(dst, customServoMixers(i)->speed);
905 sbufWriteU8(dst, customServoMixers(i)->min);
906 sbufWriteU8(dst, customServoMixers(i)->max);
907 sbufWriteU8(dst, customServoMixers(i)->box);
908 }
909 break;
910 #endif
911
912 case MSP_MOTOR:
913 for (unsigned i = 0; i < 8; i++) {
914 if (i >= MAX_SUPPORTED_MOTORS || !pwmGetMotors()[i].enabled) {
915 sbufWriteU16(dst, 0);
916 continue;
917 }
918
919 sbufWriteU16(dst, convertMotorToExternal(motor[i]));
920 }
921 break;
922
923 case MSP_RC:
924 for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
925 sbufWriteU16(dst, rcData[i]);
926 }
927 break;
928
929 case MSP_ATTITUDE:
930 sbufWriteU16(dst, attitude.values.roll);
931 sbufWriteU16(dst, attitude.values.pitch);
932 sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
933 break;
934
935 case MSP_ALTITUDE:
936 #if defined(USE_BARO) || defined(USE_RANGEFINDER)
937 sbufWriteU32(dst, getEstimatedAltitude());
938 #else
939 sbufWriteU32(dst, 0);
940 #endif
941 sbufWriteU16(dst, getEstimatedVario());
942 break;
943
944 case MSP_SONAR_ALTITUDE:
945 #if defined(USE_RANGEFINDER)
946 sbufWriteU32(dst, rangefinderGetLatestAltitude());
947 #else
948 sbufWriteU32(dst, 0);
949 #endif
950 break;
951
952 case MSP_BOARD_ALIGNMENT_CONFIG:
953 sbufWriteU16(dst, boardAlignment()->rollDegrees);
954 sbufWriteU16(dst, boardAlignment()->pitchDegrees);
955 sbufWriteU16(dst, boardAlignment()->yawDegrees);
956 break;
957
958 case MSP_ARMING_CONFIG:
959 sbufWriteU8(dst, armingConfig()->auto_disarm_delay);
960 sbufWriteU8(dst, 0);
961 sbufWriteU8(dst, imuConfig()->small_angle);
962 break;
963
964 case MSP_RC_TUNING:
965 sbufWriteU8(dst, currentControlRateProfile->rcRates[FD_ROLL]);
966 sbufWriteU8(dst, currentControlRateProfile->rcExpo[FD_ROLL]);
967 for (int i = 0 ; i < 3; i++) {
968 sbufWriteU8(dst, currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
969 }
970 sbufWriteU8(dst, currentControlRateProfile->dynThrPID);
971 sbufWriteU8(dst, currentControlRateProfile->thrMid8);
972 sbufWriteU8(dst, currentControlRateProfile->thrExpo8);
973 sbufWriteU16(dst, currentControlRateProfile->tpa_breakpoint);
974 sbufWriteU8(dst, currentControlRateProfile->rcExpo[FD_YAW]);
975 sbufWriteU8(dst, currentControlRateProfile->rcRates[FD_YAW]);
976 sbufWriteU8(dst, currentControlRateProfile->rcRates[FD_PITCH]);
977 sbufWriteU8(dst, currentControlRateProfile->rcExpo[FD_PITCH]);
978 break;
979
980 case MSP_PID:
981 for (int i = 0; i < PID_ITEM_COUNT; i++) {
982 sbufWriteU8(dst, currentPidProfile->pid[i].P);
983 sbufWriteU8(dst, currentPidProfile->pid[i].I);
984 sbufWriteU8(dst, currentPidProfile->pid[i].D);
985 }
986 break;
987
988 case MSP_PIDNAMES:
989 for (const char *c = pidnames; *c; c++) {
990 sbufWriteU8(dst, *c);
991 }
992 break;
993
994 case MSP_PID_CONTROLLER:
995 sbufWriteU8(dst, PID_CONTROLLER_BETAFLIGHT);
996 break;
997
998 case MSP_MODE_RANGES:
999 for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
1000 const modeActivationCondition_t *mac = modeActivationConditions(i);
1001 const box_t *box = findBoxByBoxId(mac->modeId);
1002 sbufWriteU8(dst, box->permanentId);
1003 sbufWriteU8(dst, mac->auxChannelIndex);
1004 sbufWriteU8(dst, mac->range.startStep);
1005 sbufWriteU8(dst, mac->range.endStep);
1006 }
1007 break;
1008
1009 case MSP_ADJUSTMENT_RANGES:
1010 for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
1011 const adjustmentRange_t *adjRange = adjustmentRanges(i);
1012 sbufWriteU8(dst, adjRange->adjustmentIndex);
1013 sbufWriteU8(dst, adjRange->auxChannelIndex);
1014 sbufWriteU8(dst, adjRange->range.startStep);
1015 sbufWriteU8(dst, adjRange->range.endStep);
1016 sbufWriteU8(dst, adjRange->adjustmentFunction);
1017 sbufWriteU8(dst, adjRange->auxSwitchChannelIndex);
1018 }
1019 break;
1020
1021 case MSP_MOTOR_CONFIG:
1022 sbufWriteU16(dst, motorConfig()->minthrottle);
1023 sbufWriteU16(dst, motorConfig()->maxthrottle);
1024 sbufWriteU16(dst, motorConfig()->mincommand);
1025 break;
1026
1027 #ifdef USE_MAG
1028 case MSP_COMPASS_CONFIG:
1029 sbufWriteU16(dst, compassConfig()->mag_declination / 10);
1030 break;
1031 #endif
1032
1033 #if defined(USE_ESC_SENSOR)
1034 case MSP_ESC_SENSOR_DATA:
1035 if (feature(FEATURE_ESC_SENSOR)) {
1036 sbufWriteU8(dst, getMotorCount());
1037 for (int i = 0; i < getMotorCount(); i++) {
1038 const escSensorData_t *escData = getEscSensorData(i);
1039 sbufWriteU8(dst, escData->temperature);
1040 sbufWriteU16(dst, escData->rpm);
1041 }
1042 } else {
1043 unsupportedCommand = true;
1044 }
1045
1046 break;
1047 #endif
1048
1049 #ifdef USE_GPS
1050 case MSP_GPS_CONFIG:
1051 sbufWriteU8(dst, gpsConfig()->provider);
1052 sbufWriteU8(dst, gpsConfig()->sbasMode);
1053 sbufWriteU8(dst, gpsConfig()->autoConfig);
1054 sbufWriteU8(dst, gpsConfig()->autoBaud);
1055 break;
1056
1057 case MSP_RAW_GPS:
1058 sbufWriteU8(dst, STATE(GPS_FIX));
1059 sbufWriteU8(dst, gpsSol.numSat);
1060 sbufWriteU32(dst, gpsSol.llh.lat);
1061 sbufWriteU32(dst, gpsSol.llh.lon);
1062 sbufWriteU16(dst, (uint16_t)constrain(gpsSol.llh.alt / 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.
1063 sbufWriteU16(dst, gpsSol.groundSpeed);
1064 sbufWriteU16(dst, gpsSol.groundCourse);
1065 break;
1066
1067 case MSP_COMP_GPS:
1068 sbufWriteU16(dst, GPS_distanceToHome);
1069 sbufWriteU16(dst, GPS_directionToHome);
1070 sbufWriteU8(dst, GPS_update & 1);
1071 break;
1072
1073 case MSP_GPSSVINFO:
1074 sbufWriteU8(dst, GPS_numCh);
1075 for (int i = 0; i < GPS_numCh; i++) {
1076 sbufWriteU8(dst, GPS_svinfo_chn[i]);
1077 sbufWriteU8(dst, GPS_svinfo_svid[i]);
1078 sbufWriteU8(dst, GPS_svinfo_quality[i]);
1079 sbufWriteU8(dst, GPS_svinfo_cno[i]);
1080 }
1081 break;
1082 #endif
1083
1084 case MSP_ACC_TRIM:
1085 sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.pitch);
1086 sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.roll);
1087 break;
1088
1089 case MSP_MIXER_CONFIG:
1090 sbufWriteU8(dst, mixerConfig()->mixerMode);
1091 sbufWriteU8(dst, mixerConfig()->yaw_motors_reversed);
1092 break;
1093
1094 case MSP_RX_CONFIG:
1095 sbufWriteU8(dst, rxConfig()->serialrx_provider);
1096 sbufWriteU16(dst, rxConfig()->maxcheck);
1097 sbufWriteU16(dst, rxConfig()->midrc);
1098 sbufWriteU16(dst, rxConfig()->mincheck);
1099 sbufWriteU8(dst, rxConfig()->spektrum_sat_bind);
1100 sbufWriteU16(dst, rxConfig()->rx_min_usec);
1101 sbufWriteU16(dst, rxConfig()->rx_max_usec);
1102 sbufWriteU8(dst, rxConfig()->rcInterpolation);
1103 sbufWriteU8(dst, rxConfig()->rcInterpolationInterval);
1104 sbufWriteU16(dst, rxConfig()->airModeActivateThreshold * 10 + 1000);
1105 #ifdef USE_RX_SPI
1106 sbufWriteU8(dst, rxSpiConfig()->rx_spi_protocol);
1107 sbufWriteU32(dst, rxSpiConfig()->rx_spi_id);
1108 sbufWriteU8(dst, rxSpiConfig()->rx_spi_rf_channel_count);
1109 #else
1110 sbufWriteU8(dst, 0);
1111 sbufWriteU32(dst, 0);
1112 sbufWriteU8(dst, 0);
1113 #endif
1114 sbufWriteU8(dst, rxConfig()->fpvCamAngleDegrees);
1115 sbufWriteU8(dst, rxConfig()->rcInterpolationChannels);
1116 #if defined(USE_RC_SMOOTHING_FILTER)
1117 sbufWriteU8(dst, rxConfig()->rc_smoothing_type);
1118 sbufWriteU8(dst, rxConfig()->rc_smoothing_input_cutoff);
1119 sbufWriteU8(dst, rxConfig()->rc_smoothing_derivative_cutoff);
1120 sbufWriteU8(dst, rxConfig()->rc_smoothing_input_type);
1121 sbufWriteU8(dst, rxConfig()->rc_smoothing_derivative_type);
1122 #else
1123 sbufWriteU8(dst, 0);
1124 sbufWriteU8(dst, 0);
1125 sbufWriteU8(dst, 0);
1126 sbufWriteU8(dst, 0);
1127 sbufWriteU8(dst, 0);
1128 #endif
1129
1130 break;
1131 case MSP_FAILSAFE_CONFIG:
1132 sbufWriteU8(dst, failsafeConfig()->failsafe_delay);
1133 sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay);
1134 sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
1135 sbufWriteU8(dst, failsafeConfig()->failsafe_switch_mode);
1136 sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay);
1137 sbufWriteU8(dst, failsafeConfig()->failsafe_procedure);
1138 break;
1139
1140 case MSP_RXFAIL_CONFIG:
1141 for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
1142 sbufWriteU8(dst, rxFailsafeChannelConfigs(i)->mode);
1143 sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxFailsafeChannelConfigs(i)->step));
1144 }
1145 break;
1146
1147 case MSP_RSSI_CONFIG:
1148 sbufWriteU8(dst, rxConfig()->rssi_channel);
1149 break;
1150
1151 case MSP_RX_MAP:
1152 sbufWriteData(dst, rxConfig()->rcmap, RX_MAPPABLE_CHANNEL_COUNT);
1153 break;
1154
1155 case MSP_CF_SERIAL_CONFIG:
1156 for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
1157 if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
1158 continue;
1159 };
1160 sbufWriteU8(dst, serialConfig()->portConfigs[i].identifier);
1161 sbufWriteU16(dst, serialConfig()->portConfigs[i].functionMask);
1162 sbufWriteU8(dst, serialConfig()->portConfigs[i].msp_baudrateIndex);
1163 sbufWriteU8(dst, serialConfig()->portConfigs[i].gps_baudrateIndex);
1164 sbufWriteU8(dst, serialConfig()->portConfigs[i].telemetry_baudrateIndex);
1165 sbufWriteU8(dst, serialConfig()->portConfigs[i].blackbox_baudrateIndex);
1166 }
1167 break;
1168
1169 #ifdef USE_LED_STRIP
1170 case MSP_LED_COLORS:
1171 for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
1172 const hsvColor_t *color = &ledStripConfig()->colors[i];
1173 sbufWriteU16(dst, color->h);
1174 sbufWriteU8(dst, color->s);
1175 sbufWriteU8(dst, color->v);
1176 }
1177 break;
1178
1179 case MSP_LED_STRIP_CONFIG:
1180 for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
1181 const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
1182 sbufWriteU32(dst, *ledConfig);
1183 }
1184 break;
1185
1186 case MSP_LED_STRIP_MODECOLOR:
1187 for (int i = 0; i < LED_MODE_COUNT; i++) {
1188 for (int j = 0; j < LED_DIRECTION_COUNT; j++) {
1189 sbufWriteU8(dst, i);
1190 sbufWriteU8(dst, j);
1191 sbufWriteU8(dst, ledStripConfig()->modeColors[i].color[j]);
1192 }
1193 }
1194
1195 for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) {
1196 sbufWriteU8(dst, LED_MODE_COUNT);
1197 sbufWriteU8(dst, j);
1198 sbufWriteU8(dst, ledStripConfig()->specialColors.color[j]);
1199 }
1200
1201 sbufWriteU8(dst, LED_AUX_CHANNEL);
1202 sbufWriteU8(dst, 0);
1203 sbufWriteU8(dst, ledStripConfig()->ledstrip_aux_channel);
1204 break;
1205 #endif
1206
1207 case MSP_DATAFLASH_SUMMARY:
1208 serializeDataflashSummaryReply(dst);
1209 break;
1210
1211 case MSP_BLACKBOX_CONFIG:
1212 #ifdef USE_BLACKBOX
1213 sbufWriteU8(dst, 1); //Blackbox supported
1214 sbufWriteU8(dst, blackboxConfig()->device);
1215 sbufWriteU8(dst, 1); // Rate numerator, not used anymore
1216 sbufWriteU8(dst, blackboxGetRateDenom());
1217 sbufWriteU16(dst, blackboxConfig()->p_ratio);
1218 #else
1219 sbufWriteU8(dst, 0); // Blackbox not supported
1220 sbufWriteU8(dst, 0);
1221 sbufWriteU8(dst, 0);
1222 sbufWriteU8(dst, 0);
1223 sbufWriteU16(dst, 0);
1224 #endif
1225 break;
1226
1227 case MSP_SDCARD_SUMMARY:
1228 serializeSDCardSummaryReply(dst);
1229 break;
1230
1231 case MSP_MOTOR_3D_CONFIG:
1232 sbufWriteU16(dst, flight3DConfig()->deadband3d_low);
1233 sbufWriteU16(dst, flight3DConfig()->deadband3d_high);
1234 sbufWriteU16(dst, flight3DConfig()->neutral3d);
1235 break;
1236
1237 case MSP_RC_DEADBAND:
1238 sbufWriteU8(dst, rcControlsConfig()->deadband);
1239 sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
1240 sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband);
1241 sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle);
1242 break;
1243
1244 case MSP_SENSOR_ALIGNMENT:
1245 sbufWriteU8(dst, gyroConfig()->gyro_align);
1246 sbufWriteU8(dst, accelerometerConfig()->acc_align);
1247 sbufWriteU8(dst, compassConfig()->mag_align);
1248 break;
1249
1250 case MSP_ADVANCED_CONFIG:
1251 sbufWriteU8(dst, gyroConfig()->gyro_sync_denom);
1252 sbufWriteU8(dst, pidConfig()->pid_process_denom);
1253 sbufWriteU8(dst, motorConfig()->dev.useUnsyncedPwm);
1254 sbufWriteU8(dst, motorConfig()->dev.motorPwmProtocol);
1255 sbufWriteU16(dst, motorConfig()->dev.motorPwmRate);
1256 sbufWriteU16(dst, motorConfig()->digitalIdleOffsetValue);
1257 sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
1258 sbufWriteU8(dst, motorConfig()->dev.motorPwmInversion);
1259 sbufWriteU8(dst, gyroConfig()->gyro_to_use);
1260 sbufWriteU8(dst, gyroConfig()->gyro_high_fsr);
1261 sbufWriteU8(dst, gyroConfig()->gyroMovementCalibrationThreshold);
1262 sbufWriteU16(dst, gyroConfig()->gyroCalibrationDuration);
1263 sbufWriteU16(dst, gyroConfig()->gyro_offset_yaw);
1264 sbufWriteU8(dst, gyroConfig()->checkOverflow);
1265
1266 break;
1267 case MSP_FILTER_CONFIG :
1268 sbufWriteU8(dst, gyroConfig()->gyro_lowpass_hz);
1269 sbufWriteU16(dst, currentPidProfile->dterm_lowpass_hz);
1270 sbufWriteU16(dst, currentPidProfile->yaw_lowpass_hz);
1271 sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1);
1272 sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1);
1273 sbufWriteU16(dst, currentPidProfile->dterm_notch_hz);
1274 sbufWriteU16(dst, currentPidProfile->dterm_notch_cutoff);
1275 sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2);
1276 sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2);
1277 sbufWriteU8(dst, currentPidProfile->dterm_filter_type);
1278 sbufWriteU8(dst, gyroConfig()->gyro_hardware_lpf);
1279 sbufWriteU8(dst, gyroConfig()->gyro_32khz_hardware_lpf);
1280 sbufWriteU16(dst, gyroConfig()->gyro_lowpass_hz);
1281 sbufWriteU16(dst, gyroConfig()->gyro_lowpass2_hz);
1282 sbufWriteU8(dst, gyroConfig()->gyro_lowpass_type);
1283 sbufWriteU8(dst, gyroConfig()->gyro_lowpass2_type);
1284 sbufWriteU16(dst, currentPidProfile->dterm_lowpass2_hz);
1285
1286 break;
1287 case MSP_PID_ADVANCED:
1288 sbufWriteU16(dst, 0);
1289 sbufWriteU16(dst, 0);
1290 sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit
1291 sbufWriteU8(dst, 0); // reserved
1292 sbufWriteU8(dst, currentPidProfile->vbatPidCompensation);
1293 sbufWriteU8(dst, currentPidProfile->setpointRelaxRatio);
1294 sbufWriteU8(dst, MIN(currentPidProfile->dtermSetpointWeight, 255));
1295 sbufWriteU8(dst, 0); // reserved
1296 sbufWriteU8(dst, 0); // reserved
1297 sbufWriteU8(dst, 0); // reserved
1298 sbufWriteU16(dst, currentPidProfile->rateAccelLimit);
1299 sbufWriteU16(dst, currentPidProfile->yawRateAccelLimit);
1300 sbufWriteU8(dst, currentPidProfile->levelAngleLimit);
1301 sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity
1302 sbufWriteU16(dst, currentPidProfile->itermThrottleThreshold);
1303 sbufWriteU16(dst, currentPidProfile->itermAcceleratorGain);
1304 sbufWriteU16(dst, currentPidProfile->dtermSetpointWeight);
1305 sbufWriteU8(dst, currentPidProfile->smart_feedforward);
1306 sbufWriteU8(dst, currentPidProfile->throttle_boost);
1307 sbufWriteU8(dst, currentPidProfile->throttle_boost_cutoff);
1308 break;
1309
1310 case MSP_SENSOR_CONFIG:
1311 sbufWriteU8(dst, accelerometerConfig()->acc_hardware);
1312 sbufWriteU8(dst, barometerConfig()->baro_hardware);
1313 sbufWriteU8(dst, compassConfig()->mag_hardware);
1314 break;
1315
1316 #if defined(USE_VTX_COMMON)
1317 case MSP_VTX_CONFIG:
1318 {
1319 const vtxDevice_t *vtxDevice = vtxCommonDevice();
1320 if (vtxDevice) {
1321 uint8_t pitmode=0;
1322 vtxCommonGetPitMode(vtxDevice, &pitmode);
1323 sbufWriteU8(dst, vtxCommonGetDeviceType(vtxDevice));
1324 sbufWriteU8(dst, vtxSettingsConfig()->band);
1325 sbufWriteU8(dst, vtxSettingsConfig()->channel);
1326 sbufWriteU8(dst, vtxSettingsConfig()->power);
1327 sbufWriteU8(dst, pitmode);
1328 sbufWriteU16(dst, vtxSettingsConfig()->freq);
1329 // future extensions here...
1330 } else {
1331 sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX detected
1332 }
1333 }
1334
1335 break;
1336 #endif
1337
1338 case MSP_TX_INFO:
1339 sbufWriteU8(dst, rssiSource);
1340 uint8_t rtcDateTimeIsSet = 0;
1341 #ifdef USE_RTC_TIME
1342 dateTime_t dt;
1343 if (rtcGetDateTime(&dt)) {
1344 rtcDateTimeIsSet = 1;
1345 }
1346 #else
1347 rtcDateTimeIsSet = RTC_NOT_SUPPORTED;
1348 #endif
1349 sbufWriteU8(dst, rtcDateTimeIsSet);
1350
1351 break;
1352 #ifdef USE_RTC_TIME
1353 case MSP_RTC:
1354 {
1355 dateTime_t dt;
1356 if (rtcGetDateTime(&dt)) {
1357 sbufWriteU16(dst, dt.year);
1358 sbufWriteU8(dst, dt.month);
1359 sbufWriteU8(dst, dt.day);
1360 sbufWriteU8(dst, dt.hours);
1361 sbufWriteU8(dst, dt.minutes);
1362 sbufWriteU8(dst, dt.seconds);
1363 sbufWriteU16(dst, dt.millis);
1364 }
1365 }
1366
1367 break;
1368 #endif
1369 default:
1370 unsupportedCommand = true;
1371 }
1372 return !unsupportedCommand;
1373 }
1374 #endif // USE_OSD_SLAVE
1375
1376 static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
1377 {
1378 #if defined(USE_OSD_SLAVE)
1379 UNUSED(dst);
1380 #endif
1381
1382 switch (cmdMSP) {
1383 #if !defined(USE_OSD_SLAVE)
1384 case MSP_BOXNAMES:
1385 {
1386 const int page = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
1387 serializeBoxReply(dst, page, &serializeBoxNameFn);
1388 }
1389 break;
1390 case MSP_BOXIDS:
1391 {
1392 const int page = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
1393 serializeBoxReply(dst, page, &serializeBoxPermanentIdFn);
1394 }
1395 break;
1396 #endif
1397 case MSP_REBOOT:
1398 if (sbufBytesRemaining(src)) {
1399 rebootMode = sbufReadU8(src);
1400
1401 if (rebootMode >= MSP_REBOOT_COUNT
1402 #if !defined(USE_USB_MSC)
1403 || rebootMode == MSP_REBOOT_MSC
1404 #endif
1405 ) {
1406 return MSP_RESULT_ERROR;
1407 }
1408 } else {
1409 rebootMode = MSP_REBOOT_FIRMWARE;
1410 }
1411
1412 sbufWriteU8(dst, rebootMode);
1413
1414 #if defined(USE_USB_MSC)
1415 if (rebootMode == MSP_REBOOT_MSC) {
1416 if (mscCheckFilesystemReady()) {
1417 sbufWriteU8(dst, 1);
1418 } else {
1419 sbufWriteU8(dst, 0);
1420
1421 return MSP_RESULT_ACK;
1422 }
1423 }
1424 #endif
1425
1426 if (mspPostProcessFn) {
1427 *mspPostProcessFn = mspRebootFn;
1428 }
1429
1430 break;
1431 default:
1432 return MSP_RESULT_CMD_UNKNOWN;
1433 }
1434 return MSP_RESULT_ACK;
1435 }
1436
1437 #ifdef USE_FLASHFS
1438 static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
1439 {
1440 const unsigned int dataSize = sbufBytesRemaining(src);
1441 const uint32_t readAddress = sbufReadU32(src);
1442 uint16_t readLength;
1443 bool allowCompression = false;
1444 bool useLegacyFormat;
1445 if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
1446 readLength = sbufReadU16(src);
1447 if (sbufBytesRemaining(src)) {
1448 allowCompression = sbufReadU8(src);
1449 }
1450 useLegacyFormat = false;
1451 } else {
1452 readLength = 128;
1453 useLegacyFormat = true;
1454 }
1455
1456 serializeDataflashReadReply(dst, readAddress, readLength, useLegacyFormat, allowCompression);
1457 }
1458 #endif
1459
1460 #ifdef USE_OSD_SLAVE
1461 static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
1462 {
1463 UNUSED(cmdMSP);
1464 UNUSED(src);
1465
1466 switch(cmdMSP) {
1467 case MSP_RESET_CONF:
1468 resetEEPROM();
1469 readEEPROM();
1470 break;
1471 case MSP_EEPROM_WRITE:
1472 writeEEPROM();
1473 readEEPROM();
1474 break;
1475 default:
1476 // we do not know how to handle the (valid) message, indicate error MSP $M!
1477 return MSP_RESULT_ERROR;
1478 }
1479 return MSP_RESULT_ACK;
1480 }
1481
1482 #else
1483
1484 static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
1485 {
1486 uint32_t i;
1487 uint8_t value;
1488 const unsigned int dataSize = sbufBytesRemaining(src);
1489 switch (cmdMSP) {
1490 case MSP_SELECT_SETTING:
1491 value = sbufReadU8(src);
1492 if ((value & RATEPROFILE_MASK) == 0) {
1493 if (!ARMING_FLAG(ARMED)) {
1494 if (value >= MAX_PROFILE_COUNT) {
1495 value = 0;
1496 }
1497 changePidProfile(value);
1498 }
1499 } else {
1500 value = value & ~RATEPROFILE_MASK;
1501
1502 if (value >= CONTROL_RATE_PROFILE_COUNT) {
1503 value = 0;
1504 }
1505 changeControlRateProfile(value);
1506 }
1507 break;
1508
1509 case MSP_COPY_PROFILE:
1510 value = sbufReadU8(src); // 0 = pid profile, 1 = control rate profile
1511 uint8_t dstProfileIndex = sbufReadU8(src);
1512 uint8_t srcProfileIndex = sbufReadU8(src);
1513 if (value == 0) {
1514 pidCopyProfile(dstProfileIndex, srcProfileIndex);
1515 }
1516 else if (value == 1) {
1517 copyControlRateProfile(dstProfileIndex, srcProfileIndex);
1518 }
1519 break;
1520
1521 #if defined(USE_GPS) || defined(USE_MAG)
1522 case MSP_SET_HEADING:
1523 magHold = sbufReadU16(src);
1524 break;
1525 #endif
1526
1527 case MSP_SET_RAW_RC:
1528 #ifdef USE_RX_MSP
1529 {
1530 uint8_t channelCount = dataSize / sizeof(uint16_t);
1531 if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) {
1532 return MSP_RESULT_ERROR;
1533 } else {
1534 uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
1535 for (int i = 0; i < channelCount; i++) {
1536 frame[i] = sbufReadU16(src);
1537 }
1538 rxMspFrameReceive(frame, channelCount);
1539 }
1540 }
1541 #endif
1542 break;
1543 case MSP_SET_ACC_TRIM:
1544 accelerometerConfigMutable()->accelerometerTrims.values.pitch = sbufReadU16(src);
1545 accelerometerConfigMutable()->accelerometerTrims.values.roll = sbufReadU16(src);
1546 break;
1547 case MSP_SET_ARMING_CONFIG:
1548 armingConfigMutable()->auto_disarm_delay = sbufReadU8(src);
1549 sbufReadU8(src); // reserved
1550 if (sbufBytesRemaining(src)) {
1551 imuConfigMutable()->small_angle = sbufReadU8(src);
1552 }
1553 break;
1554
1555 case MSP_SET_PID_CONTROLLER:
1556 break;
1557
1558 case MSP_SET_PID:
1559 for (int i = 0; i < PID_ITEM_COUNT; i++) {
1560 currentPidProfile->pid[i].P = sbufReadU8(src);
1561 currentPidProfile->pid[i].I = sbufReadU8(src);
1562 currentPidProfile->pid[i].D = sbufReadU8(src);
1563 }
1564 pidInitConfig(currentPidProfile);
1565 break;
1566
1567 case MSP_SET_MODE_RANGE:
1568 i = sbufReadU8(src);
1569 if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
1570 modeActivationCondition_t *mac = modeActivationConditionsMutable(i);
1571 i = sbufReadU8(src);
1572 const box_t *box = findBoxByPermanentId(i);
1573 if (box) {
1574 mac->modeId = box->boxId;
1575 mac->auxChannelIndex = sbufReadU8(src);
1576 mac->range.startStep = sbufReadU8(src);
1577 mac->range.endStep = sbufReadU8(src);
1578
1579 useRcControlsConfig(currentPidProfile);
1580 } else {
1581 return MSP_RESULT_ERROR;
1582 }
1583 } else {
1584 return MSP_RESULT_ERROR;
1585 }
1586 break;
1587
1588 case MSP_SET_ADJUSTMENT_RANGE:
1589 i = sbufReadU8(src);
1590 if (i < MAX_ADJUSTMENT_RANGE_COUNT) {
1591 adjustmentRange_t *adjRange = adjustmentRangesMutable(i);
1592 i = sbufReadU8(src);
1593 if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) {
1594 adjRange->adjustmentIndex = i;
1595 adjRange->auxChannelIndex = sbufReadU8(src);
1596 adjRange->range.startStep = sbufReadU8(src);
1597 adjRange->range.endStep = sbufReadU8(src);
1598 adjRange->adjustmentFunction = sbufReadU8(src);
1599 adjRange->auxSwitchChannelIndex = sbufReadU8(src);
1600 } else {
1601 return MSP_RESULT_ERROR;
1602 }
1603 } else {
1604 return MSP_RESULT_ERROR;
1605 }
1606 break;
1607
1608 case MSP_SET_RC_TUNING:
1609 if (sbufBytesRemaining(src) >= 10) {
1610 value = sbufReadU8(src);
1611 if (currentControlRateProfile->rcRates[FD_PITCH] == currentControlRateProfile->rcRates[FD_ROLL]) {
1612 currentControlRateProfile->rcRates[FD_PITCH] = value;
1613 }
1614 currentControlRateProfile->rcRates[FD_ROLL] = value;
1615
1616 value = sbufReadU8(src);
1617 if (currentControlRateProfile->rcExpo[FD_PITCH] == currentControlRateProfile->rcExpo[FD_ROLL]) {
1618 currentControlRateProfile->rcExpo[FD_PITCH] = value;
1619 }
1620 currentControlRateProfile->rcExpo[FD_ROLL] = value;
1621
1622 for (int i = 0; i < 3; i++) {
1623 currentControlRateProfile->rates[i] = sbufReadU8(src);
1624 }
1625
1626 value = sbufReadU8(src);
1627 currentControlRateProfile->dynThrPID = MIN(value, CONTROL_RATE_CONFIG_TPA_MAX);
1628 currentControlRateProfile->thrMid8 = sbufReadU8(src);
1629 currentControlRateProfile->thrExpo8 = sbufReadU8(src);
1630 currentControlRateProfile->tpa_breakpoint = sbufReadU16(src);
1631
1632 if (sbufBytesRemaining(src) >= 1) {
1633 currentControlRateProfile->rcExpo[FD_YAW] = sbufReadU8(src);
1634 }
1635
1636 if (sbufBytesRemaining(src) >= 1) {
1637 currentControlRateProfile->rcRates[FD_YAW] = sbufReadU8(src);
1638 }
1639
1640 if (sbufBytesRemaining(src) >= 1) {
1641 currentControlRateProfile->rcRates[FD_PITCH] = sbufReadU8(src);
1642 }
1643
1644 if (sbufBytesRemaining(src) >= 1) {
1645 currentControlRateProfile->rcExpo[FD_PITCH] = sbufReadU8(src);
1646 }
1647
1648 initRcProcessing();
1649 } else {
1650 return MSP_RESULT_ERROR;
1651 }
1652 break;
1653
1654 case MSP_SET_MOTOR_CONFIG:
1655 motorConfigMutable()->minthrottle = sbufReadU16(src);
1656 motorConfigMutable()->maxthrottle = sbufReadU16(src);
1657 motorConfigMutable()->mincommand = sbufReadU16(src);
1658 break;
1659
1660 #ifdef USE_GPS
1661 case MSP_SET_GPS_CONFIG:
1662 gpsConfigMutable()->provider = sbufReadU8(src);
1663 gpsConfigMutable()->sbasMode = sbufReadU8(src);
1664 gpsConfigMutable()->autoConfig = sbufReadU8(src);
1665 gpsConfigMutable()->autoBaud = sbufReadU8(src);
1666 break;
1667 #endif
1668
1669 #ifdef USE_MAG
1670 case MSP_SET_COMPASS_CONFIG:
1671 compassConfigMutable()->mag_declination = sbufReadU16(src) * 10;
1672 break;
1673 #endif
1674
1675 case MSP_SET_MOTOR:
1676 for (int i = 0; i < getMotorCount(); i++) {
1677 motor_disarmed[i] = convertExternalToMotor(sbufReadU16(src));
1678 }
1679 break;
1680
1681 case MSP_SET_SERVO_CONFIGURATION:
1682 #ifdef USE_SERVOS
1683 if (dataSize != 1 + 12) {
1684 return MSP_RESULT_ERROR;
1685 }
1686 i = sbufReadU8(src);
1687 if (i >= MAX_SUPPORTED_SERVOS) {
1688 return MSP_RESULT_ERROR;
1689 } else {
1690 servoParamsMutable(i)->min = sbufReadU16(src);
1691 servoParamsMutable(i)->max = sbufReadU16(src);
1692 servoParamsMutable(i)->middle = sbufReadU16(src);
1693 servoParamsMutable(i)->rate = sbufReadU8(src);
1694 servoParamsMutable(i)->forwardFromChannel = sbufReadU8(src);
1695 servoParamsMutable(i)->reversedSources = sbufReadU32(src);
1696 }
1697 #endif
1698 break;
1699
1700 case MSP_SET_SERVO_MIX_RULE:
1701 #ifdef USE_SERVOS
1702 i = sbufReadU8(src);
1703 if (i >= MAX_SERVO_RULES) {
1704 return MSP_RESULT_ERROR;
1705 } else {
1706 customServoMixersMutable(i)->targetChannel = sbufReadU8(src);
1707 customServoMixersMutable(i)->inputSource = sbufReadU8(src);
1708 customServoMixersMutable(i)->rate = sbufReadU8(src);
1709 customServoMixersMutable(i)->speed = sbufReadU8(src);
1710 customServoMixersMutable(i)->min = sbufReadU8(src);
1711 customServoMixersMutable(i)->max = sbufReadU8(src);
1712 customServoMixersMutable(i)->box = sbufReadU8(src);
1713 loadCustomServoMixer();
1714 }
1715 #endif
1716 break;
1717
1718 case MSP_SET_MOTOR_3D_CONFIG:
1719 flight3DConfigMutable()->deadband3d_low = sbufReadU16(src);
1720 flight3DConfigMutable()->deadband3d_high = sbufReadU16(src);
1721 flight3DConfigMutable()->neutral3d = sbufReadU16(src);
1722 break;
1723
1724 case MSP_SET_RC_DEADBAND:
1725 rcControlsConfigMutable()->deadband = sbufReadU8(src);
1726 rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
1727 rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src);
1728 flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src);
1729 break;
1730
1731 case MSP_SET_RESET_CURR_PID:
1732 resetPidProfile(currentPidProfile);
1733 break;
1734 case MSP_SET_SENSOR_ALIGNMENT:
1735 gyroConfigMutable()->gyro_align = sbufReadU8(src);
1736 accelerometerConfigMutable()->acc_align = sbufReadU8(src);
1737 compassConfigMutable()->mag_align = sbufReadU8(src);
1738 break;
1739
1740 case MSP_SET_ADVANCED_CONFIG:
1741 gyroConfigMutable()->gyro_sync_denom = sbufReadU8(src);
1742 pidConfigMutable()->pid_process_denom = sbufReadU8(src);
1743 motorConfigMutable()->dev.useUnsyncedPwm = sbufReadU8(src);
1744 #ifdef USE_DSHOT
1745 motorConfigMutable()->dev.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
1746 #else
1747 motorConfigMutable()->dev.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
1748 #endif
1749 motorConfigMutable()->dev.motorPwmRate = sbufReadU16(src);
1750 if (sbufBytesRemaining(src) >= 2) {
1751 motorConfigMutable()->digitalIdleOffsetValue = sbufReadU16(src);
1752 }
1753 if (sbufBytesRemaining(src)) {
1754 gyroConfigMutable()->gyro_use_32khz = sbufReadU8(src);
1755 }
1756 if (sbufBytesRemaining(src)) {
1757 motorConfigMutable()->dev.motorPwmInversion = sbufReadU8(src);
1758 }
1759 if (sbufBytesRemaining(src) >= 8) {
1760 gyroConfigMutable()->gyro_to_use = sbufReadU8(src);
1761 gyroConfigMutable()->gyro_high_fsr = sbufReadU8(src);
1762 gyroConfigMutable()->gyroMovementCalibrationThreshold = sbufReadU8(src);
1763 gyroConfigMutable()->gyroCalibrationDuration = sbufReadU16(src);
1764 gyroConfigMutable()->gyro_offset_yaw = sbufReadU16(src);
1765 gyroConfigMutable()->checkOverflow = sbufReadU8(src);
1766 }
1767
1768 validateAndFixGyroConfig();
1769
1770 break;
1771 case MSP_SET_FILTER_CONFIG:
1772 gyroConfigMutable()->gyro_lowpass_hz = sbufReadU8(src);
1773 currentPidProfile->dterm_lowpass_hz = sbufReadU16(src);
1774 currentPidProfile->yaw_lowpass_hz = sbufReadU16(src);
1775 if (sbufBytesRemaining(src) >= 8) {
1776 gyroConfigMutable()->gyro_soft_notch_hz_1 = sbufReadU16(src);
1777 gyroConfigMutable()->gyro_soft_notch_cutoff_1 = sbufReadU16(src);
1778 currentPidProfile->dterm_notch_hz = sbufReadU16(src);
1779 currentPidProfile->dterm_notch_cutoff = sbufReadU16(src);
1780 }
1781 if (sbufBytesRemaining(src) >= 4) {
1782 gyroConfigMutable()->gyro_soft_notch_hz_2 = sbufReadU16(src);
1783 gyroConfigMutable()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
1784 }
1785 if (sbufBytesRemaining(src) >= 1) {
1786 currentPidProfile->dterm_filter_type = sbufReadU8(src);
1787 }
1788 if (sbufBytesRemaining(src) >= 10) {
1789 gyroConfigMutable()->gyro_hardware_lpf = sbufReadU8(src);
1790 gyroConfigMutable()->gyro_32khz_hardware_lpf = sbufReadU8(src);
1791 gyroConfigMutable()->gyro_lowpass_hz = sbufReadU16(src);
1792 gyroConfigMutable()->gyro_lowpass2_hz = sbufReadU16(src);
1793 gyroConfigMutable()->gyro_lowpass_type = sbufReadU8(src);
1794 gyroConfigMutable()->gyro_lowpass2_type = sbufReadU8(src);
1795 currentPidProfile->dterm_lowpass2_hz = sbufReadU16(src);
1796 }
1797 // reinitialize the gyro filters with the new values
1798 validateAndFixGyroConfig();
1799 gyroInitFilters();
1800 // reinitialize the PID filters with the new values
1801 pidInitFilters(currentPidProfile);
1802
1803 break;
1804 case MSP_SET_PID_ADVANCED:
1805 sbufReadU16(src);
1806 sbufReadU16(src);
1807 sbufReadU16(src); // was pidProfile.yaw_p_limit
1808 sbufReadU8(src); // reserved
1809 currentPidProfile->vbatPidCompensation = sbufReadU8(src);
1810 currentPidProfile->setpointRelaxRatio = sbufReadU8(src);
1811 currentPidProfile->dtermSetpointWeight = sbufReadU8(src);
1812 sbufReadU8(src); // reserved
1813 sbufReadU8(src); // reserved
1814 sbufReadU8(src); // reserved
1815 currentPidProfile->rateAccelLimit = sbufReadU16(src);
1816 currentPidProfile->yawRateAccelLimit = sbufReadU16(src);
1817 if (sbufBytesRemaining(src) >= 2) {
1818 currentPidProfile->levelAngleLimit = sbufReadU8(src);
1819 sbufReadU8(src); // was pidProfile.levelSensitivity
1820 }
1821 if (sbufBytesRemaining(src) >= 4) {
1822 currentPidProfile->itermThrottleThreshold = sbufReadU16(src);
1823 currentPidProfile->itermAcceleratorGain = sbufReadU16(src);
1824 }
1825 if (sbufBytesRemaining(src) >= 2) {
1826 currentPidProfile->dtermSetpointWeight = sbufReadU16(src);
1827 }
1828 if (sbufBytesRemaining(src) >= 3) {
1829 currentPidProfile->smart_feedforward = sbufReadU8(src);
1830 currentPidProfile->throttle_boost = sbufReadU8(src);
1831 currentPidProfile->throttle_boost_cutoff = sbufReadU8(src);
1832 }
1833 pidInitConfig(currentPidProfile);
1834 break;
1835
1836 case MSP_SET_SENSOR_CONFIG:
1837 accelerometerConfigMutable()->acc_hardware = sbufReadU8(src);
1838 barometerConfigMutable()->baro_hardware = sbufReadU8(src);
1839 compassConfigMutable()->mag_hardware = sbufReadU8(src);
1840 break;
1841
1842 case MSP_RESET_CONF:
1843 if (!ARMING_FLAG(ARMED)) {
1844 resetEEPROM();
1845 readEEPROM();
1846 }
1847 break;
1848
1849 case MSP_ACC_CALIBRATION:
1850 if (!ARMING_FLAG(ARMED))
1851 accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
1852 break;
1853
1854 case MSP_MAG_CALIBRATION:
1855 if (!ARMING_FLAG(ARMED))
1856 ENABLE_STATE(CALIBRATE_MAG);
1857 break;
1858
1859 case MSP_EEPROM_WRITE:
1860 if (ARMING_FLAG(ARMED)) {
1861 return MSP_RESULT_ERROR;
1862 }
1863 writeEEPROM();
1864 readEEPROM();
1865 break;
1866
1867 #ifdef USE_BLACKBOX
1868 case MSP_SET_BLACKBOX_CONFIG:
1869 // Don't allow config to be updated while Blackbox is logging
1870 if (blackboxMayEditConfig()) {
1871 blackboxConfigMutable()->device = sbufReadU8(src);
1872 const int rateNum = sbufReadU8(src); // was rate_num
1873 const int rateDenom = sbufReadU8(src); // was rate_denom
1874 if (sbufBytesRemaining(src) >= 2) {
1875 // p_ratio specified, so use it directly
1876 blackboxConfigMutable()->p_ratio = sbufReadU16(src);
1877 } else {
1878 // p_ratio not specified in MSP, so calculate it from old rateNum and rateDenom
1879 blackboxConfigMutable()->p_ratio = blackboxCalculatePDenom(rateNum, rateDenom);
1880 }
1881 }
1882 break;
1883 #endif
1884
1885 #ifdef USE_VTX_COMMON
1886 case MSP_SET_VTX_CONFIG:
1887 {
1888 vtxDevice_t *vtxDevice = vtxCommonDevice();
1889 if (vtxDevice) {
1890 if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) {
1891 uint16_t newFrequency = sbufReadU16(src);
1892 if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { //value is band and channel
1893 const uint8_t newBand = (newFrequency / 8) + 1;
1894 const uint8_t newChannel = (newFrequency % 8) + 1;
1895 vtxSettingsConfigMutable()->band = newBand;
1896 vtxSettingsConfigMutable()->channel = newChannel;
1897 vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(newBand, newChannel);
1898 } else { //value is frequency in MHz
1899 vtxSettingsConfigMutable()->band = 0;
1900 vtxSettingsConfigMutable()->freq = newFrequency;
1901 }
1902
1903 if (sbufBytesRemaining(src) > 1) {
1904 vtxSettingsConfigMutable()->power = sbufReadU8(src);
1905 // Delegate pitmode to vtx directly
1906 const uint8_t newPitmode = sbufReadU8(src);
1907 uint8_t currentPitmode = 0;
1908 vtxCommonGetPitMode(vtxDevice, &currentPitmode);
1909 if (currentPitmode != newPitmode) {
1910 vtxCommonSetPitMode(vtxDevice, newPitmode);
1911 }
1912 }
1913 }
1914 }
1915 }
1916 break;
1917 #endif
1918
1919 #ifdef USE_CAMERA_CONTROL
1920 case MSP_CAMERA_CONTROL:
1921 {
1922 if (ARMING_FLAG(ARMED)) {
1923 return MSP_RESULT_ERROR;
1924 }
1925
1926 const uint8_t key = sbufReadU8(src);
1927 cameraControlKeyPress(key, 0);
1928 }
1929 break;
1930 #endif
1931
1932 case MSP_SET_ARMING_DISABLED:
1933 {
1934 const uint8_t command = sbufReadU8(src);
1935 uint8_t disableRunawayTakeoff = 0;
1936 #ifndef USE_RUNAWAY_TAKEOFF
1937 UNUSED(disableRunawayTakeoff);
1938 #endif
1939 if (sbufBytesRemaining(src)) {
1940 disableRunawayTakeoff = sbufReadU8(src);
1941 }
1942 if (command) {
1943 setArmingDisabled(ARMING_DISABLED_MSP);
1944 if (ARMING_FLAG(ARMED)) {
1945 disarm();
1946 }
1947 #ifdef USE_RUNAWAY_TAKEOFF
1948 runawayTakeoffTemporaryDisable(false);
1949 #endif
1950 } else {
1951 unsetArmingDisabled(ARMING_DISABLED_MSP);
1952 #ifdef USE_RUNAWAY_TAKEOFF
1953 runawayTakeoffTemporaryDisable(disableRunawayTakeoff);
1954 #endif
1955 }
1956 }
1957 break;
1958
1959 #ifdef USE_FLASHFS
1960 case MSP_DATAFLASH_ERASE:
1961 flashfsEraseCompletely();
1962 break;
1963 #endif
1964
1965 #ifdef USE_GPS
1966 case MSP_SET_RAW_GPS:
1967 if (sbufReadU8(src)) {
1968 ENABLE_STATE(GPS_FIX);
1969 } else {
1970 DISABLE_STATE(GPS_FIX);
1971 }
1972 gpsSol.numSat = sbufReadU8(src);
1973 gpsSol.llh.lat = sbufReadU32(src);
1974 gpsSol.llh.lon = sbufReadU32(src);
1975 gpsSol.llh.alt = 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.
1976 gpsSol.groundSpeed = sbufReadU16(src);
1977 GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers
1978 break;
1979 #endif // USE_GPS
1980 case MSP_SET_FEATURE_CONFIG:
1981 featureClearAll();
1982 featureSet(sbufReadU32(src)); // features bitmap
1983 break;
1984
1985 #ifdef USE_BEEPER
1986 case MSP_SET_BEEPER_CONFIG:
1987 beeperConfigMutable()->beeper_off_flags = sbufReadU32(src);
1988 if (sbufBytesRemaining(src) >= 1) {
1989 beeperConfigMutable()->dshotBeaconTone = sbufReadU8(src);
1990 }
1991 if (sbufBytesRemaining(src) >= 4) {
1992 beeperConfigMutable()->dshotBeaconOffFlags = sbufReadU32(src);
1993 }
1994 break;
1995 #endif
1996
1997 case MSP_SET_BOARD_ALIGNMENT_CONFIG:
1998 boardAlignmentMutable()->rollDegrees = sbufReadU16(src);
1999 boardAlignmentMutable()->pitchDegrees = sbufReadU16(src);
2000 boardAlignmentMutable()->yawDegrees = sbufReadU16(src);
2001 break;
2002
2003 case MSP_SET_MIXER_CONFIG:
2004 #ifndef USE_QUAD_MIXER_ONLY
2005 mixerConfigMutable()->mixerMode = sbufReadU8(src);
2006 #else
2007 sbufReadU8(src);
2008 #endif
2009 if (sbufBytesRemaining(src) >= 1) {
2010 mixerConfigMutable()->yaw_motors_reversed = sbufReadU8(src);
2011 }
2012 break;
2013
2014 case MSP_SET_RX_CONFIG:
2015 rxConfigMutable()->serialrx_provider = sbufReadU8(src);
2016 rxConfigMutable()->maxcheck = sbufReadU16(src);
2017 rxConfigMutable()->midrc = sbufReadU16(src);
2018 rxConfigMutable()->mincheck = sbufReadU16(src);
2019 rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src);
2020 if (sbufBytesRemaining(src) >= 4) {
2021 rxConfigMutable()->rx_min_usec = sbufReadU16(src);
2022 rxConfigMutable()->rx_max_usec = sbufReadU16(src);
2023 }
2024 if (sbufBytesRemaining(src) >= 4) {
2025 rxConfigMutable()->rcInterpolation = sbufReadU8(src);
2026 rxConfigMutable()->rcInterpolationInterval = sbufReadU8(src);
2027 rxConfigMutable()->airModeActivateThreshold = (sbufReadU16(src) - 1000) / 10;
2028 }
2029 if (sbufBytesRemaining(src) >= 6) {
2030 #ifdef USE_RX_SPI
2031 rxSpiConfigMutable()->rx_spi_protocol = sbufReadU8(src);
2032 rxSpiConfigMutable()->rx_spi_id = sbufReadU32(src);
2033 rxSpiConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(src);
2034 #else
2035 sbufReadU8(src);
2036 sbufReadU32(src);
2037 sbufReadU8(src);
2038 #endif
2039 }
2040 if (sbufBytesRemaining(src) >= 1) {
2041 rxConfigMutable()->fpvCamAngleDegrees = sbufReadU8(src);
2042 }
2043 if (sbufBytesRemaining(src) >= 6) {
2044 // Added in MSP API 1.40
2045 rxConfigMutable()->rcInterpolationChannels = sbufReadU8(src);
2046 #if defined(USE_RC_SMOOTHING_FILTER)
2047 rxConfigMutable()->rc_smoothing_type = sbufReadU8(src);
2048 rxConfigMutable()->rc_smoothing_input_cutoff = sbufReadU8(src);
2049 rxConfigMutable()->rc_smoothing_derivative_cutoff = sbufReadU8(src);
2050 rxConfigMutable()->rc_smoothing_input_type = sbufReadU8(src);
2051 rxConfigMutable()->rc_smoothing_derivative_type = sbufReadU8(src);
2052 #else
2053 sbufReadU8(src);
2054 sbufReadU8(src);
2055 sbufReadU8(src);
2056 sbufReadU8(src);
2057 sbufReadU8(src);
2058 #endif
2059 }
2060
2061 break;
2062 case MSP_SET_FAILSAFE_CONFIG:
2063 failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
2064 failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src);
2065 failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src);
2066 failsafeConfigMutable()->failsafe_switch_mode = sbufReadU8(src);
2067 failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src);
2068 failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src);
2069 break;
2070
2071 case MSP_SET_RXFAIL_CONFIG:
2072 i = sbufReadU8(src);
2073 if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) {
2074 rxFailsafeChannelConfigsMutable(i)->mode = sbufReadU8(src);
2075 rxFailsafeChannelConfigsMutable(i)->step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
2076 } else {
2077 return MSP_RESULT_ERROR;
2078 }
2079 break;
2080
2081 case MSP_SET_RSSI_CONFIG:
2082 rxConfigMutable()->rssi_channel = sbufReadU8(src);
2083 break;
2084
2085 case MSP_SET_RX_MAP:
2086 for (int i = 0; i < RX_MAPPABLE_CHANNEL_COUNT; i++) {
2087 rxConfigMutable()->rcmap[i] = sbufReadU8(src);
2088 }
2089 break;
2090
2091 case MSP_SET_CF_SERIAL_CONFIG:
2092 {
2093 uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4);
2094
2095 if (dataSize % portConfigSize != 0) {
2096 return MSP_RESULT_ERROR;
2097 }
2098
2099 uint8_t remainingPortsInPacket = dataSize / portConfigSize;
2100
2101 while (remainingPortsInPacket--) {
2102 uint8_t identifier = sbufReadU8(src);
2103
2104 serialPortConfig_t *portConfig = serialFindPortConfiguration(identifier);
2105 if (!portConfig) {
2106 return MSP_RESULT_ERROR;
2107 }
2108
2109 portConfig->identifier = identifier;
2110 portConfig->functionMask = sbufReadU16(src);
2111 portConfig->msp_baudrateIndex = sbufReadU8(src);
2112 portConfig->gps_baudrateIndex = sbufReadU8(src);
2113 portConfig->telemetry_baudrateIndex = sbufReadU8(src);
2114 portConfig->blackbox_baudrateIndex = sbufReadU8(src);
2115 }
2116 }
2117 break;
2118
2119 #ifdef USE_LED_STRIP
2120 case MSP_SET_LED_COLORS:
2121 for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
2122 hsvColor_t *color = &ledStripConfigMutable()->colors[i];
2123 color->h = sbufReadU16(src);
2124 color->s = sbufReadU8(src);
2125 color->v = sbufReadU8(src);
2126 }
2127 break;
2128
2129 case MSP_SET_LED_STRIP_CONFIG:
2130 {
2131 i = sbufReadU8(src);
2132 if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) {
2133 return MSP_RESULT_ERROR;
2134 }
2135 ledConfig_t *ledConfig = &ledStripConfigMutable()->ledConfigs[i];
2136 *ledConfig = sbufReadU32(src);
2137 reevaluateLedConfig();
2138 }
2139 break;
2140
2141 case MSP_SET_LED_STRIP_MODECOLOR:
2142 {
2143 ledModeIndex_e modeIdx = sbufReadU8(src);
2144 int funIdx = sbufReadU8(src);
2145 int color = sbufReadU8(src);
2146
2147 if (!setModeColor(modeIdx, funIdx, color))
2148 return MSP_RESULT_ERROR;
2149 }
2150 break;
2151 #endif
2152
2153 case MSP_SET_NAME:
2154 memset(pilotConfigMutable()->name, 0, ARRAYLEN(pilotConfig()->name));
2155 for (unsigned int i = 0; i < MIN(MAX_NAME_LENGTH, dataSize); i++) {
2156 pilotConfigMutable()->name[i] = sbufReadU8(src);
2157 }
2158 break;
2159
2160 #ifdef USE_RTC_TIME
2161 case MSP_SET_RTC:
2162 {
2163 dateTime_t dt;
2164 dt.year = sbufReadU16(src);
2165 dt.month = sbufReadU8(src);
2166 dt.day = sbufReadU8(src);
2167 dt.hours = sbufReadU8(src);
2168 dt.minutes = sbufReadU8(src);
2169 dt.seconds = sbufReadU8(src);
2170 dt.millis = 0;
2171 rtcSetDateTime(&dt);
2172 }
2173
2174 break;
2175 #endif
2176
2177 case MSP_SET_TX_INFO:
2178 setRssiMsp(sbufReadU8(src));
2179
2180 break;
2181
2182 #if defined(USE_BOARD_INFO)
2183 case MSP_SET_BOARD_INFO:
2184 if (!boardInformationIsSet()) {
2185 uint8_t length = sbufReadU8(src);
2186 char boardName[MAX_BOARD_NAME_LENGTH + 1];
2187 sbufReadData(src, boardName, MIN(length, MAX_BOARD_NAME_LENGTH));
2188 if (length > MAX_BOARD_NAME_LENGTH) {
2189 sbufAdvance(src, length - MAX_BOARD_NAME_LENGTH);
2190 }
2191 boardName[length] = '\0';
2192 length = sbufReadU8(src);
2193 char manufacturerId[MAX_MANUFACTURER_ID_LENGTH + 1];
2194 sbufReadData(src, manufacturerId, MIN(length, MAX_MANUFACTURER_ID_LENGTH));
2195 if (length > MAX_MANUFACTURER_ID_LENGTH) {
2196 sbufAdvance(src, length - MAX_MANUFACTURER_ID_LENGTH);
2197 }
2198 manufacturerId[length] = '\0';
2199
2200 setBoardName(boardName);
2201 setManufacturerId(manufacturerId);
2202 persistBoardInformation();
2203 } else {
2204 return MSP_RESULT_ERROR;
2205 }
2206
2207 break;
2208 #if defined(USE_SIGNATURE)
2209 case MSP_SET_SIGNATURE:
2210 if (!signatureIsSet()) {
2211 uint8_t signature[SIGNATURE_LENGTH];
2212 sbufReadData(src, signature, SIGNATURE_LENGTH);
2213 setSignature(signature);
2214 persistSignature();
2215 } else {
2216 return MSP_RESULT_ERROR;
2217 }
2218
2219 break;
2220 #endif
2221 #endif // USE_BOARD_INFO
2222 default:
2223 // we do not know how to handle the (valid) message, indicate error MSP $M!
2224 return MSP_RESULT_ERROR;
2225 }
2226 return MSP_RESULT_ACK;
2227 }
2228 #endif // USE_OSD_SLAVE
2229
2230 static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
2231 {
2232 UNUSED(mspPostProcessFn);
2233 const unsigned int dataSize = sbufBytesRemaining(src);
2234 UNUSED(dataSize); // maybe unused due to compiler options
2235
2236 switch (cmdMSP) {
2237 #ifdef USE_TRANSPONDER
2238 case MSP_SET_TRANSPONDER_CONFIG: {
2239 // Backward compatibility to BFC 3.1.1 is lost for this message type
2240
2241 uint8_t provider = sbufReadU8(src);
2242 uint8_t bytesRemaining = dataSize - 1;
2243
2244 if (provider > TRANSPONDER_PROVIDER_COUNT) {
2245 return MSP_RESULT_ERROR;
2246 }
2247
2248 const uint8_t requirementIndex = provider - 1;
2249 const uint8_t transponderDataSize = transponderRequirements[requirementIndex].dataLength;
2250
2251 transponderConfigMutable()->provider = provider;
2252
2253 if (provider == TRANSPONDER_NONE) {
2254 break;
2255 }
2256
2257 if (bytesRemaining != transponderDataSize) {
2258 return MSP_RESULT_ERROR;
2259 }
2260
2261 if (provider != transponderConfig()->provider) {
2262 transponderStopRepeating();
2263 }
2264
2265 memset(transponderConfigMutable()->data, 0, sizeof(transponderConfig()->data));
2266
2267 for (unsigned int i = 0; i < transponderDataSize; i++) {
2268 transponderConfigMutable()->data[i] = sbufReadU8(src);
2269 }
2270 transponderUpdateData();
2271 break;
2272 }
2273 #endif
2274
2275 case MSP_SET_VOLTAGE_METER_CONFIG: {
2276 int8_t id = sbufReadU8(src);
2277
2278 //
2279 // find and configure an ADC voltage sensor
2280 //
2281 int8_t voltageSensorADCIndex;
2282 for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) {
2283 if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) {
2284 break;
2285 }
2286 }
2287
2288 if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) {
2289 voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src);
2290 voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src);
2291 voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src);
2292 } else {
2293 // if we had any other types of voltage sensor to configure, this is where we'd do it.
2294 sbufReadU8(src);
2295 sbufReadU8(src);
2296 sbufReadU8(src);
2297 }
2298 break;
2299 }
2300
2301 case MSP_SET_CURRENT_METER_CONFIG: {
2302 int id = sbufReadU8(src);
2303
2304 switch (id) {
2305 case CURRENT_METER_ID_BATTERY_1:
2306 currentSensorADCConfigMutable()->scale = sbufReadU16(src);
2307 currentSensorADCConfigMutable()->offset = sbufReadU16(src);
2308 break;
2309 #ifdef USE_VIRTUAL_CURRENT_METER
2310 case CURRENT_METER_ID_VIRTUAL_1:
2311 currentSensorVirtualConfigMutable()->scale = sbufReadU16(src);
2312 currentSensorVirtualConfigMutable()->offset = sbufReadU16(src);
2313 break;
2314 #endif
2315 default:
2316 sbufReadU16(src);
2317 sbufReadU16(src);
2318 break;
2319 }
2320 break;
2321 }
2322
2323 case MSP_SET_BATTERY_CONFIG:
2324 batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
2325 batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
2326 batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
2327 batteryConfigMutable()->batteryCapacity = sbufReadU16(src);
2328 batteryConfigMutable()->voltageMeterSource = sbufReadU8(src);
2329 batteryConfigMutable()->currentMeterSource = sbufReadU8(src);
2330 break;
2331
2332 #if defined(USE_OSD) || defined (USE_OSD_SLAVE)
2333 case MSP_SET_OSD_CONFIG:
2334 {
2335 const uint8_t addr = sbufReadU8(src);
2336
2337 if ((int8_t)addr == -1) {
2338 /* Set general OSD settings */
2339 #ifdef USE_MAX7456
2340 vcdProfileMutable()->video_system = sbufReadU8(src);
2341 #else
2342 sbufReadU8(src); // Skip video system
2343 #endif
2344 #if defined(USE_OSD)
2345 osdConfigMutable()->units = sbufReadU8(src);
2346
2347 // Alarms
2348 osdConfigMutable()->rssi_alarm = sbufReadU8(src);
2349 osdConfigMutable()->cap_alarm = sbufReadU16(src);
2350 sbufReadU16(src); // Skip unused (previously fly timer)
2351 osdConfigMutable()->alt_alarm = sbufReadU16(src);
2352
2353 if (sbufBytesRemaining(src) >= 2) {
2354 /* Enabled warnings */
2355 osdConfigMutable()->enabledWarnings = sbufReadU16(src);
2356 }
2357 #endif
2358 } else if ((int8_t)addr == -2) {
2359 #if defined(USE_OSD)
2360 // Timers
2361 uint8_t index = sbufReadU8(src);
2362 if (index > OSD_TIMER_COUNT) {
2363 return MSP_RESULT_ERROR;
2364 }
2365 osdConfigMutable()->timers[index] = sbufReadU16(src);
2366 #endif
2367 return MSP_RESULT_ERROR;
2368 } else {
2369 #if defined(USE_OSD)
2370 const uint16_t value = sbufReadU16(src);
2371
2372 /* Get screen index, 0 is post flight statistics, 1 and above are in flight OSD screens */
2373 const uint8_t screen = (sbufBytesRemaining(src) >= 1) ? sbufReadU8(src) : 1;
2374
2375 if (screen == 0 && addr < OSD_STAT_COUNT) {
2376 /* Set statistic item enable */
2377 osdStatSetState(addr, (value != 0));
2378 } else if (addr < OSD_ITEM_COUNT) {
2379 /* Set element positions */
2380 osdConfigMutable()->item_pos[addr] = value;
2381 } else {
2382 return MSP_RESULT_ERROR;
2383 }
2384 #else
2385 return MSP_RESULT_ERROR;
2386 #endif
2387 }
2388 }
2389 break;
2390
2391 case MSP_OSD_CHAR_WRITE:
2392 #ifdef USE_MAX7456
2393 {
2394 uint8_t font_data[64];
2395 const uint8_t addr = sbufReadU8(src);
2396 for (int i = 0; i < 54; i++) {
2397 font_data[i] = sbufReadU8(src);
2398 }
2399 // !!TODO - replace this with a device independent implementation
2400 max7456WriteNvm(addr, font_data);
2401 }
2402 break;
2403 #else
2404 return MSP_RESULT_ERROR;
2405 #endif
2406 #endif // OSD || USE_OSD_SLAVE
2407
2408 default:
2409 return mspProcessInCommand(cmdMSP, src);
2410 }
2411 return MSP_RESULT_ACK;
2412 }
2413
2414 /*
2415 * Returns MSP_RESULT_ACK, MSP_RESULT_ERROR or MSP_RESULT_NO_REPLY
2416 */
2417 mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
2418 {
2419 int ret = MSP_RESULT_ACK;
2420 sbuf_t *dst = &reply->buf;
2421 sbuf_t *src = &cmd->buf;
2422 const uint8_t cmdMSP = cmd->cmd;
2423 // initialize reply by default
2424 reply->cmd = cmd->cmd;
2425
2426 if (mspCommonProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
2427 ret = MSP_RESULT_ACK;
2428 } else if (mspProcessOutCommand(cmdMSP, dst)) {
2429 ret = MSP_RESULT_ACK;
2430 } else if ((ret = mspFcProcessOutCommandWithArg(cmdMSP, src, dst, mspPostProcessFn)) != MSP_RESULT_CMD_UNKNOWN) {
2431 /* ret */;
2432 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
2433 } else if (cmdMSP == MSP_SET_4WAY_IF) {
2434 mspFc4waySerialCommand(dst, src, mspPostProcessFn);
2435 ret = MSP_RESULT_ACK;
2436 #endif
2437 #ifdef USE_FLASHFS
2438 } else if (cmdMSP == MSP_DATAFLASH_READ) {
2439 mspFcDataFlashReadCommand(dst, src);
2440 ret = MSP_RESULT_ACK;
2441 #endif
2442 } else {
2443 ret = mspCommonProcessInCommand(cmdMSP, src, mspPostProcessFn);
2444 }
2445 reply->result = ret;
2446 return ret;
2447 }
2448
2449 void mspFcProcessReply(mspPacket_t *reply)
2450 {
2451 sbuf_t *src = &reply->buf;
2452 UNUSED(src); // potentially unused depending on compile options.
2453
2454 switch (reply->cmd) {
2455 #ifndef OSD_SLAVE
2456 case MSP_ANALOG:
2457 {
2458 uint8_t batteryVoltage = sbufReadU8(src);
2459 uint16_t mAhDrawn = sbufReadU16(src);
2460 uint16_t rssi = sbufReadU16(src);
2461 uint16_t amperage = sbufReadU16(src);
2462
2463 UNUSED(rssi);
2464 UNUSED(batteryVoltage);
2465 UNUSED(amperage);
2466 UNUSED(mAhDrawn);
2467
2468 #ifdef USE_MSP_CURRENT_METER
2469 currentMeterMSPSet(amperage, mAhDrawn);
2470 #endif
2471 }
2472 break;
2473 #endif
2474
2475 #ifdef USE_OSD_SLAVE
2476 case MSP_DISPLAYPORT:
2477 {
2478 osdSlaveIsLocked = true; // lock it as soon as a MSP_DISPLAYPORT message is received to prevent accidental CLI/DFU mode.
2479
2480 const int subCmd = sbufReadU8(src);
2481
2482 switch (subCmd) {
2483 case 0: // HEARTBEAT
2484 //debug[0]++;
2485 osdSlaveHeartbeat();
2486 break;
2487 case 1: // RELEASE
2488 //debug[1]++;
2489 break;
2490 case 2: // CLEAR
2491 //debug[2]++;
2492 osdSlaveClearScreen();
2493 break;
2494 case 3:
2495 {
2496 //debug[3]++;
2497 #define MSP_OSD_MAX_STRING_LENGTH 30 // FIXME move this
2498 const uint8_t y = sbufReadU8(src); // row
2499 const uint8_t x = sbufReadU8(src); // column
2500 sbufReadU8(src); // reserved
2501 char buf[MSP_OSD_MAX_STRING_LENGTH + 1];
2502 const int len = MIN(sbufBytesRemaining(src), MSP_OSD_MAX_STRING_LENGTH);
2503 sbufReadData(src, &buf, len);
2504 buf[len] = 0;
2505 osdSlaveWrite(x, y, buf);
2506 }
2507 break;
2508 case 4:
2509 osdSlaveDrawScreen();
2510 break;
2511 }
2512 }
2513 break;
2514 #endif
2515 }
2516 }
2517
2518 void mspInit(void)
2519 {
2520 #ifndef USE_OSD_SLAVE
2521 initActiveBoxIds();
2522 #endif
2523 }