?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Merge some betaflight changes into CF.
[betaflight.git] / src / main / fc / msp_server_fc.c
blob30473bfe7246db414ed40e61d71126ab5bde61e1
1 /*
2 * This file is part of Cleanflight.
3 *
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
8 *
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
16 */
17
18 #include <stdbool.h>
19 #include <stdint.h>
20 #include <stdlib.h>
21 #include <string.h>
22 #include <math.h>
23
24 #include "build/build_config.h"
25 #include "build/debug.h"
26 #include <platform.h>
27
28 #include "common/axis.h"
29 #include "common/utils.h"
30 #include "common/color.h"
31 #include "common/maths.h"
32 #include "common/streambuf.h"
33 #include "common/filter.h"
34
35 #include "config/parameter_group.h"
36 #include "config/parameter_group_ids.h"
37 #include "config/feature.h"
38 #include "config/profile.h"
39
40 #include "common/pilot.h"
41
42 #include "drivers/system.h"
43 #include "drivers/sensor.h"
44 #include "drivers/accgyro.h"
45 #include "drivers/compass.h"
46 #include "drivers/gpio.h"
47 #include "drivers/pwm_mapping.h"
48 #include "drivers/serial.h"
49 #include "drivers/bus_i2c.h"
50 #include "drivers/sdcard.h"
51 #include "drivers/buf_writer.h"
52
53 #include "rx/rx.h"
54 #include "rx/msp.h"
55
56 #include "msp/msp.h"
57 #include "msp/msp_protocol.h"
58 #include "msp/msp_serial.h"
59
60 #include "fc/rate_profile.h"
61 #include "fc/rc_controls.h"
62 #include "fc/rc_adjustments.h"
63 #include "fc/fc_tasks.h"
64 #include "fc/runtime_config.h"
65 #include "fc/config.h"
66
67 #include "scheduler/scheduler.h"
68
69 #include "io/motor_and_servo.h"
70 #include "io/gps.h"
71 #include "io/gimbal.h"
72 #include "io/serial.h"
73 #include "io/ledstrip.h"
74 #include "io/flashfs.h"
75 #include "io/transponder_ir.h"
76 #include "io/asyncfatfs/asyncfatfs.h"
77 #include "io/serial_4way.h"
78
79 #include "telemetry/telemetry.h"
80
81 #include "sensors/boardalignment.h"
82 #include "sensors/sensors.h"
83 #include "sensors/amperage.h"
84 #include "sensors/voltage.h"
85 #include "sensors/battery.h"
86 #include "sensors/sonar.h"
87 #include "sensors/acceleration.h"
88 #include "sensors/barometer.h"
89 #include "sensors/compass.h"
90 #include "sensors/gyro.h"
91
92 #include "flight/mixer.h"
93 #include "flight/servos.h"
94 #include "flight/pid.h"
95 #include "flight/imu.h"
96 #include "flight/failsafe.h"
97 #include "flight/navigation.h"
98 #include "flight/altitudehold.h"
99
100 #include "blackbox/blackbox.h"
101
102 #include "fc/cleanflight_fc.h"
103
104 #include "build/version.h"
105 #ifdef USE_HARDWARE_REVISION_DETECTION
106 #include "hardware_revision.h"
107 #endif
108
109 #include "msp/msp_server.h"
110 #include "fc/msp_server_fc.h"
111
112 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
113 #include "io/serial_4way.h"
114 #endif
115
116 extern uint16_t cycleTime; // FIXME dependency on mw.c
117 extern uint16_t rssi; // FIXME dependency on mw.c
118 extern void resetPidProfile(pidProfile_t *pidProfile);
119
120 static const char * const flightControllerIdentifier = CLEANFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
121 static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
122
123 typedef struct box_e {
124 const char *boxName; // GUI-readable box name
125 const uint8_t boxId; // see boxId_e (it is equal to table index, may be optimized)
126 const uint8_t permanentId; // ID for MSP protocol
127 } box_t;
128
129 static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
130 { "ARM", BOXARM, 0 },
131 { "ANGLE", BOXANGLE, 1 },
132 { "HORIZON", BOXHORIZON, 2 },
133 { "BARO", BOXBARO, 3 },
134 //{ "VARIO", BOXVARIO, 4 },
135 { "MAG", BOXMAG, 5 },
136 { "HEADFREE", BOXHEADFREE, 6 },
137 { "HEADADJ", BOXHEADADJ, 7 },
138 { "CAMSTAB", BOXCAMSTAB, 8 },
139 { "CAMTRIG", BOXCAMTRIG, 9 },
140 { "GPS HOME", BOXGPSHOME, 10 },
141 { "GPS HOLD", BOXGPSHOLD, 11 },
142 { "PASSTHRU", BOXPASSTHRU, 12 },
143 { "BEEPER", BOXBEEPERON, 13 },
144 { "LEDMAX", BOXLEDMAX, 14 },
145 { "LEDLOW", BOXLEDLOW, 15 },
146 { "LLIGHTS", BOXLLIGHTS, 16 },
147 { "CALIB", BOXCALIB, 17 },
148 { "GOVERNOR", BOXGOV, 18 },
149 { "OSD SW", BOXOSD, 19 },
150 { "TELEMETRY", BOXTELEMETRY, 20 },
151 { "GTUNE", BOXGTUNE, 21 },
152 { "SONAR", BOXSONAR, 22 },
153 { "SERVO1", BOXSERVO1, 23 },
154 { "SERVO2", BOXSERVO2, 24 },
155 { "SERVO3", BOXSERVO3, 25 },
156 { "BLACKBOX", BOXBLACKBOX, 26 },
157 { "FAILSAFE", BOXFAILSAFE, 27 },
158 { "AIR MODE", BOXAIRMODE, 28 },
159 };
160
161 // mask of enabled IDs, calculated on start based on enabled features. boxId_e is used as bit index.
162 static uint32_t activeBoxIds;
163
164 // from mixer.c
165 extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
166
167 static const char pidnames[] =
168 "ROLL;"
169 "PITCH;"
170 "YAW;"
171 "ALT;"
172 "Pos;"
173 "PosR;"
174 "NavR;"
175 "LEVEL;"
176 "MAG;"
177 "VEL;";
178
179 typedef enum {
180 MSP_SDCARD_STATE_NOT_PRESENT = 0,
181 MSP_SDCARD_STATE_FATAL = 1,
182 MSP_SDCARD_STATE_CARD_INIT = 2,
183 MSP_SDCARD_STATE_FS_INIT = 3,
184 MSP_SDCARD_STATE_READY = 4,
185 } mspSDCardState_e;
186
187 typedef enum {
188 MSP_SDCARD_FLAG_SUPPORTTED = 1,
189 } mspSDCardFlags_e;
190
191 typedef enum {
192 MSP_FLASHFS_BIT_READY = 1,
193 MSP_FLASHFS_BIT_SUPPORTED = 2,
194 } mspFlashfsFlags_e;
195
196 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
197 void msp4WayIfFn(mspPort_t *msp)
198 {
199 waitForSerialPortToFinishTransmitting(msp->port);
200 // esc4wayInit() was called in msp command
201 // modal switch to esc4way, will return only after 4way exit command
202 // port parameters are shared with esc4way, no need to close/reopen it
203 esc4wayProcess(msp->port);
204 // continue processing
205 }
206 #endif
207
208 void mspRebootFn(mspPort_t *msp)
209 {
210 waitForSerialPortToFinishTransmitting(msp->port); // TODO - postpone reboot, allow all modules to react
211 stopMotors();
212 handleOneshotFeatureChangeOnRestart();
213 systemReset();
214
215 // control should never return here.
216 while(1) ;
217 }
218
219 static const box_t *findBoxByBoxId(uint8_t boxId)
220 {
221 for (unsigned i = 0; i < ARRAYLEN(boxes); i++) {
222 const box_t *candidate = &boxes[i];
223 if (candidate->boxId == boxId) {
224 return candidate;
225 }
226 }
227 return NULL;
228 }
229
230 static const box_t *findBoxByPermenantId(uint8_t permanentId)
231 {
232 for (unsigned i = 0; i < ARRAYLEN(boxes); i++) {
233 const box_t *candidate = &boxes[i];
234 if (candidate->permanentId == permanentId) {
235 return candidate;
236 }
237 }
238 return NULL;
239 }
240
241 static void serializeBoxNamesReply(mspPacket_t *reply)
242 {
243 sbuf_t *dst = &reply->buf;
244 for (int i = 0; i < CHECKBOX_ITEM_COUNT; i++) {
245 if(!(activeBoxIds & (1 << i)))
246 continue; // box is not enabled
247 const box_t *box = findBoxByBoxId(i);
248 sbufWriteString(dst, box->boxName);
249 sbufWriteU8(dst, ';'); // TODO - sbufWriteChar?
250 }
251 }
252
253 static void serializeBoxIdsReply(mspPacket_t *reply)
254 {
255 sbuf_t *dst = &reply->buf;
256 for (int i = 0; i < CHECKBOX_ITEM_COUNT; i++) {
257 if(!(activeBoxIds & (1 << i)))
258 continue;
259 const box_t *box = findBoxByBoxId(i);
260 sbufWriteU8(dst, box->permanentId);
261 }
262 }
263
264 static void initActiveBoxIds(void)
265 {
266 uint32_t ena = 0;
267
268 ena |= 1 << BOXARM;
269
270 if (sensors(SENSOR_ACC)) {
271 ena |= 1 << BOXANGLE;
272 ena |= 1 << BOXHORIZON;
273 }
274
275 #ifdef BARO
276 if (sensors(SENSOR_BARO)) {
277 ena |= 1 << BOXBARO;
278 }
279 #endif
280
281 ena |= 1 << BOXAIRMODE;
282
283 if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
284 ena |= 1 << BOXMAG;
285 ena |= 1 << BOXHEADFREE;
286 ena |= 1 << BOXHEADADJ;
287 }
288
289 if (feature(FEATURE_SERVO_TILT))
290 ena |= 1 << BOXCAMSTAB;
291
292 #ifdef GPS
293 if (feature(FEATURE_GPS)) {
294 ena |= 1 << BOXGPSHOME;
295 ena |= 1 << BOXGPSHOLD;
296 }
297 #endif
298
299 if (mixerConfig()->mixerMode == MIXER_FLYING_WING
300 || mixerConfig()->mixerMode == MIXER_AIRPLANE
301 || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE)
302 ena |= 1 << BOXPASSTHRU;
303
304 ena |= 1 << BOXBEEPERON;
305
306 #ifdef LED_STRIP
307 if (feature(FEATURE_LED_STRIP)) {
308 ena |= 1 << BOXLEDLOW;
309 }
310 #endif
311
312 if (feature(FEATURE_INFLIGHT_ACC_CAL))
313 ena |= 1 << BOXCALIB;
314
315 ena |= 1 << BOXOSD;
316
317 #ifdef TELEMETRY
318 if (feature(FEATURE_TELEMETRY) && telemetryConfig()->telemetry_switch)
319 ena |= 1 << BOXTELEMETRY;
320 #endif
321
322 if (feature(FEATURE_SONAR)){
323 ena |= 1 << BOXSONAR;
324 }
325
326 #ifdef USE_SERVOS
327 if (mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) {
328 ena |= 1 << BOXSERVO1;
329 ena |= 1 << BOXSERVO2;
330 ena |= 1 << BOXSERVO3;
331 }
332 #endif
333
334 #ifdef BLACKBOX
335 if (feature(FEATURE_BLACKBOX)){
336 ena |= 1 << BOXBLACKBOX;
337 }
338 #endif
339
340 if (feature(FEATURE_FAILSAFE)){
341 ena |= 1 << BOXFAILSAFE;
342 }
343
344 #ifdef GTUNE
345 ena |= 1 << BOXGTUNE;
346 #endif
347
348 // check that all enabled IDs are in boxes array (check is skipped when using findBoxBy<id>() functions
349 for(boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++)
350 if((ena & (1 << boxId))
351 && findBoxByBoxId(boxId) == NULL)
352 ena &= ~ (1 << boxId); // this should not happen, but handle it gracefully
353 activeBoxIds = ena;
354 }
355
356 #define IS_ENABLED(mask) (mask == 0 ? 0 : 1)
357
358 static uint32_t packFlightModeFlags(void)
359 {
360 // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
361 // Requires new Multiwii protocol version to fix
362 // It would be preferable to setting the enabled bits based on BOXINDEX.
363
364 uint32_t boxEnabledMask = 0; // enabled BOXes, bits indexed by boxId_e
365
366 // enable BOXes dependent on FLIGHT_MODE, use mapping table
367 static const int8_t flightMode_boxId_map[] = FLIGHT_MODE_BOXID_MAP_INITIALIZER;
368 flightModeFlags_e flightModeCopyMask = ~(GTUNE_MODE); // BOXGTUNE is based on rcMode, not flight mode
369 for(unsigned i = 0; i < ARRAYLEN(flightMode_boxId_map); i++) {
370 if(flightMode_boxId_map[i] == -1)
371 continue; // boxId_e does not exist for this FLIGHT_MODE
372 if((flightModeCopyMask & (1 << i)) == 0)
373 continue; // this flightmode is not copied
374 if(FLIGHT_MODE(1 << i))
375 boxEnabledMask |= 1 << flightMode_boxId_map[i];
376 }
377
378 // enable BOXes dependent on rcMode bits, indexes are the same.
379 // only subset of BOXes depend on rcMode, use mask to mark them
380 #define BM(x) (1 << (x))
381 const uint32_t rcModeCopyMask = BM(BOXHEADADJ) | BM(BOXCAMSTAB) | BM(BOXCAMTRIG) | BM(BOXBEEPERON)
382 | BM(BOXLEDMAX) | BM(BOXLEDLOW) | BM(BOXLLIGHTS) | BM(BOXCALIB) | BM(BOXGOV) | BM(BOXOSD)
383 | BM(BOXTELEMETRY) | BM(BOXGTUNE) | BM(BOXBLACKBOX) | BM(BOXAIRMODE) ;
384 for(unsigned i = 0; i < sizeof(rcModeCopyMask) * 8; i++) {
385 if((rcModeCopyMask & BM(i)) == 0)
386 continue;
387 if(rcModeIsActive(i))
388 boxEnabledMask |= 1 << i;
389 }
390 #undef BM
391 // copy ARM state
392 if(ARMING_FLAG(ARMED))
393 boxEnabledMask |= 1 << BOXARM;
394
395 // map boxId_e enabled bits to MSP status indexes
396 // only active boxIds are sent in status over MSP, other bits are not counted
397 uint32_t mspBoxEnabledMask = 0;
398 unsigned mspBoxIdx = 0; // index of active boxId (matches sent permanentId and boxNames)
399 for (boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++) {
400 if((activeBoxIds & (1 << boxId)) == 0)
401 continue; // this box is not active
402 if (boxEnabledMask & (1 << boxId))
403 mspBoxEnabledMask |= 1 << mspBoxIdx; // box is enabled
404 mspBoxIdx++; // next output bit ID
405 }
406 return mspBoxEnabledMask;
407 }
408
409 static void serializeSDCardSummaryReply(mspPacket_t *reply)
410 {
411 sbuf_t *dst = &reply->buf;
412 #ifdef USE_SDCARD
413 uint8_t flags = MSP_SDCARD_FLAG_SUPPORTTED;
414 uint8_t state;
415
416 sbufWriteU8(dst, flags);
417
418 // Merge the card and filesystem states together
419 if (!sdcard_isInserted()) {
420 state = MSP_SDCARD_STATE_NOT_PRESENT;
421 } else if (!sdcard_isFunctional()) {
422 state = MSP_SDCARD_STATE_FATAL;
423 } else {
424 switch (afatfs_getFilesystemState()) {
425 case AFATFS_FILESYSTEM_STATE_READY:
426 state = MSP_SDCARD_STATE_READY;
427 break;
428 case AFATFS_FILESYSTEM_STATE_INITIALIZATION:
429 if (sdcard_isInitialized()) {
430 state = MSP_SDCARD_STATE_FS_INIT;
431 } else {
432 state = MSP_SDCARD_STATE_CARD_INIT;
433 }
434 break;
435 case AFATFS_FILESYSTEM_STATE_FATAL:
436 case AFATFS_FILESYSTEM_STATE_UNKNOWN:
437 default:
438 state = MSP_SDCARD_STATE_FATAL;
439 break;
440 }
441 }
442
443 sbufWriteU8(dst, state);
444 sbufWriteU8(dst, afatfs_getLastError());
445 // Write free space and total space in kilobytes
446 sbufWriteU32(dst, afatfs_getContiguousFreeSpace() / 1024);
447 sbufWriteU32(dst, sdcard_getMetadata()->numBlocks / 2); // Block size is half a kilobyte
448 #else
449 sbufWriteU8(dst, 0);
450 sbufWriteU8(dst, 0);
451 sbufWriteU8(dst, 0);
452 sbufWriteU32(dst, 0);
453 sbufWriteU32(dst, 0);
454 #endif
455 }
456
457 static void serializeDataflashSummaryReply(mspPacket_t *reply)
458 {
459 sbuf_t *dst = &reply->buf;
460 #ifdef USE_FLASHFS
461 const flashGeometry_t *geometry = flashfsGetGeometry();
462 uint8_t flags = (flashfsIsReady() ? MSP_FLASHFS_BIT_READY : 0) | MSP_FLASHFS_BIT_SUPPORTED;
463
464 sbufWriteU8(dst, flags);
465 sbufWriteU32(dst, geometry->sectors);
466 sbufWriteU32(dst, geometry->totalSize);
467 sbufWriteU32(dst, flashfsGetOffset()); // Effectively the current number of bytes stored on the volume
468 #else
469 sbufWriteU8(dst, 0); // FlashFS is neither ready nor supported
470 sbufWriteU32(dst, 0);
471 sbufWriteU32(dst, 0);
472 sbufWriteU32(dst, 0);
473 #endif
474 }
475
476 #ifdef USE_FLASHFS
477 static void serializeDataflashReadReply(mspPacket_t *reply, uint32_t address, int size)
478 {
479 sbuf_t *dst = &reply->buf;
480 sbufWriteU32(dst, address);
481 size = MIN(size, sbufBytesRemaining(dst)); // limit reply to available buffer space
482 // bytesRead will be lower than that requested if we reach end of volume
483 int bytesRead = flashfsReadAbs(address, sbufPtr(dst), size);
484 sbufAdvance(dst, bytesRead);
485 }
486 #endif
487
488 // return positive for ACK, negative on error, zero for no reply
489 int mspServerCommandHandler(mspPacket_t *cmd, mspPacket_t *reply)
490 {
491 sbuf_t *dst = &reply->buf;
492 sbuf_t *src = &cmd->buf;
493
494 int len = sbufBytesRemaining(src);
495
496 switch (cmd->cmd) {
497 case MSP_API_VERSION:
498 sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
499
500 sbufWriteU8(dst, API_VERSION_MAJOR);
501 sbufWriteU8(dst, API_VERSION_MINOR);
502 break;
503
504 case MSP_FC_VARIANT:
505 sbufWriteData(dst, flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
506 break;
507
508 case MSP_FC_VERSION:
509 sbufWriteU8(dst, FC_VERSION_MAJOR);
510 sbufWriteU8(dst, FC_VERSION_MINOR);
511 sbufWriteU8(dst, FC_VERSION_PATCH_LEVEL);
512 break;
513
514 case MSP_BOARD_INFO:
515 sbufWriteData(dst, boardIdentifier, BOARD_IDENTIFIER_LENGTH);
516 #ifdef USE_HARDWARE_REVISION_DETECTION
517 sbufWriteU16(dst, hardwareRevision);
518 #else
519 sbufWriteU16(dst, 0); // No hardware revision available.
520 #endif
521 sbufWriteU8(dst, 0); // 0 == FC, 1 == OSD, 2 == FC with OSD
522 break;
523
524 case MSP_BUILD_INFO:
525 sbufWriteData(dst, buildDate, BUILD_DATE_LENGTH);
526 sbufWriteData(dst, buildTime, BUILD_TIME_LENGTH);
527 sbufWriteData(dst, shortGitRevision, GIT_SHORT_REVISION_LENGTH);
528 break;
529
530 // DEPRECATED - Use MSP_API_VERSION
531 case MSP_IDENT:
532 sbufWriteU8(dst, MW_VERSION);
533 sbufWriteU8(dst, mixerConfig()->mixerMode);
534 sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
535 sbufWriteU32(dst, CAP_DYNBALANCE); // "capability"
536 break;
537
538 case MSP_STATUS:
539 sbufWriteU16(dst, cycleTime);
540 #ifdef USE_I2C
541 sbufWriteU16(dst, i2cGetErrorCounter());
542 #else
543 sbufWriteU16(dst, 0);
544 #endif
545 sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
546 sbufWriteU32(dst, packFlightModeFlags());
547 sbufWriteU8(dst, getCurrentProfile());
548 sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
549 break;
550
551 case MSP_RAW_IMU: {
552 // Hack scale due to choice of units for sensor data in multiwii
553 unsigned scale_shift = (acc.acc_1G > 1024) ? 3 : 0;
554
555 for (unsigned i = 0; i < 3; i++)
556 sbufWriteU16(dst, accSmooth[i] >> scale_shift);
557 for (unsigned i = 0; i < 3; i++)
558 sbufWriteU16(dst, gyroADC[i]);
559 for (unsigned i = 0; i < 3; i++)
560 sbufWriteU16(dst, magADC[i]);
561 break;
562 }
563
564 #ifdef USE_SERVOS
565 case MSP_SERVO:
566 sbufWriteData(dst, &servo, MAX_SUPPORTED_SERVOS * 2);
567 break;
568
569 case MSP_SERVO_CONFIGURATIONS:
570 for (unsigned i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
571 sbufWriteU16(dst, servoProfile()->servoConf[i].min);
572 sbufWriteU16(dst, servoProfile()->servoConf[i].max);
573 sbufWriteU16(dst, servoProfile()->servoConf[i].middle);
574 sbufWriteU8(dst, servoProfile()->servoConf[i].rate);
575 sbufWriteU8(dst, servoProfile()->servoConf[i].angleAtMin);
576 sbufWriteU8(dst, servoProfile()->servoConf[i].angleAtMax);
577 sbufWriteU8(dst, servoProfile()->servoConf[i].forwardFromChannel);
578 sbufWriteU32(dst, servoProfile()->servoConf[i].reversedSources);
579 }
580 break;
581
582 case MSP_SERVO_MIX_RULES:
583 for (unsigned i = 0; i < MAX_SERVO_RULES; i++) {
584 sbufWriteU8(dst, customServoMixer(i)->targetChannel);
585 sbufWriteU8(dst, customServoMixer(i)->inputSource);
586 sbufWriteU8(dst, customServoMixer(i)->rate);
587 sbufWriteU8(dst, customServoMixer(i)->speed);
588 sbufWriteU8(dst, customServoMixer(i)->min);
589 sbufWriteU8(dst, customServoMixer(i)->max);
590 sbufWriteU8(dst, customServoMixer(i)->box);
591 }
592 break;
593 #endif
594
595 case MSP_MOTOR:
596 for (unsigned i = 0; i < 8; i++) {
597 sbufWriteU16(dst, i < MAX_SUPPORTED_MOTORS ? motor[i] : 0);
598 }
599 break;
600
601 case MSP_RC:
602 for (int i = 0; i < rxRuntimeConfig.channelCount; i++)
603 sbufWriteU16(dst, rcData[i]);
604 break;
605
606 case MSP_ATTITUDE:
607 sbufWriteU16(dst, attitude.values.roll);
608 sbufWriteU16(dst, attitude.values.pitch);
609 sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
610 break;
611
612 case MSP_ALTITUDE:
613 #if defined(BARO) || defined(SONAR)
614 sbufWriteU32(dst, altitudeHoldGetEstimatedAltitude());
615 sbufWriteU16(dst, vario);
616 #else
617 sbufWriteU32(dst, 0);
618 sbufWriteU16(dst, 0);
619 #endif
620 break;
621
622 case MSP_SONAR_ALTITUDE:
623 #if defined(SONAR)
624 sbufWriteU32(dst, sonarGetLatestAltitude());
625 #else
626 sbufWriteU32(dst, 0);
627 #endif
628 break;
629
630 case MSP_ANALOG: {
631 amperageMeter_t *amperageMeter = getAmperageMeter(batteryConfig()->amperageMeterSource);
632
633 sbufWriteU8(dst, (uint8_t)constrain(vbat, 0, 255));
634 sbufWriteU16(dst, (uint16_t)constrain(amperageMeter->mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
635 sbufWriteU16(dst, rssi);
636
637 if (mspServerConfig()->multiwiiCurrentMeterOutput) {
638 sbufWriteU16(dst, (uint16_t)constrain(amperageMeter->amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
639 } else {
640 sbufWriteU16(dst, (int16_t)constrain(amperageMeter->amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
641 }
642 break;
643 }
644
645 case MSP_ARMING_CONFIG:
646 sbufWriteU8(dst, armingConfig()->auto_disarm_delay);
647 sbufWriteU8(dst, armingConfig()->disarm_kill_switch);
648 break;
649
650 case MSP_RC_TUNING:
651 sbufWriteU8(dst, currentControlRateProfile->rcRate8);
652 sbufWriteU8(dst, currentControlRateProfile->rcExpo8);
653 for (unsigned i = 0 ; i < 3; i++) {
654 sbufWriteU8(dst, currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
655 }
656 sbufWriteU8(dst, currentControlRateProfile->dynThrPID);
657 sbufWriteU8(dst, currentControlRateProfile->thrMid8);
658 sbufWriteU8(dst, currentControlRateProfile->thrExpo8);
659 sbufWriteU16(dst, currentControlRateProfile->tpa_breakpoint);
660 sbufWriteU8(dst, currentControlRateProfile->rcYawExpo8);
661 break;
662
663 case MSP_PID:
664 for (int i = 0; i < PID_ITEM_COUNT; i++) {
665 sbufWriteU8(dst, pidProfile()->P8[i]);
666 sbufWriteU8(dst, pidProfile()->I8[i]);
667 sbufWriteU8(dst, pidProfile()->D8[i]);
668 }
669 break;
670
671 case MSP_PIDNAMES:
672 sbufWriteString(dst, pidnames);
673 break;
674
675 case MSP_PID_CONTROLLER:
676 sbufWriteU8(dst, pidProfile()->pidController);
677 break;
678
679 case MSP_MODE_RANGES:
680 for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
681 modeActivationCondition_t *mac = &modeActivationProfile()->modeActivationConditions[i];
682 const box_t *box = findBoxByBoxId(mac->modeId);
683 sbufWriteU8(dst, box->permanentId);
684 sbufWriteU8(dst, mac->auxChannelIndex);
685 sbufWriteU8(dst, mac->range.startStep);
686 sbufWriteU8(dst, mac->range.endStep);
687 }
688 break;
689
690 case MSP_ADJUSTMENT_RANGES:
691 for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
692 adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i];
693 sbufWriteU8(dst, adjRange->adjustmentIndex);
694 sbufWriteU8(dst, adjRange->auxChannelIndex);
695 sbufWriteU8(dst, adjRange->range.startStep);
696 sbufWriteU8(dst, adjRange->range.endStep);
697 sbufWriteU8(dst, adjRange->adjustmentFunction);
698 sbufWriteU8(dst, adjRange->auxSwitchChannelIndex);
699 }
700 break;
701
702 case MSP_BOXNAMES:
703 serializeBoxNamesReply(reply);
704 break;
705
706 case MSP_BOXIDS:
707 serializeBoxIdsReply(reply);
708 break;
709
710 case MSP_MISC:
711 sbufWriteU16(dst, rxConfig()->midrc);
712
713 sbufWriteU16(dst, motorAndServoConfig()->minthrottle);
714 sbufWriteU16(dst, motorAndServoConfig()->maxthrottle);
715 sbufWriteU16(dst, motorAndServoConfig()->mincommand);
716
717 sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
718
719 #ifdef GPS
720 sbufWriteU8(dst, gpsConfig()->provider); // gps_type
721 sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
722 sbufWriteU8(dst, gpsConfig()->sbasMode); // gps_ubx_sbas
723 #else
724 sbufWriteU8(dst, 0); // gps_type
725 sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
726 sbufWriteU8(dst, 0); // gps_ubx_sbas
727 #endif
728 sbufWriteU8(dst, mspServerConfig()->multiwiiCurrentMeterOutput);
729 sbufWriteU8(dst, rxConfig()->rssi_channel);
730 sbufWriteU8(dst, 0);
731
732 sbufWriteU16(dst, compassConfig()->mag_declination);
733 break;
734
735 case MSP_MOTOR_PINS:
736 // FIXME This is hardcoded and should not be.
737 for (int i = 0; i < 8; i++)
738 sbufWriteU8(dst, i + 1);
739 break;
740
741 #ifdef GPS
742 case MSP_RAW_GPS:
743 sbufWriteU8(dst, STATE(GPS_FIX));
744 sbufWriteU8(dst, GPS_numSat);
745 sbufWriteU32(dst, GPS_coord[LAT]);
746 sbufWriteU32(dst, GPS_coord[LON]);
747 sbufWriteU16(dst, GPS_altitude);
748 sbufWriteU16(dst, GPS_speed);
749 sbufWriteU16(dst, GPS_ground_course);
750 break;
751
752 case MSP_COMP_GPS:
753 sbufWriteU16(dst, GPS_distanceToHome);
754 sbufWriteU16(dst, GPS_directionToHome);
755 sbufWriteU8(dst, GPS_update & 1);
756 break;
757
758 case MSP_WP: {
759 uint8_t wp_no = sbufReadU8(src); // get the wp number
760 int32_t lat = 0, lon = 0;
761 if (wp_no == 0) {
762 lat = GPS_home[LAT];
763 lon = GPS_home[LON];
764 } else if (wp_no == 16) {
765 lat = GPS_hold[LAT];
766 lon = GPS_hold[LON];
767 }
768 sbufWriteU8(dst, wp_no);
769 sbufWriteU32(dst, lat);
770 sbufWriteU32(dst, lon);
771 sbufWriteU32(dst, AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps
772 sbufWriteU16(dst, 0); // heading will come here (deg)
773 sbufWriteU16(dst, 0); // time to stay (ms) will come here
774 sbufWriteU8(dst, 0); // nav flag will come here
775 break;
776 }
777
778 case MSP_GPSSVINFO:
779 sbufWriteU8(dst, GPS_numCh);
780 for (int i = 0; i < GPS_numCh; i++){
781 sbufWriteU8(dst, GPS_svinfo_chn[i]);
782 sbufWriteU8(dst, GPS_svinfo_svid[i]);
783 sbufWriteU8(dst, GPS_svinfo_quality[i]);
784 sbufWriteU8(dst, GPS_svinfo_cno[i]);
785 }
786 break;
787 #endif
788
789 case MSP_DEBUG:
790 // output some useful QA statistics
791 // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock]
792
793 for (int i = 0; i < DEBUG16_VALUE_COUNT; i++)
794 sbufWriteU16(dst, debug[i]); // 4 variables are here for general monitoring purpose
795 break;
796
797 // Additional commands that are not compatible with MultiWii
798 case MSP_ACC_TRIM:
799 sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.pitch);
800 sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.roll);
801 break;
802
803 case MSP_UID:
804 sbufWriteU32(dst, U_ID_0);
805 sbufWriteU32(dst, U_ID_1);
806 sbufWriteU32(dst, U_ID_2);
807 break;
808
809 case MSP_FEATURE:
810 sbufWriteU32(dst, featureMask());
811 break;
812
813 case MSP_BOARD_ALIGNMENT:
814 sbufWriteU16(dst, boardAlignment()->rollDegrees);
815 sbufWriteU16(dst, boardAlignment()->pitchDegrees);
816 sbufWriteU16(dst, boardAlignment()->yawDegrees);
817 break;
818
819 case MSP_VOLTAGE_METER_CONFIG:
820 for (int i = 0; i < MAX_VOLTAGE_METERS; i++) {
821 sbufWriteU8(dst, voltageMeterConfig(i)->vbatscale);
822 sbufWriteU8(dst, voltageMeterConfig(i)->vbatresdivval);
823 sbufWriteU8(dst, voltageMeterConfig(i)->vbatresdivmultiplier);
824 }
825 break;
826
827 case MSP_AMPERAGE_METER_CONFIG:
828 for (int i = 0; i < MAX_AMPERAGE_METERS; i++) {
829 sbufWriteU16(dst, amperageMeterConfig(i)->scale);
830 sbufWriteU16(dst, amperageMeterConfig(i)->offset);
831 }
832 break;
833
834 case MSP_BATTERY_CONFIG:
835 sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage);
836 sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage);
837 sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage);
838 sbufWriteU16(dst, batteryConfig()->batteryCapacity);
839 sbufWriteU8(dst, batteryConfig()->amperageMeterSource);
840 break;
841
842 case MSP_MIXER:
843 sbufWriteU8(dst, mixerConfig()->mixerMode);
844 break;
845
846 case MSP_RX_CONFIG:
847 sbufWriteU8(dst, rxConfig()->serialrx_provider);
848 sbufWriteU16(dst, rxConfig()->maxcheck);
849 sbufWriteU16(dst, rxConfig()->midrc);
850 sbufWriteU16(dst, rxConfig()->mincheck);
851 sbufWriteU8(dst, rxConfig()->spektrum_sat_bind);
852 sbufWriteU16(dst, rxConfig()->rx_min_usec);
853 sbufWriteU16(dst, rxConfig()->rx_max_usec);
854 break;
855
856 case MSP_FAILSAFE_CONFIG:
857 sbufWriteU8(dst, failsafeConfig()->failsafe_delay);
858 sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay);
859 sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
860 sbufWriteU8(dst, failsafeConfig()->failsafe_kill_switch);
861 sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay);
862 sbufWriteU8(dst, failsafeConfig()->failsafe_procedure);
863 break;
864
865 case MSP_RXFAIL_CONFIG:
866 for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
867 sbufWriteU8(dst, failsafeChannelConfigs(i)->mode);
868 sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(failsafeChannelConfigs(i)->step));
869 }
870 break;
871
872 case MSP_RSSI_CONFIG:
873 sbufWriteU8(dst, rxConfig()->rssi_channel);
874 break;
875
876 case MSP_RX_MAP:
877 for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++)
878 sbufWriteU8(dst, rxConfig()->rcmap[i]);
879 break;
880
881 case MSP_BF_CONFIG:
882 sbufWriteU8(dst, mixerConfig()->mixerMode);
883
884 sbufWriteU32(dst, featureMask());
885
886 sbufWriteU8(dst, rxConfig()->serialrx_provider);
887
888 sbufWriteU16(dst, boardAlignment()->rollDegrees);
889 sbufWriteU16(dst, boardAlignment()->pitchDegrees);
890 sbufWriteU16(dst, boardAlignment()->yawDegrees);
891 break;
892
893 case MSP_CF_SERIAL_CONFIG:
894 for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
895 if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
896 continue;
897 };
898 sbufWriteU8(dst, serialConfig()->portConfigs[i].identifier);
899 sbufWriteU16(dst, serialConfig()->portConfigs[i].functionMask);
900 for (int baudRateIndex = 0; baudRateIndex < FUNCTION_BAUD_RATE_COUNT; baudRateIndex++) {
901 sbufWriteU8(dst, serialConfig()->portConfigs[i].baudRates[baudRateIndex]);
902 }
903 }
904 break;
905
906 #ifdef LED_STRIP
907 case MSP_LED_COLORS:
908 for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
909 hsvColor_t *color = colors(i);
910 sbufWriteU16(dst, color->h);
911 sbufWriteU8(dst, color->s);
912 sbufWriteU8(dst, color->v);
913 }
914 break;
915
916 case MSP_LED_STRIP_CONFIG:
917 for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
918 ledConfig_t *ledConfig = ledConfigs(i);
919 sbufWriteU32(dst, *ledConfig);
920 }
921 break;
922
923 case MSP_LED_STRIP_MODECOLOR:
924 for (int i = 0; i < LED_MODE_COUNT; i++) {
925 for (int j = 0; j < LED_DIRECTION_COUNT; j++) {
926 sbufWriteU8(dst, i);
927 sbufWriteU8(dst, j);
928 sbufWriteU8(dst, modeColors(i)->color[j]);
929 }
930 }
931 for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) {
932 sbufWriteU8(dst, LED_MODE_COUNT);
933 sbufWriteU8(dst, j);
934 sbufWriteU8(dst, specialColors_System.color[j]);
935 }
936 break;
937 #endif
938
939 case MSP_DATAFLASH_SUMMARY:
940 serializeDataflashSummaryReply(reply);
941 break;
942
943 #ifdef USE_FLASHFS
944 case MSP_DATAFLASH_READ: {
945 uint32_t readAddress = sbufReadU32(src);
946
947 serializeDataflashReadReply(reply, readAddress, 128);
948 break;
949 }
950 #endif
951
952 case MSP_BLACKBOX_CONFIG:
953
954 #ifdef BLACKBOX
955 sbufWriteU8(dst, 1); //Blackbox supported
956 sbufWriteU8(dst, blackboxConfig()->device);
957 sbufWriteU8(dst, blackboxConfig()->rate_num);
958 sbufWriteU8(dst, blackboxConfig()->rate_denom);
959 #else
960 sbufWriteU8(dst, 0); // Blackbox not supported
961 sbufWriteU8(dst, 0);
962 sbufWriteU8(dst, 0);
963 sbufWriteU8(dst, 0);
964 #endif
965 break;
966
967 case MSP_SDCARD_SUMMARY:
968 serializeSDCardSummaryReply(reply);
969 break;
970
971 case MSP_BATTERY_STATES: {
972 amperageMeter_t *amperageMeter = getAmperageMeter(batteryConfig()->amperageMeterSource);
973
974 sbufWriteU8(dst, (uint8_t)getBatteryState() == BATTERY_NOT_PRESENT ? 0 : 1); // battery connected - 0 not connected, 1 connected
975 sbufWriteU8(dst, (uint8_t)constrain(vbat, 0, 255));
976 sbufWriteU16(dst, (uint16_t)constrain(amperageMeter->mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
977 break;
978 }
979
980 case MSP_CURRENT_METERS:
981 for (int i = 0; i < MAX_AMPERAGE_METERS; i++) {
982 amperageMeter_t *meter = getAmperageMeter(i);
983 // write out amperage, once for each current meter.
984 sbufWriteU16(dst, (uint16_t)constrain(meter->amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
985 sbufWriteU32(dst, meter->mAhDrawn);
986 }
987 break;
988 case MSP_VOLTAGE_METERS:
989 // write out voltage, once for each meter.
990 for (int i = 0; i < MAX_VOLTAGE_METERS; i++) {
991 uint16_t voltage = getVoltageMeter(i)->vbat;
992 sbufWriteU8(dst, (uint8_t)constrain(voltage, 0, 255));
993 }
994 break;
995
996 case MSP_TRANSPONDER_CONFIG:
997 #ifdef TRANSPONDER
998 sbufWriteU8(dst, 1); //Transponder supported
999 sbufWriteData(dst, transponderConfig()->data, sizeof(transponderConfig()->data));
1000 #else
1001 sbufWriteU8(dst, 0); // Transponder not supported
1002 #endif
1003 break;
1004
1005 case MSP_BF_BUILD_INFO:
1006 sbufWriteData(dst, buildDate, 11); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
1007 sbufWriteU32(dst, 0); // future exp
1008 sbufWriteU32(dst, 0); // future exp
1009 break;
1010
1011 case MSP_3D:
1012 sbufWriteU16(dst, motor3DConfig()->deadband3d_low);
1013 sbufWriteU16(dst, motor3DConfig()->deadband3d_high);
1014 sbufWriteU16(dst, motor3DConfig()->neutral3d);
1015 break;
1016
1017 case MSP_RC_DEADBAND:
1018 sbufWriteU8(dst, rcControlsConfig()->deadband);
1019 sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
1020 sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband);
1021 sbufWriteU16(dst, rcControlsConfig()->deadband3d_throttle);
1022 break;
1023
1024 case MSP_SENSOR_ALIGNMENT:
1025 sbufWriteU8(dst, sensorAlignmentConfig()->gyro_align);
1026 sbufWriteU8(dst, sensorAlignmentConfig()->acc_align);
1027 sbufWriteU8(dst, sensorAlignmentConfig()->mag_align);
1028 break;
1029
1030 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
1031 case MSP_SET_4WAY_IF:
1032 // initialize 4way ESC interface, return number of ESCs available
1033 sbufWriteU8(dst, esc4wayInit());
1034 mspPostProcessFn = msp4WayIfFn;
1035 break;
1036 #endif
1037
1038 case MSP_SELECT_SETTING:
1039 if (!ARMING_FLAG(ARMED)) {
1040 int profile = sbufReadU8(src);
1041 changeProfile(profile);
1042 }
1043 break;
1044
1045 case MSP_SET_HEAD:
1046 magHold = sbufReadU16(src);
1047 break;
1048
1049 case MSP_SET_RAW_RC: {
1050 uint8_t channelCount = len / sizeof(uint16_t);
1051 if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT)
1052 return -1;
1053 uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
1054
1055 for (unsigned i = 0; i < channelCount; i++) {
1056 frame[i] = sbufReadU16(src);
1057 }
1058
1059 rxMspFrameReceive(frame, channelCount);
1060 break;
1061 }
1062
1063 case MSP_SET_ACC_TRIM:
1064 accelerometerConfig()->accelerometerTrims.values.pitch = sbufReadU16(src);
1065 accelerometerConfig()->accelerometerTrims.values.roll = sbufReadU16(src);
1066 break;
1067
1068 case MSP_SET_ARMING_CONFIG:
1069 armingConfig()->auto_disarm_delay = sbufReadU8(src);
1070 armingConfig()->disarm_kill_switch = sbufReadU8(src);
1071 break;
1072
1073 case MSP_SET_PID_CONTROLLER:
1074 pidProfile()->pidController = sbufReadU8(src);
1075 pidSetController(pidProfile()->pidController);
1076 break;
1077
1078 case MSP_SET_PID:
1079 for (int i = 0; i < PID_ITEM_COUNT; i++) {
1080 pidProfile()->P8[i] = sbufReadU8(src);
1081 pidProfile()->I8[i] = sbufReadU8(src);
1082 pidProfile()->D8[i] = sbufReadU8(src);
1083 }
1084 break;
1085
1086 case MSP_SET_MODE_RANGE: {
1087 int i = sbufReadU8(src);
1088 if (i >= MAX_MODE_ACTIVATION_CONDITION_COUNT)
1089 return -1;
1090 modeActivationCondition_t *mac = &modeActivationProfile()->modeActivationConditions[i];
1091 int permId = sbufReadU8(src);
1092 const box_t *box = findBoxByPermenantId(permId);
1093 if (box == NULL)
1094 return -1;
1095 mac->modeId = box->boxId;
1096 mac->auxChannelIndex = sbufReadU8(src);
1097 mac->range.startStep = sbufReadU8(src);
1098 mac->range.endStep = sbufReadU8(src);
1099
1100 useRcControlsConfig(modeActivationProfile()->modeActivationConditions);
1101 break;
1102 }
1103
1104 case MSP_SET_ADJUSTMENT_RANGE: {
1105 int aRange = sbufReadU8(src);
1106 if (aRange >= MAX_ADJUSTMENT_RANGE_COUNT)
1107 return -1;
1108 adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[aRange];
1109 int aIndex = sbufReadU8(src);
1110 if (aIndex > MAX_SIMULTANEOUS_ADJUSTMENT_COUNT)
1111 return -1;
1112 adjRange->adjustmentIndex = aIndex;
1113 adjRange->auxChannelIndex = sbufReadU8(src);
1114 adjRange->range.startStep = sbufReadU8(src);
1115 adjRange->range.endStep = sbufReadU8(src);
1116 adjRange->adjustmentFunction = sbufReadU8(src);
1117 adjRange->auxSwitchChannelIndex = sbufReadU8(src);
1118 break;
1119 }
1120
1121 case MSP_SET_RC_TUNING:
1122 if (len < 10)
1123 return -1;
1124 currentControlRateProfile->rcRate8 = sbufReadU8(src);
1125 currentControlRateProfile->rcExpo8 = sbufReadU8(src);
1126 for (int i = 0; i < 3; i++) {
1127 unsigned rate = sbufReadU8(src);
1128 currentControlRateProfile->rates[i] = MIN(rate, i == YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
1129 }
1130 unsigned rate = sbufReadU8(src);
1131 currentControlRateProfile->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX);
1132 currentControlRateProfile->thrMid8 = sbufReadU8(src);
1133 currentControlRateProfile->thrExpo8 = sbufReadU8(src);
1134 currentControlRateProfile->tpa_breakpoint = sbufReadU16(src);
1135 if (len < 11)
1136 break;
1137 currentControlRateProfile->rcYawExpo8 = sbufReadU8(src);
1138 break;
1139
1140 case MSP_SET_MISC: {
1141 unsigned midrc = sbufReadU16(src);
1142 if (midrc > 1400 && midrc < 1600)
1143 rxConfig()->midrc = midrc;
1144
1145 motorAndServoConfig()->minthrottle = sbufReadU16(src);
1146 motorAndServoConfig()->maxthrottle = sbufReadU16(src);
1147 motorAndServoConfig()->mincommand = sbufReadU16(src);
1148
1149 failsafeConfig()->failsafe_throttle = sbufReadU16(src);
1150
1151 #ifdef GPS
1152 gpsConfig()->provider = sbufReadU8(src); // gps_type
1153 sbufReadU8(src); // gps_baudrate
1154 gpsConfig()->sbasMode = sbufReadU8(src); // gps_ubx_sbas
1155 #else
1156 sbufReadU8(src); // gps_type
1157 sbufReadU8(src); // gps_baudrate
1158 sbufReadU8(src); // gps_ubx_sbas
1159 #endif
1160 mspServerConfig()->multiwiiCurrentMeterOutput = sbufReadU8(src);
1161 rxConfig()->rssi_channel = sbufReadU8(src);
1162 sbufReadU8(src);
1163
1164 compassConfig()->mag_declination = sbufReadU16(src);
1165 break;
1166 }
1167
1168 case MSP_SET_MOTOR:
1169 for (int i = 0; i < 8; i++) {
1170 const int16_t disarmed = sbufReadU16(src);
1171 if (i < MAX_SUPPORTED_MOTORS) {
1172 motor_disarmed[i] = disarmed;
1173 }
1174 }
1175 break;
1176
1177 case MSP_SET_SERVO_CONFIGURATION: {
1178 #ifdef USE_SERVOS
1179 if (len != 1 + sizeof(servoParam_t))
1180 return -1;
1181 unsigned i = sbufReadU8(src);
1182 if (i >= MAX_SUPPORTED_SERVOS)
1183 return -1;
1184
1185 servoProfile()->servoConf[i].min = sbufReadU16(src);
1186 servoProfile()->servoConf[i].max = sbufReadU16(src);
1187 servoProfile()->servoConf[i].middle = sbufReadU16(src);
1188 servoProfile()->servoConf[i].rate = sbufReadU8(src);
1189 servoProfile()->servoConf[i].angleAtMin = sbufReadU8(src);
1190 servoProfile()->servoConf[i].angleAtMax = sbufReadU8(src);
1191 servoProfile()->servoConf[i].forwardFromChannel = sbufReadU8(src);
1192 servoProfile()->servoConf[i].reversedSources = sbufReadU32(src);
1193 #endif
1194 break;
1195 }
1196
1197 case MSP_SET_SERVO_MIX_RULE: {
1198 #ifdef USE_SERVOS
1199 int i = sbufReadU8(src);
1200 if (i >= MAX_SERVO_RULES)
1201 return -1;
1202
1203 customServoMixer(i)->targetChannel = sbufReadU8(src);
1204 customServoMixer(i)->inputSource = sbufReadU8(src);
1205 customServoMixer(i)->rate = sbufReadU8(src);
1206 customServoMixer(i)->speed = sbufReadU8(src);
1207 customServoMixer(i)->min = sbufReadU8(src);
1208 customServoMixer(i)->max = sbufReadU8(src);
1209 customServoMixer(i)->box = sbufReadU8(src);
1210 loadCustomServoMixer();
1211 #endif
1212 break;
1213 }
1214
1215 case MSP_SET_3D:
1216 motor3DConfig()->deadband3d_low = sbufReadU16(src);
1217 motor3DConfig()->deadband3d_high = sbufReadU16(src);
1218 motor3DConfig()->neutral3d = sbufReadU16(src);
1219 break;
1220
1221 case MSP_SET_RC_DEADBAND:
1222 rcControlsConfig()->deadband = sbufReadU8(src);
1223 rcControlsConfig()->yaw_deadband = sbufReadU8(src);
1224 rcControlsConfig()->alt_hold_deadband = sbufReadU8(src);
1225 rcControlsConfig()->deadband3d_throttle = sbufReadU16(src);
1226 break;
1227
1228 case MSP_SET_RESET_CURR_PID:
1229 PG_RESET_CURRENT(pidProfile);
1230 break;
1231
1232 case MSP_SET_SENSOR_ALIGNMENT:
1233 sensorAlignmentConfig()->gyro_align = sbufReadU8(src);
1234 sensorAlignmentConfig()->acc_align = sbufReadU8(src);
1235 sensorAlignmentConfig()->mag_align = sbufReadU8(src);
1236 break;
1237
1238 case MSP_RESET_CONF:
1239 if (!ARMING_FLAG(ARMED)) {
1240 resetEEPROM();
1241 readEEPROM();
1242 }
1243 break;
1244
1245 case MSP_ACC_CALIBRATION:
1246 if (!ARMING_FLAG(ARMED))
1247 accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
1248 break;
1249
1250 case MSP_MAG_CALIBRATION:
1251 if (!ARMING_FLAG(ARMED))
1252 ENABLE_STATE(CALIBRATE_MAG);
1253 break;
1254
1255 case MSP_EEPROM_WRITE:
1256 if (ARMING_FLAG(ARMED))
1257 return -1;
1258 writeEEPROM();
1259 readEEPROM();
1260 break;
1261
1262 #ifdef BLACKBOX
1263 case MSP_SET_BLACKBOX_CONFIG:
1264 // Don't allow config to be updated while Blackbox is logging
1265 if (!blackboxMayEditConfig())
1266 return -1;
1267 blackboxConfig()->device = sbufReadU8(src);
1268 blackboxConfig()->rate_num = sbufReadU8(src);
1269 blackboxConfig()->rate_denom = sbufReadU8(src);
1270 break;
1271 #endif
1272
1273 #ifdef TRANSPONDER
1274 case MSP_SET_TRANSPONDER_CONFIG:
1275 if (len != sizeof(transponderConfig()->data))
1276 return -1;
1277 sbufReadData(src, transponderConfig()->data, sizeof(transponderConfig()->data));
1278 transponderUpdateData(transponderConfig()->data);
1279 break;
1280 #endif
1281
1282 #ifdef USE_FLASHFS
1283 case MSP_DATAFLASH_ERASE:
1284 flashfsEraseCompletely();
1285 break;
1286 #endif
1287
1288 #ifdef GPS
1289 case MSP_SET_RAW_GPS:
1290 if (sbufReadU8(src)) {
1291 ENABLE_STATE(GPS_FIX);
1292 } else {
1293 DISABLE_STATE(GPS_FIX);
1294 }
1295 GPS_numSat = sbufReadU8(src);
1296 GPS_coord[LAT] = sbufReadU32(src);
1297 GPS_coord[LON] = sbufReadU32(src);
1298 GPS_altitude = sbufReadU16(src);
1299 GPS_speed = sbufReadU16(src);
1300 GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers
1301 break;
1302
1303 case MSP_SET_WP: {
1304 uint8_t wp_no = sbufReadU8(src); // get the wp number
1305 int32_t lat = sbufReadU32(src);
1306 int32_t lon = sbufReadU32(src);
1307 int32_t alt = sbufReadU32(src); // to set altitude (cm)
1308 sbufReadU16(src); // future: to set heading (deg)
1309 sbufReadU16(src); // future: to set time to stay (ms)
1310 sbufReadU8(src); // future: to set nav flag
1311 if (wp_no == 0) {
1312 GPS_home[LAT] = lat;
1313 GPS_home[LON] = lon;
1314 DISABLE_FLIGHT_MODE(GPS_HOME_MODE); // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS
1315 ENABLE_STATE(GPS_FIX_HOME);
1316 if (alt != 0)
1317 AltHold = alt; // temporary implementation to test feature with apps
1318 } else if (wp_no == 16) { // OK with SERIAL GPS -- NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS
1319 GPS_hold[LAT] = lat;
1320 GPS_hold[LON] = lon;
1321 if (alt != 0)
1322 AltHold = alt; // temporary implementation to test feature with apps
1323 nav_mode = NAV_MODE_WP;
1324 GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
1325 }
1326 break;
1327 }
1328 #endif
1329
1330 case MSP_SET_FEATURE:
1331 featureClearAll();
1332 featureSet(sbufReadU32(src)); // features bitmap
1333 break;
1334
1335 case MSP_SET_BOARD_ALIGNMENT:
1336 boardAlignment()->rollDegrees = sbufReadU16(src);
1337 boardAlignment()->pitchDegrees = sbufReadU16(src);
1338 boardAlignment()->yawDegrees = sbufReadU16(src);
1339 break;
1340
1341 case MSP_SET_VOLTAGE_METER_CONFIG: {
1342 int index = sbufReadU8(src);
1343
1344 if (index >= MAX_VOLTAGE_METERS) {
1345 return -1;
1346 }
1347
1348 voltageMeterConfig(index)->vbatscale = sbufReadU8(src);
1349 voltageMeterConfig(index)->vbatresdivval = sbufReadU8(src);
1350 voltageMeterConfig(index)->vbatresdivmultiplier = sbufReadU8(src);
1351 break;
1352 }
1353
1354 case MSP_SET_AMPERAGE_METER_CONFIG: {
1355 int index = sbufReadU8(src);
1356
1357 if (index >= MAX_AMPERAGE_METERS) {
1358 return -1;
1359 }
1360
1361 amperageMeterConfig(index)->scale = sbufReadU16(src);
1362 amperageMeterConfig(index)->offset = sbufReadU16(src);
1363 break;
1364 }
1365
1366 case MSP_SET_BATTERY_CONFIG:
1367 batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
1368 batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
1369 batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
1370 batteryConfig()->batteryCapacity = sbufReadU16(src);
1371 batteryConfig()->amperageMeterSource = sbufReadU8(src);
1372 break;
1373
1374
1375 #ifndef USE_QUAD_MIXER_ONLY
1376 case MSP_SET_MIXER:
1377 mixerConfig()->mixerMode = sbufReadU8(src);
1378 break;
1379 #endif
1380
1381 case MSP_SET_RX_CONFIG:
1382 rxConfig()->serialrx_provider = sbufReadU8(src);
1383 rxConfig()->maxcheck = sbufReadU16(src);
1384 rxConfig()->midrc = sbufReadU16(src);
1385 rxConfig()->mincheck = sbufReadU16(src);
1386 rxConfig()->spektrum_sat_bind = sbufReadU8(src);
1387 if (sbufBytesRemaining(src) < 2)
1388 break;
1389 rxConfig()->rx_min_usec = sbufReadU16(src);
1390 rxConfig()->rx_max_usec = sbufReadU16(src);
1391 break;
1392
1393 case MSP_SET_FAILSAFE_CONFIG:
1394 failsafeConfig()->failsafe_delay = sbufReadU8(src);
1395 failsafeConfig()->failsafe_off_delay = sbufReadU8(src);
1396 failsafeConfig()->failsafe_throttle = sbufReadU16(src);
1397 failsafeConfig()->failsafe_kill_switch = sbufReadU8(src);
1398 failsafeConfig()->failsafe_throttle_low_delay = sbufReadU16(src);
1399 failsafeConfig()->failsafe_procedure = sbufReadU8(src);
1400 break;
1401
1402 case MSP_SET_RXFAIL_CONFIG: {
1403 int channel = sbufReadU8(src);
1404 if (channel >= MAX_SUPPORTED_RC_CHANNEL_COUNT)
1405 return -1;
1406 failsafeChannelConfigs(channel)->mode = sbufReadU8(src);
1407 failsafeChannelConfigs(channel)->step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
1408 break;
1409 }
1410
1411 case MSP_SET_RSSI_CONFIG:
1412 rxConfig()->rssi_channel = sbufReadU8(src);
1413 break;
1414
1415 case MSP_SET_RX_MAP:
1416 for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) {
1417 rxConfig()->rcmap[i] = sbufReadU8(src);
1418 }
1419 break;
1420
1421 case MSP_SET_BF_CONFIG:
1422 #ifdef USE_QUAD_MIXER_ONLY
1423 sbufReadU8(src); // mixerMode ignored
1424 #else
1425 mixerConfig()->mixerMode = sbufReadU8(src); // mixerMode
1426 #endif
1427
1428 featureClearAll();
1429 featureSet(sbufReadU32(src)); // features bitmap
1430
1431 rxConfig()->serialrx_provider = sbufReadU8(src); // serialrx_type
1432
1433 boardAlignment()->rollDegrees = sbufReadU16(src); // board_align_roll
1434 boardAlignment()->pitchDegrees = sbufReadU16(src); // board_align_pitch
1435 boardAlignment()->yawDegrees = sbufReadU16(src); // board_align_yaw
1436
1437 break;
1438
1439 case MSP_SET_CF_SERIAL_CONFIG: {
1440 int portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4);
1441
1442 if (len % portConfigSize != 0)
1443 return -1;
1444
1445 while (sbufBytesRemaining(src) >= portConfigSize) {
1446 uint8_t identifier = sbufReadU8(src);
1447
1448 serialPortConfig_t *portConfig = serialFindPortConfiguration(identifier);
1449 if (!portConfig)
1450 return -1;
1451
1452 portConfig->identifier = identifier;
1453 portConfig->functionMask = sbufReadU16(src);
1454 for (int baudRateIndex = 0; baudRateIndex < FUNCTION_BAUD_RATE_COUNT; baudRateIndex++) {
1455 portConfig->baudRates[baudRateIndex] = sbufReadU8(src);
1456 }
1457 }
1458 break;
1459 }
1460
1461 #ifdef LED_STRIP
1462 case MSP_SET_LED_COLORS:
1463
1464 for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT && sbufBytesRemaining(src) >= 4; i++) {
1465 hsvColor_t *color = colors(i);
1466
1467 int h = sbufReadU16(src);
1468 int s = sbufReadU8(src);
1469 int v = sbufReadU8(src);
1470
1471 if (h > HSV_HUE_MAX || s > HSV_SATURATION_MAX || v > HSV_VALUE_MAX) {
1472 memset(color, 0, sizeof(*color));
1473 return -1;
1474 }
1475
1476 color->h = h;
1477 color->s = s;
1478 color->v = v;
1479 }
1480 break;
1481
1482 case MSP_SET_LED_STRIP_CONFIG: {
1483 int i = sbufReadU8(src);
1484 if (len != (1 + 4) || i >= LED_MAX_STRIP_LENGTH)
1485 return -1;
1486
1487 ledConfig_t *ledConfig = ledConfigs(i);
1488 *ledConfig = sbufReadU32(src);
1489
1490 reevaluateLedConfig();
1491 }
1492 break;
1493
1494 case MSP_SET_LED_STRIP_MODECOLOR:
1495 while (sbufBytesRemaining(src) >= 3) {
1496 ledModeIndex_e modeIdx = sbufReadU8(src);
1497 int funIdx = sbufReadU8(src);
1498 int color = sbufReadU8(src);
1499
1500 if (!setModeColor(modeIdx, funIdx, color))
1501 return -1;
1502 }
1503 break;
1504 #endif
1505
1506 case MSP_REBOOT:
1507 mspPostProcessFn = mspRebootFn;
1508 break;
1509
1510 default:
1511 // we do not know how to handle the message
1512 return -1;
1513 }
1514 return 1; // message was handled successfully
1515 }
1516
1517 void mspInit(void)
1518 {
1519 initActiveBoxIds();
1520 }