?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
vOrtex 250 change
[betaflight.git] / src / main / io / serial_msp.c
blobbebc1c5cd34ba48a704e5950b0d4b40c0f1683b1
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_config.h"
25 #include "debug.h"
26
27 #include "platform.h"
28
29 #include "common/axis.h"
30 #include "common/color.h"
31 #include "common/maths.h"
32
33 #include "drivers/system.h"
34
35 #include "drivers/sensor.h"
36 #include "drivers/accgyro.h"
37 #include "drivers/compass.h"
38
39 #include "drivers/serial.h"
40 #include "drivers/bus_i2c.h"
41 #include "drivers/gpio.h"
42 #include "drivers/timer.h"
43 #include "drivers/pwm_rx.h"
44 #include "drivers/gyro_sync.h"
45
46 #include "rx/rx.h"
47 #include "rx/msp.h"
48
49 #include "io/beeper.h"
50 #include "io/escservo.h"
51 #include "io/rc_controls.h"
52 #include "io/gps.h"
53 #include "io/gimbal.h"
54 #include "io/serial.h"
55 #include "io/ledstrip.h"
56 #include "io/flashfs.h"
57
58 #include "telemetry/telemetry.h"
59
60 #include "sensors/boardalignment.h"
61 #include "sensors/sensors.h"
62 #include "sensors/battery.h"
63 #include "sensors/sonar.h"
64 #include "sensors/acceleration.h"
65 #include "sensors/barometer.h"
66 #include "sensors/compass.h"
67 #include "sensors/gyro.h"
68
69 #include "flight/mixer.h"
70 #include "flight/pid.h"
71 #include "flight/imu.h"
72 #include "flight/failsafe.h"
73 #include "flight/navigation.h"
74 #include "flight/altitudehold.h"
75
76 #include "mw.h"
77
78 #include "config/runtime_config.h"
79 #include "config/config.h"
80 #include "config/config_profile.h"
81 #include "config/config_master.h"
82
83 #include "version.h"
84 #ifdef NAZE
85 #include "hardware_revision.h"
86 #endif
87
88 #include "serial_msp.h"
89
90 #ifdef USE_SERIAL_1WIRE
91 #include "io/serial_1wire.h"
92 #endif
93 static serialPort_t *mspSerialPort;
94
95 extern uint16_t cycleTime; // FIXME dependency on mw.c
96 extern uint16_t rssi; // FIXME dependency on mw.c
97
98 void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse);
99
100 /**
101 * MSP Guidelines, emphasis is used to clarify.
102 *
103 * Each FlightController (FC, Server) MUST change the API version when any MSP command is added, deleted, or changed.
104 *
105 * If you fork the FC source code and release your own version, you MUST change the Flight Controller Identifier.
106 *
107 * NEVER release a modified copy of this code that shares the same Flight controller IDENT and API version
108 * if the API doesn't match EXACTLY.
109 *
110 * Consumers of the API (API clients) SHOULD first attempt to get a response from the MSP_API_VERSION command.
111 * If no response is obtained then client MAY try the legacy MSP_IDENT command.
112 *
113 * API consumers should ALWAYS handle communication failures gracefully and attempt to continue
114 * without the information if possible. Clients MAY log/display a suitable message.
115 *
116 * API clients should NOT attempt any communication if they can't handle the returned API MAJOR VERSION.
117 *
118 * API clients SHOULD attempt communication if the API MINOR VERSION has increased from the time
119 * the API client was written and handle command failures gracefully. Clients MAY disable
120 * functionality that depends on the commands while still leaving other functionality intact.
121 * Clients SHOULD operate in READ-ONLY mode and SHOULD present a warning to the user to state
122 * that the newer API version may cause problems before using API commands that change FC state.
123 *
124 * It is for this reason that each MSP command should be specific as possible, such that changes
125 * to commands break as little functionality as possible.
126 *
127 * API client authors MAY use a compatibility matrix/table when determining if they can support
128 * a given command from a given flight controller at a given api version level.
129 *
130 * Developers MUST NOT create new MSP commands that do more than one thing.
131 *
132 * Failure to follow these guidelines will likely invoke the wrath of developers trying to write tools
133 * that use the API and the users of those tools.
134 */
135
136 #define MSP_PROTOCOL_VERSION 0
137
138 #define API_VERSION_MAJOR 1 // increment when major changes are made
139 #define API_VERSION_MINOR 13 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
140
141 #define API_VERSION_LENGTH 2
142
143 #define MULTIWII_IDENTIFIER "MWII";
144 #define CLEANFLIGHT_IDENTIFIER "CLFL"
145 #define BETAFLIGHT_IDENTIFIER "BTFL"
146 #define BASEFLIGHT_IDENTIFIER "BAFL";
147
148 #define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4
149 static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
150
151 #define FLIGHT_CONTROLLER_VERSION_LENGTH 3
152 #define FLIGHT_CONTROLLER_VERSION_MASK 0xFFF
153
154 static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
155 #define BOARD_IDENTIFIER_LENGTH 4 // 4 UPPER CASE alpha numeric characters that identify the board being used.
156 #define BOARD_HARDWARE_REVISION_LENGTH 2
157
158 // These are baseflight specific flags but they are useless now since MW 2.3 uses the upper 4 bits for the navigation version.
159 #define CAP_PLATFORM_32BIT ((uint32_t)1 << 31)
160 #define CAP_BASEFLIGHT_CONFIG ((uint32_t)1 << 30)
161
162 // MW 2.3 stores NAVI_VERSION in the top 4 bits of the capability mask.
163 #define CAP_NAVI_VERSION_BIT_4_MSB ((uint32_t)1 << 31)
164 #define CAP_NAVI_VERSION_BIT_3 ((uint32_t)1 << 30)
165 #define CAP_NAVI_VERSION_BIT_2 ((uint32_t)1 << 29)
166 #define CAP_NAVI_VERSION_BIT_1_LSB ((uint32_t)1 << 28)
167
168 #define CAP_DYNBALANCE ((uint32_t)1 << 2)
169 #define CAP_FLAPS ((uint32_t)1 << 3)
170 #define CAP_NAVCAP ((uint32_t)1 << 4)
171 #define CAP_EXTAUX ((uint32_t)1 << 5)
172
173 #define MSP_API_VERSION 1 //out message
174 #define MSP_FC_VARIANT 2 //out message
175 #define MSP_FC_VERSION 3 //out message
176 #define MSP_BOARD_INFO 4 //out message
177 #define MSP_BUILD_INFO 5 //out message
178
179 //
180 // MSP commands for Cleanflight original features
181 //
182 #define MSP_MODE_RANGES 34 //out message Returns all mode ranges
183 #define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range
184
185 #define MSP_FEATURE 36
186 #define MSP_SET_FEATURE 37
187
188 #define MSP_BOARD_ALIGNMENT 38
189 #define MSP_SET_BOARD_ALIGNMENT 39
190
191 #define MSP_CURRENT_METER_CONFIG 40
192 #define MSP_SET_CURRENT_METER_CONFIG 41
193
194 #define MSP_MIXER 42
195 #define MSP_SET_MIXER 43
196
197 #define MSP_RX_CONFIG 44
198 #define MSP_SET_RX_CONFIG 45
199
200 #define MSP_LED_COLORS 46
201 #define MSP_SET_LED_COLORS 47
202
203 #define MSP_LED_STRIP_CONFIG 48
204 #define MSP_SET_LED_STRIP_CONFIG 49
205
206 #define MSP_RSSI_CONFIG 50
207 #define MSP_SET_RSSI_CONFIG 51
208
209 #define MSP_ADJUSTMENT_RANGES 52
210 #define MSP_SET_ADJUSTMENT_RANGE 53
211
212 // private - only to be used by the configurator, the commands are likely to change
213 #define MSP_CF_SERIAL_CONFIG 54
214 #define MSP_SET_CF_SERIAL_CONFIG 55
215
216 #define MSP_VOLTAGE_METER_CONFIG 56
217 #define MSP_SET_VOLTAGE_METER_CONFIG 57
218
219 #define MSP_SONAR_ALTITUDE 58 //out message get sonar altitude [cm]
220
221 #define MSP_PID_CONTROLLER 59
222 #define MSP_SET_PID_CONTROLLER 60
223
224 #define MSP_ARMING_CONFIG 61 //out message Returns auto_disarm_delay and disarm_kill_switch parameters
225 #define MSP_SET_ARMING_CONFIG 62 //in message Sets auto_disarm_delay and disarm_kill_switch parameters
226
227 #define MSP_DATAFLASH_SUMMARY 70 //out message - get description of dataflash chip
228 #define MSP_DATAFLASH_READ 71 //out message - get content of dataflash chip
229 #define MSP_DATAFLASH_ERASE 72 //in message - erase dataflash chip
230
231 #define MSP_LOOP_TIME 73 //out message Returns FC cycle time i.e looptime parameter
232 #define MSP_SET_LOOP_TIME 74 //in message Sets FC cycle time i.e looptime parameter
233
234 #define MSP_FAILSAFE_CONFIG 75 //out message Returns FC Fail-Safe settings
235 #define MSP_SET_FAILSAFE_CONFIG 76 //in message Sets FC Fail-Safe settings
236
237 #define MSP_RXFAIL_CONFIG 77 //out message Returns RXFAIL settings
238 #define MSP_SET_RXFAIL_CONFIG 78 //in message Sets RXFAIL settings
239
240 //
241 // Baseflight MSP commands (if enabled they exist in Cleanflight)
242 //
243 #define MSP_RX_MAP 64 //out message get channel map (also returns number of channels total)
244 #define MSP_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from MSP_RX_MAP
245
246 // FIXME - Provided for backwards compatibility with configurator code until configurator is updated.
247 // DEPRECATED - DO NOT USE "MSP_BF_CONFIG" and MSP_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead.
248 #define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere
249 #define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save
250
251 #define MSP_REBOOT 68 //in message reboot settings
252
253 // DEPRECATED - Use MSP_BUILD_INFO instead
254 #define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion
255
256 //
257 // Multwii original MSP commands
258 //
259
260 // DEPRECATED - See MSP_API_VERSION and MSP_MIXER
261 #define MSP_IDENT 100 //out message mixerMode + multiwii version + protocol version + capability variable
262
263
264 #define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number
265 #define MSP_RAW_IMU 102 //out message 9 DOF
266 #define MSP_SERVO 103 //out message servos
267 #define MSP_MOTOR 104 //out message motors
268 #define MSP_RC 105 //out message rc channels and more
269 #define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course
270 #define MSP_COMP_GPS 107 //out message distance home, direction home
271 #define MSP_ATTITUDE 108 //out message 2 angles 1 heading
272 #define MSP_ALTITUDE 109 //out message altitude, variometer
273 #define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX
274 #define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
275 #define MSP_PID 112 //out message P I D coeff (9 are used currently)
276 #define MSP_BOX 113 //out message BOX setup (number is dependant of your setup)
277 #define MSP_MISC 114 //out message powermeter trig
278 #define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI
279 #define MSP_BOXNAMES 116 //out message the aux switch names
280 #define MSP_PIDNAMES 117 //out message the PID names
281 #define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold
282 #define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes
283 #define MSP_SERVO_CONFIGURATIONS 120 //out message All servo configurations.
284 #define MSP_NAV_STATUS 121 //out message Returns navigation status
285 #define MSP_NAV_CONFIG 122 //out message Returns navigation parameters
286 #define MSP_PID_FLOAT 123 //out message P I D Used for Luxfloat
287
288 #define MSP_SET_RAW_RC 200 //in message 8 rc chan
289 #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
290 #define MSP_SET_PID 202 //in message P I D coeff (9 are used currently)
291 #define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup)
292 #define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo
293 #define MSP_ACC_CALIBRATION 205 //in message no param
294 #define MSP_MAG_CALIBRATION 206 //in message no param
295 #define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use
296 #define MSP_RESET_CONF 208 //in message no param
297 #define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags)
298 #define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2)
299 #define MSP_SET_HEAD 211 //in message define a new heading hold direction
300 #define MSP_SET_SERVO_CONFIGURATION 212 //in message Servo settings
301 #define MSP_SET_MOTOR 214 //in message PropBalance function
302 #define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom
303 #define MSP_SET_PID_FLOAT 216 //in message P I D used for luxfloat
304
305 // #define MSP_BIND 240 //in message no param
306
307 #define MSP_EEPROM_WRITE 250 //in message no param
308
309 #define MSP_DEBUGMSG 253 //out message debug string buffer
310 #define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
311
312 // Additional commands that are not compatible with MultiWii
313 #define MSP_UID 160 //out message Unique device ID
314 #define MSP_ACC_TRIM 240 //out message get acc angle trim values
315 #define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
316 #define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox)
317 #define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration
318 #define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration
319 #define MSP_SET_1WIRE 243 //in message Sets 1Wire paththrough
320
321 #define INBUF_SIZE 64
322
323 typedef struct box_e {
324 const uint8_t boxId; // see boxId_e
325 const char *boxName; // GUI-readable box name
326 const uint8_t permanentId; //
327 } box_t;
328
329 // FIXME remove ;'s
330 static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
331 { BOXARM, "ARM;", 0 },
332 { BOXANGLE, "ANGLE;", 1 },
333 { BOXHORIZON, "HORIZON;", 2 },
334 { BOXBARO, "BARO;", 3 },
335 //{ BOXVARIO, "VARIO;", 4 },
336 { BOXMAG, "MAG;", 5 },
337 { BOXHEADFREE, "HEADFREE;", 6 },
338 { BOXHEADADJ, "HEADADJ;", 7 },
339 { BOXCAMSTAB, "CAMSTAB;", 8 },
340 { BOXCAMTRIG, "CAMTRIG;", 9 },
341 { BOXGPSHOME, "GPS HOME;", 10 },
342 { BOXGPSHOLD, "GPS HOLD;", 11 },
343 { BOXPASSTHRU, "PASSTHRU;", 12 },
344 { BOXBEEPERON, "BEEPER;", 13 },
345 { BOXLEDMAX, "LEDMAX;", 14 },
346 { BOXLEDLOW, "LEDLOW;", 15 },
347 { BOXLLIGHTS, "LLIGHTS;", 16 },
348 { BOXCALIB, "CALIB;", 17 },
349 { BOXGOV, "GOVERNOR;", 18 },
350 { BOXOSD, "OSD SW;", 19 },
351 { BOXTELEMETRY, "TELEMETRY;", 20 },
352 { BOXGTUNE, "GTUNE;", 21 },
353 { BOXSONAR, "SONAR;", 22 },
354 { BOXSERVO1, "SERVO1;", 23 },
355 { BOXSERVO2, "SERVO2;", 24 },
356 { BOXSERVO3, "SERVO3;", 25 },
357 { BOXBLACKBOX, "BLACKBOX;", 26 },
358 { BOXFAILSAFE, "FAILSAFE;", 27 },
359 { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
360 };
361
362 // this is calculated at startup based on enabled features.
363 static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT];
364 // this is the number of filled indexes in above array
365 static uint8_t activeBoxIdCount = 0;
366 // from mixer.c
367 extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
368
369 // cause reboot after MSP processing complete
370 static bool isRebootScheduled = false;
371
372 static const char pidnames[] =
373 "ROLL;"
374 "PITCH;"
375 "YAW;"
376 "ALT;"
377 "Pos;"
378 "PosR;"
379 "NavR;"
380 "LEVEL;"
381 "MAG;"
382 "VEL;";
383
384 typedef enum {
385 IDLE,
386 HEADER_START,
387 HEADER_M,
388 HEADER_ARROW,
389 HEADER_SIZE,
390 HEADER_CMD,
391 COMMAND_RECEIVED
392 } mspState_e;
393
394 typedef enum {
395 UNUSED_PORT = 0,
396 FOR_GENERAL_MSP,
397 FOR_TELEMETRY
398 } mspPortUsage_e;
399
400 typedef struct mspPort_s {
401 serialPort_t *port;
402 uint8_t offset;
403 uint8_t dataSize;
404 uint8_t checksum;
405 uint8_t indRX;
406 uint8_t inBuf[INBUF_SIZE];
407 mspState_e c_state;
408 uint8_t cmdMSP;
409 mspPortUsage_e mspPortUsage;
410 } mspPort_t;
411
412 static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
413
414 static mspPort_t *currentPort;
415
416 static void serialize8(uint8_t a)
417 {
418 serialWrite(mspSerialPort, a);
419 currentPort->checksum ^= a;
420 }
421
422 static void serialize16(uint16_t a)
423 {
424 serialize8((uint8_t)(a >> 0));
425 serialize8((uint8_t)(a >> 8));
426 }
427
428 static void serialize32(uint32_t a)
429 {
430 serialize16((uint16_t)(a >> 0));
431 serialize16((uint16_t)(a >> 16));
432 }
433
434 static uint8_t read8(void)
435 {
436 return currentPort->inBuf[currentPort->indRX++] & 0xff;
437 }
438
439 static uint16_t read16(void)
440 {
441 uint16_t t = read8();
442 t += (uint16_t)read8() << 8;
443 return t;
444 }
445
446 static uint32_t read32(void)
447 {
448 uint32_t t = read16();
449 t += (uint32_t)read16() << 16;
450 return t;
451 }
452
453 static void headSerialResponse(uint8_t err, uint8_t responseBodySize)
454 {
455 serialize8('$');
456 serialize8('M');
457 serialize8(err ? '!' : '>');
458 currentPort->checksum = 0; // start calculating a new checksum
459 serialize8(responseBodySize);
460 serialize8(currentPort->cmdMSP);
461 }
462
463 static void headSerialReply(uint8_t responseBodySize)
464 {
465 headSerialResponse(0, responseBodySize);
466 }
467
468 static void headSerialError(uint8_t responseBodySize)
469 {
470 headSerialResponse(1, responseBodySize);
471 }
472
473 static void tailSerialReply(void)
474 {
475 serialize8(currentPort->checksum);
476 }
477
478 static void s_struct(uint8_t *cb, uint8_t siz)
479 {
480 headSerialReply(siz);
481 while (siz--)
482 serialize8(*cb++);
483 }
484
485 static void serializeNames(const char *s)
486 {
487 const char *c;
488 for (c = s; *c; c++)
489 serialize8(*c);
490 }
491
492 static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
493 {
494 uint8_t boxIndex;
495 const box_t *candidate;
496 for (boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
497 candidate = &boxes[boxIndex];
498 if (candidate->boxId == activeBoxId) {
499 return candidate;
500 }
501 }
502 return NULL;
503 }
504
505 static const box_t *findBoxByPermenantId(uint8_t permenantId)
506 {
507 uint8_t boxIndex;
508 const box_t *candidate;
509 for (boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
510 candidate = &boxes[boxIndex];
511 if (candidate->permanentId == permenantId) {
512 return candidate;
513 }
514 }
515 return NULL;
516 }
517
518 static void serializeBoxNamesReply(void)
519 {
520 int i, activeBoxId, j, flag = 1, count = 0, len;
521 const box_t *box;
522
523 reset:
524 // in first run of the loop, we grab total size of junk to be sent
525 // then come back and actually send it
526 for (i = 0; i < activeBoxIdCount; i++) {
527 activeBoxId = activeBoxIds[i];
528
529 box = findBoxByActiveBoxId(activeBoxId);
530 if (!box) {
531 continue;
532 }
533
534 len = strlen(box->boxName);
535 if (flag) {
536 count += len;
537 } else {
538 for (j = 0; j < len; j++)
539 serialize8(box->boxName[j]);
540 }
541 }
542
543 if (flag) {
544 headSerialReply(count);
545 flag = 0;
546 goto reset;
547 }
548 }
549
550 static void serializeDataflashSummaryReply(void)
551 {
552 headSerialReply(1 + 3 * 4);
553 #ifdef USE_FLASHFS
554 const flashGeometry_t *geometry = flashfsGetGeometry();
555 serialize8(flashfsIsReady() ? 1 : 0);
556 serialize32(geometry->sectors);
557 serialize32(geometry->totalSize);
558 serialize32(flashfsGetOffset()); // Effectively the current number of bytes stored on the volume
559 #else
560 serialize8(0);
561 serialize32(0);
562 serialize32(0);
563 serialize32(0);
564 #endif
565 }
566
567 #ifdef USE_FLASHFS
568 static void serializeDataflashReadReply(uint32_t address, uint8_t size)
569 {
570 uint8_t buffer[128];
571 int bytesRead;
572
573 if (size > sizeof(buffer)) {
574 size = sizeof(buffer);
575 }
576
577 headSerialReply(4 + size);
578
579 serialize32(address);
580
581 // bytesRead will be lower than that requested if we reach end of volume
582 bytesRead = flashfsReadAbs(address, buffer, size);
583
584 for (int i = 0; i < bytesRead; i++) {
585 serialize8(buffer[i]);
586 }
587 }
588 #endif
589
590 static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort, mspPortUsage_e usage)
591 {
592 memset(mspPortToReset, 0, sizeof(mspPort_t));
593
594 mspPortToReset->port = serialPort;
595 mspPortToReset->mspPortUsage = usage;
596 }
597
598 void mspAllocateSerialPorts(serialConfig_t *serialConfig)
599 {
600 UNUSED(serialConfig);
601
602 serialPort_t *serialPort;
603
604 uint8_t portIndex = 0;
605
606 serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP);
607
608 while (portConfig && portIndex < MAX_MSP_PORT_COUNT) {
609 mspPort_t *mspPort = &mspPorts[portIndex];
610 if (mspPort->mspPortUsage != UNUSED_PORT) {
611 portIndex++;
612 continue;
613 }
614
615 serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
616 if (serialPort) {
617 resetMspPort(mspPort, serialPort, FOR_GENERAL_MSP);
618 portIndex++;
619 }
620
621 portConfig = findNextSerialPortConfig(FUNCTION_MSP);
622 }
623 }
624
625 void mspReleasePortIfAllocated(serialPort_t *serialPort)
626 {
627 uint8_t portIndex;
628 for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
629 mspPort_t *candidateMspPort = &mspPorts[portIndex];
630 if (candidateMspPort->port == serialPort) {
631 closeSerialPort(serialPort);
632 memset(candidateMspPort, 0, sizeof(mspPort_t));
633 }
634 }
635 }
636
637 void mspInit(serialConfig_t *serialConfig)
638 {
639 // calculate used boxes based on features and fill availableBoxes[] array
640 memset(activeBoxIds, 0xFF, sizeof(activeBoxIds));
641
642 activeBoxIdCount = 0;
643 activeBoxIds[activeBoxIdCount++] = BOXARM;
644
645 if (sensors(SENSOR_ACC)) {
646 activeBoxIds[activeBoxIdCount++] = BOXANGLE;
647 activeBoxIds[activeBoxIdCount++] = BOXHORIZON;
648 }
649
650 if (sensors(SENSOR_BARO)) {
651 activeBoxIds[activeBoxIdCount++] = BOXBARO;
652 }
653
654 if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
655 activeBoxIds[activeBoxIdCount++] = BOXMAG;
656 activeBoxIds[activeBoxIdCount++] = BOXHEADFREE;
657 activeBoxIds[activeBoxIdCount++] = BOXHEADADJ;
658 }
659
660 if (feature(FEATURE_SERVO_TILT))
661 activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB;
662
663 #ifdef GPS
664 if (feature(FEATURE_GPS)) {
665 activeBoxIds[activeBoxIdCount++] = BOXGPSHOME;
666 activeBoxIds[activeBoxIdCount++] = BOXGPSHOLD;
667 }
668 #endif
669
670 if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE)
671 activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
672
673 activeBoxIds[activeBoxIdCount++] = BOXBEEPERON;
674
675 #ifdef LED_STRIP
676 if (feature(FEATURE_LED_STRIP)) {
677 activeBoxIds[activeBoxIdCount++] = BOXLEDLOW;
678 }
679 #endif
680
681 if (feature(FEATURE_INFLIGHT_ACC_CAL))
682 activeBoxIds[activeBoxIdCount++] = BOXCALIB;
683
684 activeBoxIds[activeBoxIdCount++] = BOXOSD;
685
686 if (feature(FEATURE_TELEMETRY) && masterConfig.telemetryConfig.telemetry_switch)
687 activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY;
688
689 if (feature(FEATURE_SONAR)){
690 activeBoxIds[activeBoxIdCount++] = BOXSONAR;
691 }
692
693 #ifdef USE_SERVOS
694 if (masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) {
695 activeBoxIds[activeBoxIdCount++] = BOXSERVO1;
696 activeBoxIds[activeBoxIdCount++] = BOXSERVO2;
697 activeBoxIds[activeBoxIdCount++] = BOXSERVO3;
698 }
699 #endif
700
701 #ifdef BLACKBOX
702 if (feature(FEATURE_BLACKBOX)){
703 activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX;
704 }
705 #endif
706
707 if (feature(FEATURE_FAILSAFE)){
708 activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE;
709 }
710
711 #ifdef GTUNE
712 activeBoxIds[activeBoxIdCount++] = BOXGTUNE;
713 #endif
714
715 memset(mspPorts, 0x00, sizeof(mspPorts));
716 mspAllocateSerialPorts(serialConfig);
717 }
718
719 #define IS_ENABLED(mask) (mask == 0 ? 0 : 1)
720
721 static bool processOutCommand(uint8_t cmdMSP)
722 {
723 uint32_t i, tmp, junk;
724
725 #ifdef GPS
726 uint8_t wp_no;
727 int32_t lat = 0, lon = 0;
728 #endif
729
730 switch (cmdMSP) {
731 case MSP_API_VERSION:
732 headSerialReply(
733 1 + // protocol version length
734 API_VERSION_LENGTH
735 );
736 serialize8(MSP_PROTOCOL_VERSION);
737
738 serialize8(API_VERSION_MAJOR);
739 serialize8(API_VERSION_MINOR);
740 break;
741
742 case MSP_FC_VARIANT:
743 headSerialReply(FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
744
745 for (i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) {
746 serialize8(flightControllerIdentifier[i]);
747 }
748 break;
749
750 case MSP_FC_VERSION:
751 headSerialReply(FLIGHT_CONTROLLER_VERSION_LENGTH);
752
753 serialize8(FC_VERSION_MAJOR);
754 serialize8(FC_VERSION_MINOR);
755 serialize8(FC_VERSION_PATCH_LEVEL);
756 break;
757
758 case MSP_BOARD_INFO:
759 headSerialReply(
760 BOARD_IDENTIFIER_LENGTH +
761 BOARD_HARDWARE_REVISION_LENGTH
762 );
763 for (i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) {
764 serialize8(boardIdentifier[i]);
765 }
766 #ifdef NAZE
767 serialize16(hardwareRevision);
768 #else
769 serialize16(0); // No other build targets currently have hardware revision detection.
770 #endif
771 break;
772
773 case MSP_BUILD_INFO:
774 headSerialReply(
775 BUILD_DATE_LENGTH +
776 BUILD_TIME_LENGTH +
777 GIT_SHORT_REVISION_LENGTH
778 );
779
780 for (i = 0; i < BUILD_DATE_LENGTH; i++) {
781 serialize8(buildDate[i]);
782 }
783 for (i = 0; i < BUILD_TIME_LENGTH; i++) {
784 serialize8(buildTime[i]);
785 }
786
787 for (i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) {
788 serialize8(shortGitRevision[i]);
789 }
790 break;
791
792 // DEPRECATED - Use MSP_API_VERSION
793 case MSP_IDENT:
794 headSerialReply(7);
795 serialize8(MW_VERSION);
796 serialize8(masterConfig.mixerMode);
797 serialize8(MSP_PROTOCOL_VERSION);
798 serialize32(CAP_DYNBALANCE); // "capability"
799 break;
800
801 case MSP_STATUS:
802 headSerialReply(11);
803 serialize16(cycleTime);
804 #ifdef USE_I2C
805 serialize16(i2cGetErrorCounter());
806 #else
807 serialize16(0);
808 #endif
809 serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
810 // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
811 // Requires new Multiwii protocol version to fix
812 // It would be preferable to setting the enabled bits based on BOXINDEX.
813 junk = 0;
814 tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE |
815 IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON |
816 IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << BOXBARO |
817 IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG |
818 IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE |
819 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ |
820 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB |
821 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMTRIG)) << BOXCAMTRIG |
822 IS_ENABLED(FLIGHT_MODE(GPS_HOME_MODE)) << BOXGPSHOME |
823 IS_ENABLED(FLIGHT_MODE(GPS_HOLD_MODE)) << BOXGPSHOLD |
824 IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU |
825 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON |
826 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX)) << BOXLEDMAX |
827 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW |
828 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS |
829 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB)) << BOXCALIB |
830 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
831 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
832 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
833 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
834 IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
835 IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
836 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
837 IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE;
838 for (i = 0; i < activeBoxIdCount; i++) {
839 int flag = (tmp & (1 << activeBoxIds[i]));
840 if (flag)
841 junk |= 1 << i;
842 }
843 serialize32(junk);
844 serialize8(masterConfig.current_profile_index);
845 break;
846 case MSP_RAW_IMU:
847 headSerialReply(18);
848
849 // Hack scale due to choice of units for sensor data in multiwii
850 uint8_t scale = (acc_1G > 1024) ? 8 : 1;
851
852 for (i = 0; i < 3; i++)
853 serialize16(accSmooth[i] / scale);
854 for (i = 0; i < 3; i++)
855 serialize16(gyroADC[i]);
856 for (i = 0; i < 3; i++)
857 serialize16(magADC[i]);
858 break;
859 #ifdef USE_SERVOS
860 case MSP_SERVO:
861 s_struct((uint8_t *)&servo, MAX_SUPPORTED_SERVOS * 2);
862 break;
863 case MSP_SERVO_CONFIGURATIONS:
864 headSerialReply(MAX_SUPPORTED_SERVOS * sizeof(servoParam_t));
865 for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
866 serialize16(currentProfile->servoConf[i].min);
867 serialize16(currentProfile->servoConf[i].max);
868 serialize16(currentProfile->servoConf[i].middle);
869 serialize8(currentProfile->servoConf[i].rate);
870 serialize8(currentProfile->servoConf[i].angleAtMin);
871 serialize8(currentProfile->servoConf[i].angleAtMax);
872 serialize8(currentProfile->servoConf[i].forwardFromChannel);
873 serialize32(currentProfile->servoConf[i].reversedSources);
874 }
875 break;
876 case MSP_SERVO_MIX_RULES:
877 headSerialReply(MAX_SERVO_RULES * sizeof(servoMixer_t));
878 for (i = 0; i < MAX_SERVO_RULES; i++) {
879 serialize8(masterConfig.customServoMixer[i].targetChannel);
880 serialize8(masterConfig.customServoMixer[i].inputSource);
881 serialize8(masterConfig.customServoMixer[i].rate);
882 serialize8(masterConfig.customServoMixer[i].speed);
883 serialize8(masterConfig.customServoMixer[i].min);
884 serialize8(masterConfig.customServoMixer[i].max);
885 serialize8(masterConfig.customServoMixer[i].box);
886 }
887 break;
888 #endif
889 case MSP_MOTOR:
890 s_struct((uint8_t *)motor, 16);
891 break;
892 case MSP_RC:
893 headSerialReply(2 * rxRuntimeConfig.channelCount);
894 for (i = 0; i < rxRuntimeConfig.channelCount; i++)
895 serialize16(rcData[i]);
896 break;
897 case MSP_ATTITUDE:
898 headSerialReply(6);
899 serialize16(attitude.values.roll);
900 serialize16(attitude.values.pitch);
901 serialize16(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
902 break;
903 case MSP_ALTITUDE:
904 headSerialReply(6);
905 #if defined(BARO) || defined(SONAR)
906 serialize32(altitudeHoldGetEstimatedAltitude());
907 #else
908 serialize32(0);
909 #endif
910 serialize16(vario);
911 break;
912 case MSP_SONAR_ALTITUDE:
913 headSerialReply(4);
914 #if defined(SONAR)
915 serialize32(sonarGetLatestAltitude());
916 #else
917 serialize32(0);
918 #endif
919 break;
920 case MSP_ANALOG:
921 headSerialReply(7);
922 serialize8((uint8_t)constrain(vbat, 0, 255));
923 serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
924 serialize16(rssi);
925 if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) {
926 serialize16((uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
927 } else
928 serialize16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
929 break;
930 case MSP_ARMING_CONFIG:
931 headSerialReply(2);
932 serialize8(masterConfig.auto_disarm_delay);
933 serialize8(masterConfig.disarm_kill_switch);
934 break;
935 case MSP_LOOP_TIME:
936 headSerialReply(2);
937 serialize16((uint16_t)targetLooptime);
938 break;
939 case MSP_RC_TUNING:
940 headSerialReply(11);
941 serialize8(currentControlRateProfile->rcRate8);
942 serialize8(currentControlRateProfile->rcExpo8);
943 for (i = 0 ; i < 3; i++) {
944 serialize8(currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
945 }
946 serialize8(currentControlRateProfile->dynThrPID);
947 serialize8(currentControlRateProfile->thrMid8);
948 serialize8(currentControlRateProfile->thrExpo8);
949 serialize16(currentControlRateProfile->tpa_breakpoint);
950 serialize8(currentControlRateProfile->rcYawExpo8);
951 break;
952 case MSP_PID:
953 headSerialReply(3 * PID_ITEM_COUNT);
954 if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid
955 for (i = 0; i < 3; i++) {
956 serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 255));
957 serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 255));
958 serialize8(constrain(lrintf(currentProfile->pidProfile.D_f[i] * 1000.0f), 0, 255));
959 }
960 for (i = 3; i < PID_ITEM_COUNT; i++) {
961 if (i == PIDLEVEL) {
962 serialize8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 255));
963 serialize8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 255));
964 serialize8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity), 0, 255));
965 } else {
966 serialize8(currentProfile->pidProfile.P8[i]);
967 serialize8(currentProfile->pidProfile.I8[i]);
968 serialize8(currentProfile->pidProfile.D8[i]);
969 }
970 }
971 } else {
972 for (i = 0; i < PID_ITEM_COUNT; i++) {
973 serialize8(currentProfile->pidProfile.P8[i]);
974 serialize8(currentProfile->pidProfile.I8[i]);
975 serialize8(currentProfile->pidProfile.D8[i]);
976 }
977 }
978 break;
979 case MSP_PID_FLOAT:
980 headSerialReply(3 * PID_ITEM_COUNT * 2);
981 for (i = 0; i < 3; i++) {
982 serialize16(lrintf(currentProfile->pidProfile.P_f[i] * 1000.0f));
983 serialize16(lrintf(currentProfile->pidProfile.I_f[i] * 1000.0f));
984 serialize16(lrintf(currentProfile->pidProfile.D_f[i] * 1000.0f));
985 }
986 for (i = 3; i < PID_ITEM_COUNT; i++) {
987 if (i == PIDLEVEL) {
988 serialize16(lrintf(currentProfile->pidProfile.A_level * 1000.0f));
989 serialize16(lrintf(currentProfile->pidProfile.H_level * 1000.0f));
990 serialize16(currentProfile->pidProfile.H_sensitivity);
991 }
992 else {
993 serialize16(currentProfile->pidProfile.P8[i]);
994 serialize16(currentProfile->pidProfile.I8[i]);
995 serialize16(currentProfile->pidProfile.D8[i]);
996 }
997 }
998 break;
999 case MSP_PIDNAMES:
1000 headSerialReply(sizeof(pidnames) - 1);
1001 serializeNames(pidnames);
1002 break;
1003 case MSP_PID_CONTROLLER:
1004 headSerialReply(1);
1005 serialize8(currentProfile->pidProfile.pidController);
1006 break;
1007 case MSP_MODE_RANGES:
1008 headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT);
1009 for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
1010 modeActivationCondition_t *mac = &currentProfile->modeActivationConditions[i];
1011 const box_t *box = &boxes[mac->modeId];
1012 serialize8(box->permanentId);
1013 serialize8(mac->auxChannelIndex);
1014 serialize8(mac->range.startStep);
1015 serialize8(mac->range.endStep);
1016 }
1017 break;
1018 case MSP_ADJUSTMENT_RANGES:
1019 headSerialReply(MAX_ADJUSTMENT_RANGE_COUNT * (
1020 1 + // adjustment index/slot
1021 1 + // aux channel index
1022 1 + // start step
1023 1 + // end step
1024 1 + // adjustment function
1025 1 // aux switch channel index
1026 ));
1027 for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
1028 adjustmentRange_t *adjRange = &currentProfile->adjustmentRanges[i];
1029 serialize8(adjRange->adjustmentIndex);
1030 serialize8(adjRange->auxChannelIndex);
1031 serialize8(adjRange->range.startStep);
1032 serialize8(adjRange->range.endStep);
1033 serialize8(adjRange->adjustmentFunction);
1034 serialize8(adjRange->auxSwitchChannelIndex);
1035 }
1036 break;
1037 case MSP_BOXNAMES:
1038 serializeBoxNamesReply();
1039 break;
1040 case MSP_BOXIDS:
1041 headSerialReply(activeBoxIdCount);
1042 for (i = 0; i < activeBoxIdCount; i++) {
1043 const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]);
1044 if (!box) {
1045 continue;
1046 }
1047 serialize8(box->permanentId);
1048 }
1049 break;
1050 case MSP_MISC:
1051 headSerialReply(2 * 5 + 3 + 3 + 2 + 4);
1052 serialize16(masterConfig.rxConfig.midrc);
1053
1054 serialize16(masterConfig.escAndServoConfig.minthrottle);
1055 serialize16(masterConfig.escAndServoConfig.maxthrottle);
1056 serialize16(masterConfig.escAndServoConfig.mincommand);
1057
1058 serialize16(masterConfig.failsafeConfig.failsafe_throttle);
1059
1060 #ifdef GPS
1061 serialize8(masterConfig.gpsConfig.provider); // gps_type
1062 serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
1063 serialize8(masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas
1064 #else
1065 serialize8(0); // gps_type
1066 serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
1067 serialize8(0); // gps_ubx_sbas
1068 #endif
1069 serialize8(masterConfig.batteryConfig.multiwiiCurrentMeterOutput);
1070 serialize8(masterConfig.rxConfig.rssi_channel);
1071 serialize8(0);
1072
1073 serialize16(currentProfile->mag_declination / 10);
1074
1075 serialize8(masterConfig.batteryConfig.vbatscale);
1076 serialize8(masterConfig.batteryConfig.vbatmincellvoltage);
1077 serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage);
1078 serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage);
1079 break;
1080
1081 case MSP_MOTOR_PINS:
1082 // FIXME This is hardcoded and should not be.
1083 headSerialReply(8);
1084 for (i = 0; i < 8; i++)
1085 serialize8(i + 1);
1086 break;
1087 #ifdef GPS
1088 case MSP_RAW_GPS:
1089 headSerialReply(16);
1090 serialize8(STATE(GPS_FIX));
1091 serialize8(GPS_numSat);
1092 serialize32(GPS_coord[LAT]);
1093 serialize32(GPS_coord[LON]);
1094 serialize16(GPS_altitude);
1095 serialize16(GPS_speed);
1096 serialize16(GPS_ground_course);
1097 break;
1098 case MSP_COMP_GPS:
1099 headSerialReply(5);
1100 serialize16(GPS_distanceToHome);
1101 serialize16(GPS_directionToHome);
1102 serialize8(GPS_update & 1);
1103 break;
1104 case MSP_WP:
1105 wp_no = read8(); // get the wp number
1106 headSerialReply(18);
1107 if (wp_no == 0) {
1108 lat = GPS_home[LAT];
1109 lon = GPS_home[LON];
1110 } else if (wp_no == 16) {
1111 lat = GPS_hold[LAT];
1112 lon = GPS_hold[LON];
1113 }
1114 serialize8(wp_no);
1115 serialize32(lat);
1116 serialize32(lon);
1117 serialize32(AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps
1118 serialize16(0); // heading will come here (deg)
1119 serialize16(0); // time to stay (ms) will come here
1120 serialize8(0); // nav flag will come here
1121 break;
1122 case MSP_GPSSVINFO:
1123 headSerialReply(1 + (GPS_numCh * 4));
1124 serialize8(GPS_numCh);
1125 for (i = 0; i < GPS_numCh; i++){
1126 serialize8(GPS_svinfo_chn[i]);
1127 serialize8(GPS_svinfo_svid[i]);
1128 serialize8(GPS_svinfo_quality[i]);
1129 serialize8(GPS_svinfo_cno[i]);
1130 }
1131 break;
1132 #endif
1133 case MSP_DEBUG:
1134 headSerialReply(DEBUG16_VALUE_COUNT * sizeof(debug[0]));
1135
1136 // output some useful QA statistics
1137 // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock]
1138
1139 for (i = 0; i < DEBUG16_VALUE_COUNT; i++)
1140 serialize16(debug[i]); // 4 variables are here for general monitoring purpose
1141 break;
1142
1143 // Additional commands that are not compatible with MultiWii
1144 case MSP_ACC_TRIM:
1145 headSerialReply(4);
1146 serialize16(currentProfile->accelerometerTrims.values.pitch);
1147 serialize16(currentProfile->accelerometerTrims.values.roll);
1148 break;
1149
1150 case MSP_UID:
1151 headSerialReply(12);
1152 serialize32(U_ID_0);
1153 serialize32(U_ID_1);
1154 serialize32(U_ID_2);
1155 break;
1156
1157 case MSP_FEATURE:
1158 headSerialReply(4);
1159 serialize32(featureMask());
1160 break;
1161
1162 case MSP_BOARD_ALIGNMENT:
1163 headSerialReply(6);
1164 serialize16(masterConfig.boardAlignment.rollDegrees);
1165 serialize16(masterConfig.boardAlignment.pitchDegrees);
1166 serialize16(masterConfig.boardAlignment.yawDegrees);
1167 break;
1168
1169 case MSP_VOLTAGE_METER_CONFIG:
1170 headSerialReply(4);
1171 serialize8(masterConfig.batteryConfig.vbatscale);
1172 serialize8(masterConfig.batteryConfig.vbatmincellvoltage);
1173 serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage);
1174 serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage);
1175 break;
1176
1177 case MSP_CURRENT_METER_CONFIG:
1178 headSerialReply(7);
1179 serialize16(masterConfig.batteryConfig.currentMeterScale);
1180 serialize16(masterConfig.batteryConfig.currentMeterOffset);
1181 serialize8(masterConfig.batteryConfig.currentMeterType);
1182 serialize16(masterConfig.batteryConfig.batteryCapacity);
1183 break;
1184
1185 case MSP_MIXER:
1186 headSerialReply(1);
1187 serialize8(masterConfig.mixerMode);
1188 break;
1189
1190 case MSP_RX_CONFIG:
1191 headSerialReply(12);
1192 serialize8(masterConfig.rxConfig.serialrx_provider);
1193 serialize16(masterConfig.rxConfig.maxcheck);
1194 serialize16(masterConfig.rxConfig.midrc);
1195 serialize16(masterConfig.rxConfig.mincheck);
1196 serialize8(masterConfig.rxConfig.spektrum_sat_bind);
1197 serialize16(masterConfig.rxConfig.rx_min_usec);
1198 serialize16(masterConfig.rxConfig.rx_max_usec);
1199 break;
1200
1201 case MSP_FAILSAFE_CONFIG:
1202 headSerialReply(4);
1203 serialize8(masterConfig.failsafeConfig.failsafe_delay);
1204 serialize8(masterConfig.failsafeConfig.failsafe_off_delay);
1205 serialize16(masterConfig.failsafeConfig.failsafe_throttle);
1206 break;
1207
1208 case MSP_RXFAIL_CONFIG:
1209 headSerialReply(3 * (rxRuntimeConfig.channelCount));
1210 for (i = 0; i < rxRuntimeConfig.channelCount; i++) {
1211 serialize8(masterConfig.rxConfig.failsafe_channel_configurations[i].mode);
1212 serialize16(RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.failsafe_channel_configurations[i].step));
1213 }
1214 break;
1215
1216 case MSP_RSSI_CONFIG:
1217 headSerialReply(1);
1218 serialize8(masterConfig.rxConfig.rssi_channel);
1219 break;
1220
1221 case MSP_RX_MAP:
1222 headSerialReply(MAX_MAPPABLE_RX_INPUTS);
1223 for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++)
1224 serialize8(masterConfig.rxConfig.rcmap[i]);
1225 break;
1226
1227 case MSP_BF_CONFIG:
1228 headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 2 + 2);
1229 serialize8(masterConfig.mixerMode);
1230
1231 serialize32(featureMask());
1232
1233 serialize8(masterConfig.rxConfig.serialrx_provider);
1234
1235 serialize16(masterConfig.boardAlignment.rollDegrees);
1236 serialize16(masterConfig.boardAlignment.pitchDegrees);
1237 serialize16(masterConfig.boardAlignment.yawDegrees);
1238
1239 serialize16(masterConfig.batteryConfig.currentMeterScale);
1240 serialize16(masterConfig.batteryConfig.currentMeterOffset);
1241 break;
1242
1243 case MSP_CF_SERIAL_CONFIG:
1244 headSerialReply(
1245 ((sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4)) * serialGetAvailablePortCount())
1246 );
1247 for (i = 0; i < SERIAL_PORT_COUNT; i++) {
1248 if (!serialIsPortAvailable(masterConfig.serialConfig.portConfigs[i].identifier)) {
1249 continue;
1250 };
1251 serialize8(masterConfig.serialConfig.portConfigs[i].identifier);
1252 serialize16(masterConfig.serialConfig.portConfigs[i].functionMask);
1253 serialize8(masterConfig.serialConfig.portConfigs[i].msp_baudrateIndex);
1254 serialize8(masterConfig.serialConfig.portConfigs[i].gps_baudrateIndex);
1255 serialize8(masterConfig.serialConfig.portConfigs[i].telemetry_baudrateIndex);
1256 serialize8(masterConfig.serialConfig.portConfigs[i].blackbox_baudrateIndex);
1257 }
1258 break;
1259
1260 #ifdef LED_STRIP
1261 case MSP_LED_COLORS:
1262 headSerialReply(CONFIGURABLE_COLOR_COUNT * 4);
1263 for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) {
1264 hsvColor_t *color = &masterConfig.colors[i];
1265 serialize16(color->h);
1266 serialize8(color->s);
1267 serialize8(color->v);
1268 }
1269 break;
1270
1271 case MSP_LED_STRIP_CONFIG:
1272 headSerialReply(MAX_LED_STRIP_LENGTH * 7);
1273 for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) {
1274 ledConfig_t *ledConfig = &masterConfig.ledConfigs[i];
1275 serialize16((ledConfig->flags & LED_DIRECTION_MASK) >> LED_DIRECTION_BIT_OFFSET);
1276 serialize16((ledConfig->flags & LED_FUNCTION_MASK) >> LED_FUNCTION_BIT_OFFSET);
1277 serialize8(GET_LED_X(ledConfig));
1278 serialize8(GET_LED_Y(ledConfig));
1279 serialize8(ledConfig->color);
1280 }
1281 break;
1282 #endif
1283
1284 case MSP_DATAFLASH_SUMMARY:
1285 serializeDataflashSummaryReply();
1286 break;
1287
1288 #ifdef USE_FLASHFS
1289 case MSP_DATAFLASH_READ:
1290 {
1291 uint32_t readAddress = read32();
1292
1293 serializeDataflashReadReply(readAddress, 128);
1294 }
1295 break;
1296 #endif
1297
1298 case MSP_BF_BUILD_INFO:
1299 headSerialReply(11 + 4 + 4);
1300 for (i = 0; i < 11; i++)
1301 serialize8(buildDate[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
1302 serialize32(0); // future exp
1303 serialize32(0); // future exp
1304 break;
1305
1306 default:
1307 return false;
1308 }
1309 return true;
1310 }
1311
1312 static bool processInCommand(void)
1313 {
1314 uint32_t i;
1315 uint16_t tmp;
1316 uint8_t rate;
1317 #ifdef GPS
1318 uint8_t wp_no;
1319 int32_t lat = 0, lon = 0, alt = 0;
1320 #endif
1321
1322 switch (currentPort->cmdMSP) {
1323 case MSP_SELECT_SETTING:
1324 if (!ARMING_FLAG(ARMED)) {
1325 masterConfig.current_profile_index = read8();
1326 if (masterConfig.current_profile_index > 2) {
1327 masterConfig.current_profile_index = 0;
1328 }
1329 writeEEPROM();
1330 readEEPROM();
1331 }
1332 break;
1333 case MSP_SET_HEAD:
1334 magHold = read16();
1335 break;
1336 case MSP_SET_RAW_RC:
1337 {
1338 uint8_t channelCount = currentPort->dataSize / sizeof(uint16_t);
1339 if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) {
1340 headSerialError(0);
1341 } else {
1342 uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
1343
1344 for (i = 0; i < channelCount; i++) {
1345 frame[i] = read16();
1346 }
1347
1348 rxMspFrameReceive(frame, channelCount);
1349 }
1350 }
1351 break;
1352 case MSP_SET_ACC_TRIM:
1353 currentProfile->accelerometerTrims.values.pitch = read16();
1354 currentProfile->accelerometerTrims.values.roll = read16();
1355 break;
1356 case MSP_SET_ARMING_CONFIG:
1357 masterConfig.auto_disarm_delay = read8();
1358 masterConfig.disarm_kill_switch = read8();
1359 break;
1360 case MSP_SET_LOOP_TIME:
1361 break;
1362 case MSP_SET_PID_CONTROLLER:
1363 currentProfile->pidProfile.pidController = constrain(read8(), 1, 2); // Temporary configurator compatibility
1364 pidSetController(currentProfile->pidProfile.pidController);
1365 break;
1366 case MSP_SET_PID:
1367 if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) {
1368 for (i = 0; i < 3; i++) {
1369 currentProfile->pidProfile.P_f[i] = (float)read8() / 10.0f;
1370 currentProfile->pidProfile.I_f[i] = (float)read8() / 100.0f;
1371 currentProfile->pidProfile.D_f[i] = (float)read8() / 1000.0f;
1372 }
1373 for (i = 3; i < PID_ITEM_COUNT; i++) {
1374 if (i == PIDLEVEL) {
1375 currentProfile->pidProfile.A_level = (float)read8() / 10.0f;
1376 currentProfile->pidProfile.H_level = (float)read8() / 10.0f;
1377 currentProfile->pidProfile.H_sensitivity = read8();
1378 } else {
1379 currentProfile->pidProfile.P8[i] = read8();
1380 currentProfile->pidProfile.I8[i] = read8();
1381 currentProfile->pidProfile.D8[i] = read8();
1382 }
1383 }
1384 } else {
1385 for (i = 0; i < PID_ITEM_COUNT; i++) {
1386 currentProfile->pidProfile.P8[i] = read8();
1387 currentProfile->pidProfile.I8[i] = read8();
1388 currentProfile->pidProfile.D8[i] = read8();
1389 }
1390 }
1391 break;
1392 case MSP_SET_PID_FLOAT:
1393 for (i = 0; i < 3; i++) {
1394 currentProfile->pidProfile.P_f[i] = (float)read16() / 1000.0f;
1395 currentProfile->pidProfile.I_f[i] = (float)read16() / 1000.0f;
1396 currentProfile->pidProfile.D_f[i] = (float)read16() / 1000.0f;
1397 }
1398 for (i = 3; i < PID_ITEM_COUNT; i++) {
1399 if (i == PIDLEVEL) {
1400 currentProfile->pidProfile.A_level = (float)read16() / 1000.0f;
1401 currentProfile->pidProfile.H_level = (float)read16() / 1000.0f;
1402 currentProfile->pidProfile.H_sensitivity = read16();
1403 }
1404 else {
1405 currentProfile->pidProfile.P8[i] = read16();
1406 currentProfile->pidProfile.I8[i] = read16();
1407 currentProfile->pidProfile.D8[i] = read16();
1408 }
1409 }
1410 break;
1411 case MSP_SET_MODE_RANGE:
1412 i = read8();
1413 if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
1414 modeActivationCondition_t *mac = &currentProfile->modeActivationConditions[i];
1415 i = read8();
1416 const box_t *box = findBoxByPermenantId(i);
1417 if (box) {
1418 mac->modeId = box->boxId;
1419 mac->auxChannelIndex = read8();
1420 mac->range.startStep = read8();
1421 mac->range.endStep = read8();
1422
1423 useRcControlsConfig(currentProfile->modeActivationConditions, &masterConfig.escAndServoConfig, &currentProfile->pidProfile);
1424 } else {
1425 headSerialError(0);
1426 }
1427 } else {
1428 headSerialError(0);
1429 }
1430 break;
1431 case MSP_SET_ADJUSTMENT_RANGE:
1432 i = read8();
1433 if (i < MAX_ADJUSTMENT_RANGE_COUNT) {
1434 adjustmentRange_t *adjRange = &currentProfile->adjustmentRanges[i];
1435 i = read8();
1436 if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) {
1437 adjRange->adjustmentIndex = i;
1438 adjRange->auxChannelIndex = read8();
1439 adjRange->range.startStep = read8();
1440 adjRange->range.endStep = read8();
1441 adjRange->adjustmentFunction = read8();
1442 adjRange->auxSwitchChannelIndex = read8();
1443 } else {
1444 headSerialError(0);
1445 }
1446 } else {
1447 headSerialError(0);
1448 }
1449 break;
1450
1451 case MSP_SET_RC_TUNING:
1452 if (currentPort->dataSize >= 10) {
1453 currentControlRateProfile->rcRate8 = read8();
1454 currentControlRateProfile->rcExpo8 = read8();
1455 for (i = 0; i < 3; i++) {
1456 rate = read8();
1457 currentControlRateProfile->rates[i] = MIN(rate, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
1458 }
1459 rate = read8();
1460 currentControlRateProfile->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX);
1461 currentControlRateProfile->thrMid8 = read8();
1462 currentControlRateProfile->thrExpo8 = read8();
1463 currentControlRateProfile->tpa_breakpoint = read16();
1464 if (currentPort->dataSize >= 11) {
1465 currentControlRateProfile->rcYawExpo8 = read8();
1466 }
1467 } else {
1468 headSerialError(0);
1469 }
1470 break;
1471 case MSP_SET_MISC:
1472 tmp = read16();
1473 if (tmp < 1600 && tmp > 1400)
1474 masterConfig.rxConfig.midrc = tmp;
1475
1476 masterConfig.escAndServoConfig.minthrottle = read16();
1477 masterConfig.escAndServoConfig.maxthrottle = read16();
1478 masterConfig.escAndServoConfig.mincommand = read16();
1479
1480 masterConfig.failsafeConfig.failsafe_throttle = read16();
1481
1482 #ifdef GPS
1483 masterConfig.gpsConfig.provider = read8(); // gps_type
1484 read8(); // gps_baudrate
1485 masterConfig.gpsConfig.sbasMode = read8(); // gps_ubx_sbas
1486 #else
1487 read8(); // gps_type
1488 read8(); // gps_baudrate
1489 read8(); // gps_ubx_sbas
1490 #endif
1491 masterConfig.batteryConfig.multiwiiCurrentMeterOutput = read8();
1492 masterConfig.rxConfig.rssi_channel = read8();
1493 read8();
1494
1495 currentProfile->mag_declination = read16() * 10;
1496
1497 masterConfig.batteryConfig.vbatscale = read8(); // actual vbatscale as intended
1498 masterConfig.batteryConfig.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI
1499 masterConfig.batteryConfig.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI
1500 masterConfig.batteryConfig.vbatwarningcellvoltage = read8(); // vbatlevel when buzzer starts to alert
1501 break;
1502 case MSP_SET_MOTOR:
1503 for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8
1504 motor_disarmed[i] = read16();
1505 break;
1506 case MSP_SET_SERVO_CONFIGURATION:
1507 #ifdef USE_SERVOS
1508 if (currentPort->dataSize != 1 + sizeof(servoParam_t)) {
1509 headSerialError(0);
1510 break;
1511 }
1512 i = read8();
1513 if (i >= MAX_SUPPORTED_SERVOS) {
1514 headSerialError(0);
1515 } else {
1516 currentProfile->servoConf[i].min = read16();
1517 currentProfile->servoConf[i].max = read16();
1518 currentProfile->servoConf[i].middle = read16();
1519 currentProfile->servoConf[i].rate = read8();
1520 currentProfile->servoConf[i].angleAtMin = read8();
1521 currentProfile->servoConf[i].angleAtMax = read8();
1522 currentProfile->servoConf[i].forwardFromChannel = read8();
1523 currentProfile->servoConf[i].reversedSources = read32();
1524 }
1525 #endif
1526 break;
1527
1528 case MSP_SET_SERVO_MIX_RULE:
1529 #ifdef USE_SERVOS
1530 i = read8();
1531 if (i >= MAX_SERVO_RULES) {
1532 headSerialError(0);
1533 } else {
1534 masterConfig.customServoMixer[i].targetChannel = read8();
1535 masterConfig.customServoMixer[i].inputSource = read8();
1536 masterConfig.customServoMixer[i].rate = read8();
1537 masterConfig.customServoMixer[i].speed = read8();
1538 masterConfig.customServoMixer[i].min = read8();
1539 masterConfig.customServoMixer[i].max = read8();
1540 masterConfig.customServoMixer[i].box = read8();
1541 loadCustomServoMixer();
1542 }
1543 #endif
1544 break;
1545
1546 case MSP_RESET_CONF:
1547 if (!ARMING_FLAG(ARMED)) {
1548 resetEEPROM();
1549 readEEPROM();
1550 }
1551 break;
1552 case MSP_ACC_CALIBRATION:
1553 if (!ARMING_FLAG(ARMED))
1554 accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
1555 break;
1556 case MSP_MAG_CALIBRATION:
1557 if (!ARMING_FLAG(ARMED))
1558 ENABLE_STATE(CALIBRATE_MAG);
1559 break;
1560 case MSP_EEPROM_WRITE:
1561 if (ARMING_FLAG(ARMED)) {
1562 headSerialError(0);
1563 return true;
1564 }
1565 writeEEPROM();
1566 readEEPROM();
1567 break;
1568
1569 #ifdef USE_FLASHFS
1570 case MSP_DATAFLASH_ERASE:
1571 flashfsEraseCompletely();
1572 break;
1573 #endif
1574
1575 #ifdef GPS
1576 case MSP_SET_RAW_GPS:
1577 if (read8()) {
1578 ENABLE_STATE(GPS_FIX);
1579 } else {
1580 DISABLE_STATE(GPS_FIX);
1581 }
1582 GPS_numSat = read8();
1583 GPS_coord[LAT] = read32();
1584 GPS_coord[LON] = read32();
1585 GPS_altitude = read16();
1586 GPS_speed = read16();
1587 GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers
1588 break;
1589 case MSP_SET_WP:
1590 wp_no = read8(); //get the wp number
1591 lat = read32();
1592 lon = read32();
1593 alt = read32(); // to set altitude (cm)
1594 read16(); // future: to set heading (deg)
1595 read16(); // future: to set time to stay (ms)
1596 read8(); // future: to set nav flag
1597 if (wp_no == 0) {
1598 GPS_home[LAT] = lat;
1599 GPS_home[LON] = lon;
1600 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
1601 ENABLE_STATE(GPS_FIX_HOME);
1602 if (alt != 0)
1603 AltHold = alt; // temporary implementation to test feature with apps
1604 } 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
1605 GPS_hold[LAT] = lat;
1606 GPS_hold[LON] = lon;
1607 if (alt != 0)
1608 AltHold = alt; // temporary implementation to test feature with apps
1609 nav_mode = NAV_MODE_WP;
1610 GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
1611 }
1612 break;
1613 #endif
1614 case MSP_SET_FEATURE:
1615 featureClearAll();
1616 featureSet(read32()); // features bitmap
1617 break;
1618
1619 case MSP_SET_BOARD_ALIGNMENT:
1620 masterConfig.boardAlignment.rollDegrees = read16();
1621 masterConfig.boardAlignment.pitchDegrees = read16();
1622 masterConfig.boardAlignment.yawDegrees = read16();
1623 break;
1624
1625 case MSP_SET_VOLTAGE_METER_CONFIG:
1626 masterConfig.batteryConfig.vbatscale = read8(); // actual vbatscale as intended
1627 masterConfig.batteryConfig.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI
1628 masterConfig.batteryConfig.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI
1629 masterConfig.batteryConfig.vbatwarningcellvoltage = read8(); // vbatlevel when buzzer starts to alert
1630 break;
1631
1632 case MSP_SET_CURRENT_METER_CONFIG:
1633 masterConfig.batteryConfig.currentMeterScale = read16();
1634 masterConfig.batteryConfig.currentMeterOffset = read16();
1635 masterConfig.batteryConfig.currentMeterType = read8();
1636 masterConfig.batteryConfig.batteryCapacity = read16();
1637 break;
1638
1639 #ifndef USE_QUAD_MIXER_ONLY
1640 case MSP_SET_MIXER:
1641 masterConfig.mixerMode = read8();
1642 break;
1643 #endif
1644
1645 case MSP_SET_RX_CONFIG:
1646 masterConfig.rxConfig.serialrx_provider = read8();
1647 masterConfig.rxConfig.maxcheck = read16();
1648 masterConfig.rxConfig.midrc = read16();
1649 masterConfig.rxConfig.mincheck = read16();
1650 masterConfig.rxConfig.spektrum_sat_bind = read8();
1651 if (currentPort->dataSize > 8) {
1652 masterConfig.rxConfig.rx_min_usec = read16();
1653 masterConfig.rxConfig.rx_max_usec = read16();
1654 }
1655 break;
1656
1657 case MSP_SET_FAILSAFE_CONFIG:
1658 masterConfig.failsafeConfig.failsafe_delay = read8();
1659 masterConfig.failsafeConfig.failsafe_off_delay = read8();
1660 masterConfig.failsafeConfig.failsafe_throttle = read16();
1661 break;
1662
1663 case MSP_SET_RXFAIL_CONFIG:
1664 {
1665 uint8_t channelCount = currentPort->dataSize / 3;
1666 if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) {
1667 headSerialError(0);
1668 } else {
1669 for (i = 0; i < channelCount; i++) {
1670 masterConfig.rxConfig.failsafe_channel_configurations[i].mode = read8();
1671 masterConfig.rxConfig.failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(read16());
1672 }
1673 }
1674 }
1675 break;
1676
1677 case MSP_SET_RSSI_CONFIG:
1678 masterConfig.rxConfig.rssi_channel = read8();
1679 break;
1680
1681 case MSP_SET_RX_MAP:
1682 for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) {
1683 masterConfig.rxConfig.rcmap[i] = read8();
1684 }
1685 break;
1686
1687 case MSP_SET_BF_CONFIG:
1688
1689 #ifdef USE_QUAD_MIXER_ONLY
1690 read8(); // mixerMode ignored
1691 #else
1692 masterConfig.mixerMode = read8(); // mixerMode
1693 #endif
1694
1695 featureClearAll();
1696 featureSet(read32()); // features bitmap
1697
1698 masterConfig.rxConfig.serialrx_provider = read8(); // serialrx_type
1699
1700 masterConfig.boardAlignment.rollDegrees = read16(); // board_align_roll
1701 masterConfig.boardAlignment.pitchDegrees = read16(); // board_align_pitch
1702 masterConfig.boardAlignment.yawDegrees = read16(); // board_align_yaw
1703
1704 masterConfig.batteryConfig.currentMeterScale = read16();
1705 masterConfig.batteryConfig.currentMeterOffset = read16();
1706 break;
1707
1708 case MSP_SET_CF_SERIAL_CONFIG:
1709 {
1710 uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4);
1711
1712 if (currentPort->dataSize % portConfigSize != 0) {
1713 headSerialError(0);
1714 break;
1715 }
1716
1717 uint8_t remainingPortsInPacket = currentPort->dataSize / portConfigSize;
1718
1719 while (remainingPortsInPacket--) {
1720 uint8_t identifier = read8();
1721
1722 serialPortConfig_t *portConfig = serialFindPortConfiguration(identifier);
1723 if (!portConfig) {
1724 headSerialError(0);
1725 break;
1726 }
1727
1728 portConfig->identifier = identifier;
1729 portConfig->functionMask = read16();
1730 portConfig->msp_baudrateIndex = read8();
1731 portConfig->gps_baudrateIndex = read8();
1732 portConfig->telemetry_baudrateIndex = read8();
1733 portConfig->blackbox_baudrateIndex = read8();
1734 }
1735 }
1736 break;
1737
1738 #ifdef LED_STRIP
1739 case MSP_SET_LED_COLORS:
1740 for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) {
1741 hsvColor_t *color = &masterConfig.colors[i];
1742 color->h = read16();
1743 color->s = read8();
1744 color->v = read8();
1745 }
1746 break;
1747
1748 case MSP_SET_LED_STRIP_CONFIG:
1749 {
1750 i = read8();
1751 if (i >= MAX_LED_STRIP_LENGTH || currentPort->dataSize != (1 + 7)) {
1752 headSerialError(0);
1753 break;
1754 }
1755 ledConfig_t *ledConfig = &masterConfig.ledConfigs[i];
1756 uint16_t mask;
1757 // currently we're storing directions and functions in a uint16 (flags)
1758 // the msp uses 2 x uint16_t to cater for future expansion
1759 mask = read16();
1760 ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK;
1761
1762 mask = read16();
1763 ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK;
1764
1765 mask = read8();
1766 ledConfig->xy = CALCULATE_LED_X(mask);
1767
1768 mask = read8();
1769 ledConfig->xy |= CALCULATE_LED_Y(mask);
1770
1771 ledConfig->color = read8();
1772
1773 reevalulateLedConfig();
1774 }
1775 break;
1776 #endif
1777 case MSP_REBOOT:
1778 isRebootScheduled = true;
1779 break;
1780
1781 #ifdef USE_SERIAL_1WIRE
1782 case MSP_SET_1WIRE:
1783 // get channel number
1784 i = read8();
1785 // we do not give any data back, assume channel number is transmitted OK
1786 if (i == 0xFF) {
1787 // 0xFF -> preinitialize the Passthrough
1788 // switch all motor lines HI
1789 usb1WireInitialize();
1790 // and come back right afterwards
1791 // rem: App: Wait at least appx. 500 ms for BLHeli to jump into
1792 // bootloader mode before try to connect any ESC
1793 }
1794 else {
1795 // Check for channel number 0..ESC_COUNT-1
1796 if (i < ESC_COUNT) {
1797 // because we do not come back after calling usb1WirePassthrough
1798 // proceed with a success reply first
1799 headSerialReply(0);
1800 tailSerialReply();
1801 // wait for all data to send
1802 while (!isSerialTransmitBufferEmpty(mspSerialPort)) {
1803 delay(50);
1804 }
1805 // Start to activate here
1806 // motor 1 => index 0
1807 usb1WirePassthrough(i);
1808 // MPS uart is active again
1809 } else {
1810 // ESC channel higher than max. allowed
1811 // rem: BLHeliSuite will not support more than 8
1812 headSerialError(0);
1813 }
1814 // proceed as usual with MSP commands
1815 // and wait to switch to next channel
1816 // rem: App needs to call MSP_BOOT to deinitialize Passthrough
1817 }
1818 break;
1819 #endif
1820 default:
1821 // we do not know how to handle the (valid) message, indicate error MSP $M!
1822 return false;
1823 }
1824 headSerialReply(0);
1825 return true;
1826 }
1827
1828 static void mspProcessReceivedCommand() {
1829 if (!(processOutCommand(currentPort->cmdMSP) || processInCommand())) {
1830 headSerialError(0);
1831 }
1832 tailSerialReply();
1833 currentPort->c_state = IDLE;
1834 }
1835
1836 static bool mspProcessReceivedData(uint8_t c)
1837 {
1838 if (currentPort->c_state == IDLE) {
1839 if (c == '$') {
1840 currentPort->c_state = HEADER_START;
1841 } else {
1842 return false;
1843 }
1844 } else if (currentPort->c_state == HEADER_START) {
1845 currentPort->c_state = (c == 'M') ? HEADER_M : IDLE;
1846 } else if (currentPort->c_state == HEADER_M) {
1847 currentPort->c_state = (c == '<') ? HEADER_ARROW : IDLE;
1848 } else if (currentPort->c_state == HEADER_ARROW) {
1849 if (c > INBUF_SIZE) {
1850 currentPort->c_state = IDLE;
1851
1852 } else {
1853 currentPort->dataSize = c;
1854 currentPort->offset = 0;
1855 currentPort->checksum = 0;
1856 currentPort->indRX = 0;
1857 currentPort->checksum ^= c;
1858 currentPort->c_state = HEADER_SIZE;
1859 }
1860 } else if (currentPort->c_state == HEADER_SIZE) {
1861 currentPort->cmdMSP = c;
1862 currentPort->checksum ^= c;
1863 currentPort->c_state = HEADER_CMD;
1864 } else if (currentPort->c_state == HEADER_CMD && currentPort->offset < currentPort->dataSize) {
1865 currentPort->checksum ^= c;
1866 currentPort->inBuf[currentPort->offset++] = c;
1867 } else if (currentPort->c_state == HEADER_CMD && currentPort->offset >= currentPort->dataSize) {
1868 if (currentPort->checksum == c) {
1869 currentPort->c_state = COMMAND_RECEIVED;
1870 } else {
1871 currentPort->c_state = IDLE;
1872 }
1873 }
1874 return true;
1875 }
1876
1877 void setCurrentPort(mspPort_t *port)
1878 {
1879 currentPort = port;
1880 mspSerialPort = currentPort->port;
1881 }
1882
1883 void mspProcess(void)
1884 {
1885 uint8_t portIndex;
1886 mspPort_t *candidatePort;
1887
1888 for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
1889 candidatePort = &mspPorts[portIndex];
1890 if (candidatePort->mspPortUsage != FOR_GENERAL_MSP) {
1891 continue;
1892 }
1893
1894 setCurrentPort(candidatePort);
1895
1896 while (serialRxBytesWaiting(mspSerialPort)) {
1897
1898 uint8_t c = serialRead(mspSerialPort);
1899 bool consumed = mspProcessReceivedData(c);
1900
1901 if (!consumed && !ARMING_FLAG(ARMED)) {
1902 evaluateOtherData(mspSerialPort, c);
1903 }
1904
1905 if (currentPort->c_state == COMMAND_RECEIVED) {
1906 mspProcessReceivedCommand();
1907 break; // process one command at a time so as not to block.
1908 }
1909 }
1910
1911 if (isRebootScheduled) {
1912 // pause a little while to allow response to be sent
1913 while (!isSerialTransmitBufferEmpty(candidatePort->port)) {
1914 delay(50);
1915 }
1916 stopMotors();
1917 handleOneshotFeatureChangeOnRestart();
1918 systemReset();
1919 }
1920 }
1921 }
1922
1923 static const uint8_t mspTelemetryCommandSequence[] = {
1924 MSP_BOXNAMES, // repeat boxnames, in case the first transmission was lost or never received.
1925 MSP_STATUS,
1926 MSP_IDENT,
1927 MSP_RAW_IMU,
1928 MSP_ALTITUDE,
1929 MSP_RAW_GPS,
1930 MSP_RC,
1931 MSP_MOTOR_PINS,
1932 MSP_ATTITUDE,
1933 MSP_SERVO
1934 };
1935
1936 #define TELEMETRY_MSP_COMMAND_SEQUENCE_ENTRY_COUNT (sizeof(mspTelemetryCommandSequence) / sizeof(mspTelemetryCommandSequence[0]))
1937
1938 static mspPort_t *mspTelemetryPort = NULL;
1939
1940 void mspSetTelemetryPort(serialPort_t *serialPort)
1941 {
1942 uint8_t portIndex;
1943 mspPort_t *candidatePort = NULL;
1944 mspPort_t *matchedPort = NULL;
1945
1946 // find existing telemetry port
1947 for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
1948 candidatePort = &mspPorts[portIndex];
1949 if (candidatePort->mspPortUsage == FOR_TELEMETRY) {
1950 matchedPort = candidatePort;
1951 break;
1952 }
1953 }
1954
1955 if (!matchedPort) {
1956 // find unused port
1957 for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
1958 candidatePort = &mspPorts[portIndex];
1959 if (candidatePort->mspPortUsage == UNUSED_PORT) {
1960 matchedPort = candidatePort;
1961 break;
1962 }
1963 }
1964 }
1965 mspTelemetryPort = matchedPort;
1966 if (!mspTelemetryPort) {
1967 return;
1968 }
1969
1970 resetMspPort(mspTelemetryPort, serialPort, FOR_TELEMETRY);
1971 }
1972
1973 void sendMspTelemetry(void)
1974 {
1975 static uint32_t sequenceIndex = 0;
1976
1977 if (!mspTelemetryPort) {
1978 return;
1979 }
1980
1981 setCurrentPort(mspTelemetryPort);
1982
1983 processOutCommand(mspTelemetryCommandSequence[sequenceIndex]);
1984 tailSerialReply();
1985
1986 sequenceIndex++;
1987 if (sequenceIndex >= TELEMETRY_MSP_COMMAND_SEQUENCE_ENTRY_COUNT) {
1988 sequenceIndex = 0;
1989 }
1990 }