2 * This file is part of Cleanflight.
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.
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.
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/>.
24 #include "build_config.h"
28 #include "common/axis.h"
29 #include "common/color.h"
30 #include "common/maths.h"
32 #include "drivers/system.h"
34 #include "drivers/sensor.h"
35 #include "drivers/accgyro.h"
36 #include "drivers/compass.h"
38 #include "drivers/serial.h"
39 #include "drivers/bus_i2c.h"
40 #include "drivers/gpio.h"
41 #include "drivers/timer.h"
42 #include "drivers/pwm_rx.h"
44 #include "flight/flight.h"
45 #include "flight/mixer.h"
46 #include "flight/failsafe.h"
47 #include "flight/navigation.h"
50 #include "io/escservo.h"
51 #include "io/rc_controls.h"
53 #include "io/gimbal.h"
54 #include "io/serial.h"
55 #include "io/ledstrip.h"
56 #include "telemetry/telemetry.h"
57 #include "sensors/boardalignment.h"
58 #include "sensors/sensors.h"
59 #include "sensors/battery.h"
60 #include "sensors/acceleration.h"
61 #include "sensors/barometer.h"
62 #include "sensors/compass.h"
63 #include "sensors/gyro.h"
65 #include "config/runtime_config.h"
66 #include "config/config.h"
67 #include "config/config_profile.h"
68 #include "config/config_master.h"
72 #include "hardware_revision.h"
75 #include "serial_msp.h"
77 static serialPort_t
*mspSerialPort
;
79 extern uint16_t cycleTime
; // FIXME dependency on mw.c
80 extern uint16_t rssi
; // FIXME dependency on mw.c
81 extern int16_t debug
[4]; // FIXME dependency on mw.c
83 void useRcControlsConfig(modeActivationCondition_t
*modeActivationConditions
, escAndServoConfig_t
*escAndServoConfigToUse
, pidProfile_t
*pidProfileToUse
);
86 * MSP Guidelines, emphasis is used to clarify.
88 * Each FlightController (FC, Server) MUST change the API version when any MSP command is added, deleted, or changed.
90 * If you fork the FC source code and release your own version, you MUST change the Flight Controller Identifier.
92 * NEVER release a modified copy of this code that shares the same Flight controller IDENT and API version
93 * if the API doesn't match EXACTLY.
95 * Consumers of the API (API clients) SHOULD first attempt to get a response from the MSP_API_VERSION command.
96 * If no response is obtained then client MAY try the legacy MSP_IDENT command.
98 * API consumers should ALWAYS handle communication failures gracefully and attempt to continue
99 * without the information if possible. Clients MAY log/display a suitable message.
101 * API clients should NOT attempt any communication if they can't handle the returned API MAJOR VERSION.
103 * API clients SHOULD attempt communication if the API MINOR VERSION has increased from the time
104 * the API client was written and handle command failures gracefully. Clients MAY disable
105 * functionality that depends on the commands while still leaving other functionality intact.
106 * Clients SHOULD operate in READ-ONLY mode and SHOULD present a warning to the user to state
107 * that the newer API version may cause problems before using API commands that change FC state.
109 * It is for this reason that each MSP command should be specific as possible, such that changes
110 * to commands break as little functionality as possible.
112 * API client authors MAY use a compatibility matrix/table when determining if they can support
113 * a given command from a given flight controller at a given api version level.
115 * Developers MUST NOT create new MSP commands that do more than one thing.
117 * Failure to follow these guidelines will likely invoke the wrath of developers trying to write tools
118 * that use the API and the users of those tools.
121 #define MSP_PROTOCOL_VERSION 0
123 #define API_VERSION_MAJOR 1 // increment when major changes are made
124 #define API_VERSION_MINOR 3 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
126 #define API_VERSION_LENGTH 2
128 #define MULTIWII_IDENTIFIER "MWII";
129 #define CLEANFLIGHT_IDENTIFIER "CLFL"
130 #define BASEFLIGHT_IDENTIFIER "BAFL";
132 #define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4
133 static const char *flightControllerIdentifier
= CLEANFLIGHT_IDENTIFIER
; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
135 #define FLIGHT_CONTROLLER_VERSION_LENGTH 3
136 #define FLIGHT_CONTROLLER_VERSION_MASK 0xFFF
138 const char *boardIdentifier
= TARGET_BOARD_IDENTIFIER
;
139 #define BOARD_IDENTIFIER_LENGTH 4 // 4 UPPER CASE alpha numeric characters that identify the board being used.
140 #define BOARD_HARDWARE_REVISION_LENGTH 2
142 // These are baseflight specific flags but they are useless now since MW 2.3 uses the upper 4 bits for the navigation version.
143 #define CAP_PLATFORM_32BIT ((uint32_t)1 << 31)
144 #define CAP_BASEFLIGHT_CONFIG ((uint32_t)1 << 30)
146 // MW 2.3 stores NAVI_VERSION in the top 4 bits of the capability mask.
147 #define CAP_NAVI_VERSION_BIT_4_MSB ((uint32_t)1 << 31)
148 #define CAP_NAVI_VERSION_BIT_3 ((uint32_t)1 << 30)
149 #define CAP_NAVI_VERSION_BIT_2 ((uint32_t)1 << 29)
150 #define CAP_NAVI_VERSION_BIT_1_LSB ((uint32_t)1 << 28)
152 #define CAP_DYNBALANCE ((uint32_t)1 << 2)
153 #define CAP_FLAPS ((uint32_t)1 << 3)
154 #define CAP_NAVCAP ((uint32_t)1 << 4)
155 #define CAP_EXTAUX ((uint32_t)1 << 5)
157 #define MSP_API_VERSION 1 //out message
158 #define MSP_FC_VARIANT 2 //out message
159 #define MSP_FC_VERSION 3 //out message
160 #define MSP_BOARD_INFO 4 //out message
161 #define MSP_BUILD_INFO 5 //out message
164 // MSP commands for Cleanflight original features
166 #define MSP_CHANNEL_FORWARDING 32 //out message Returns channel forwarding settings
167 #define MSP_SET_CHANNEL_FORWARDING 33 //in message Channel forwarding settings
169 #define MSP_MODE_RANGES 34 //out message Returns all mode ranges
170 #define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range
172 #define MSP_FEATURE 36
173 #define MSP_SET_FEATURE 37
175 #define MSP_BOARD_ALIGNMENT 38
176 #define MSP_SET_BOARD_ALIGNMENT 39
178 #define MSP_CURRENT_METER_CONFIG 40
179 #define MSP_SET_CURRENT_METER_CONFIG 41
182 #define MSP_SET_MIXER 43
184 #define MSP_RX_CONFIG 44
185 #define MSP_SET_RX_CONFIG 45
187 #define MSP_LED_COLORS 46
188 #define MSP_SET_LED_COLORS 47
190 #define MSP_LED_STRIP_CONFIG 48
191 #define MSP_SET_LED_STRIP_CONFIG 49
193 #define MSP_RSSI_CONFIG 50
194 #define MSP_SET_RSSI_CONFIG 51
196 #define MSP_ADJUSTMENT_RANGES 52
197 #define MSP_SET_ADJUSTMENT_RANGE 53
199 // private - only to be used by the configurator, the commands are likely to change
200 #define MSP_CF_SERIAL_CONFIG 54
201 #define MSP_SET_CF_SERIAL_CONFIG 55
204 // Baseflight MSP commands (if enabled they exist in Cleanflight)
206 #define MSP_RX_MAP 64 //out message get channel map (also returns number of channels total)
207 #define MSP_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from MSP_RX_MAP
209 // FIXME - Provided for backwards compatibility with configurator code until configurator is updated.
210 // DEPRECATED - DO NOT USE "MSP_BF_CONFIG" and MSP_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead.
211 #define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere
212 #define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save
214 #define MSP_REBOOT 68 //in message reboot settings
216 // DEPRECATED - Use MSP_BUILD_INFO instead
217 #define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion
220 // Multwii original MSP commands
223 // DEPRECATED - See MSP_API_VERSION and MSP_MIXER
224 #define MSP_IDENT 100 //out message mixerMode + multiwii version + protocol version + capability variable
227 #define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number
228 #define MSP_RAW_IMU 102 //out message 9 DOF
229 #define MSP_SERVO 103 //out message 8 servos
230 #define MSP_MOTOR 104 //out message 8 motors
231 #define MSP_RC 105 //out message 8 rc chan and more
232 #define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course
233 #define MSP_COMP_GPS 107 //out message distance home, direction home
234 #define MSP_ATTITUDE 108 //out message 2 angles 1 heading
235 #define MSP_ALTITUDE 109 //out message altitude, variometer
236 #define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX
237 #define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
238 #define MSP_PID 112 //out message P I D coeff (9 are used currently)
239 #define MSP_BOX 113 //out message BOX setup (number is dependant of your setup)
240 #define MSP_MISC 114 //out message powermeter trig
241 #define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI
242 #define MSP_BOXNAMES 116 //out message the aux switch names
243 #define MSP_PIDNAMES 117 //out message the PID names
244 #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
245 #define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes
246 #define MSP_SERVO_CONF 120 //out message Servo settings
247 #define MSP_NAV_STATUS 121 //out message Returns navigation status
248 #define MSP_NAV_CONFIG 122 //out message Returns navigation parameters
250 #define MSP_SET_RAW_RC 200 //in message 8 rc chan
251 #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
252 #define MSP_SET_PID 202 //in message P I D coeff (9 are used currently)
253 #define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup)
254 #define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
255 #define MSP_ACC_CALIBRATION 205 //in message no param
256 #define MSP_MAG_CALIBRATION 206 //in message no param
257 #define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use
258 #define MSP_RESET_CONF 208 //in message no param
259 #define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags)
260 #define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2)
261 #define MSP_SET_HEAD 211 //in message define a new heading hold direction
262 #define MSP_SET_SERVO_CONF 212 //in message Servo settings
263 #define MSP_SET_MOTOR 214 //in message PropBalance function
264 #define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom
266 // #define MSP_BIND 240 //in message no param
268 #define MSP_EEPROM_WRITE 250 //in message no param
270 #define MSP_DEBUGMSG 253 //out message debug string buffer
271 #define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
273 // Additional commands that are not compatible with MultiWii
274 #define MSP_UID 160 //out message Unique device ID
275 #define MSP_ACC_TRIM 240 //out message get acc angle trim values
276 #define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
277 #define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox)
279 #define INBUF_SIZE 64
281 typedef struct box_e
{
282 const uint8_t boxId
; // see boxId_e
283 const char *boxName
; // GUI-readable box name
284 const uint8_t permanentId
; //
288 static const box_t
const boxes
[CHECKBOX_ITEM_COUNT
+ 1] = {
289 { BOXARM
, "ARM;", 0 },
290 { BOXANGLE
, "ANGLE;", 1 },
291 { BOXHORIZON
, "HORIZON;", 2 },
292 { BOXBARO
, "BARO;", 3 },
293 //{ BOXVARIO, "VARIO;", 4 },
294 { BOXMAG
, "MAG;", 5 },
295 { BOXHEADFREE
, "HEADFREE;", 6 },
296 { BOXHEADADJ
, "HEADADJ;", 7 },
297 { BOXCAMSTAB
, "CAMSTAB;", 8 },
298 { BOXCAMTRIG
, "CAMTRIG;", 9 },
299 { BOXGPSHOME
, "GPS HOME;", 10 },
300 { BOXGPSHOLD
, "GPS HOLD;", 11 },
301 { BOXPASSTHRU
, "PASSTHRU;", 12 },
302 { BOXBEEPERON
, "BEEPER;", 13 },
303 { BOXLEDMAX
, "LEDMAX;", 14 },
304 { BOXLEDLOW
, "LEDLOW;", 15 },
305 { BOXLLIGHTS
, "LLIGHTS;", 16 },
306 { BOXCALIB
, "CALIB;", 17 },
307 { BOXGOV
, "GOVERNOR;", 18 },
308 { BOXOSD
, "OSD SW;", 19 },
309 { BOXTELEMETRY
, "TELEMETRY;", 20 },
310 { BOXAUTOTUNE
, "AUTOTUNE;", 21 },
311 { BOXSONAR
, "SONAR;", 22 },
312 { CHECKBOX_ITEM_COUNT
, NULL
, 0xFF }
315 // this is calculated at startup based on enabled features.
316 static uint8_t activeBoxIds
[CHECKBOX_ITEM_COUNT
];
317 // this is the number of filled indexes in above array
318 static uint8_t activeBoxIdCount
= 0;
320 extern int16_t motor_disarmed
[MAX_SUPPORTED_MOTORS
];
322 // cause reboot after MSP processing complete
323 static bool isRebootScheduled
= false;
325 static const char pidnames
[] =
352 typedef struct mspPort_s
{
358 uint8_t inBuf
[INBUF_SIZE
];
361 mspPortUsage_e mspPortUsage
;
364 static mspPort_t mspPorts
[MAX_MSP_PORT_COUNT
];
366 static mspPort_t
*currentPort
;
368 void serialize32(uint32_t a
)
372 serialWrite(mspSerialPort
, t
);
373 currentPort
->checksum
^= t
;
375 serialWrite(mspSerialPort
, t
);
376 currentPort
->checksum
^= t
;
378 serialWrite(mspSerialPort
, t
);
379 currentPort
->checksum
^= t
;
381 serialWrite(mspSerialPort
, t
);
382 currentPort
->checksum
^= t
;
385 void serialize16(int16_t a
)
389 serialWrite(mspSerialPort
, t
);
390 currentPort
->checksum
^= t
;
392 serialWrite(mspSerialPort
, t
);
393 currentPort
->checksum
^= t
;
396 void serialize8(uint8_t a
)
398 serialWrite(mspSerialPort
, a
);
399 currentPort
->checksum
^= a
;
404 return currentPort
->inBuf
[currentPort
->indRX
++] & 0xff;
407 uint16_t read16(void)
409 uint16_t t
= read8();
410 t
+= (uint16_t)read8() << 8;
414 uint32_t read32(void)
416 uint32_t t
= read16();
417 t
+= (uint32_t)read16() << 16;
421 void headSerialResponse(uint8_t err
, uint8_t s
)
425 serialize8(err
? '!' : '>');
426 currentPort
->checksum
= 0; // start calculating a new checksum
428 serialize8(currentPort
->cmdMSP
);
431 void headSerialReply(uint8_t s
)
433 headSerialResponse(0, s
);
436 void headSerialError(uint8_t s
)
438 headSerialResponse(1, s
);
441 void tailSerialReply(void)
443 serialize8(currentPort
->checksum
);
446 void s_struct(uint8_t *cb
, uint8_t siz
)
448 headSerialReply(siz
);
453 void serializeNames(const char *s
)
460 const box_t
*findBoxByActiveBoxId(uint8_t activeBoxId
)
463 const box_t
*candidate
;
464 for (boxIndex
= 0; boxIndex
< sizeof(boxes
) / sizeof(box_t
); boxIndex
++) {
465 candidate
= &boxes
[boxIndex
];
466 if (candidate
->boxId
== activeBoxId
) {
473 const box_t
*findBoxByPermenantId(uint8_t permenantId
)
476 const box_t
*candidate
;
477 for (boxIndex
= 0; boxIndex
< sizeof(boxes
) / sizeof(box_t
); boxIndex
++) {
478 candidate
= &boxes
[boxIndex
];
479 if (candidate
->permanentId
== permenantId
) {
486 void serializeBoxNamesReply(void)
488 int i
, activeBoxId
, j
, flag
= 1, count
= 0, len
;
492 // in first run of the loop, we grab total size of junk to be sent
493 // then come back and actually send it
494 for (i
= 0; i
< activeBoxIdCount
; i
++) {
495 activeBoxId
= activeBoxIds
[i
];
497 box
= findBoxByActiveBoxId(activeBoxId
);
502 len
= strlen(box
->boxName
);
506 for (j
= 0; j
< len
; j
++)
507 serialize8(box
->boxName
[j
]);
512 headSerialReply(count
);
518 static void resetMspPort(mspPort_t
*mspPortToReset
, serialPort_t
*serialPort
, mspPortUsage_e usage
)
520 memset(mspPortToReset
, 0, sizeof(mspPort_t
));
522 mspPortToReset
->port
= serialPort
;
523 mspPortToReset
->mspPortUsage
= usage
;
526 // This rate is chosen since softserial supports it.
527 #define MSP_FALLBACK_BAUDRATE 19200
529 void mspAllocateSerialPorts(serialConfig_t
*serialConfig
)
535 for (portIndex
= 0; portIndex
< MAX_MSP_PORT_COUNT
; portIndex
++) {
536 mspPort_t
*mspPort
= &mspPorts
[portIndex
];
537 if (mspPort
->mspPortUsage
!= UNUSED_PORT
) {
541 uint32_t baudRate
= serialConfig
->msp_baudrate
;
543 bool triedFallbackRate
= false;
546 port
= openSerialPort(FUNCTION_MSP
, NULL
, baudRate
, MODE_RXTX
, SERIAL_NOT_INVERTED
);
548 if (triedFallbackRate
) {
552 baudRate
= MSP_FALLBACK_BAUDRATE
;
553 triedFallbackRate
= true;
557 if (port
&& portIndex
< MAX_MSP_PORT_COUNT
) {
558 resetMspPort(mspPort
, port
, FOR_GENERAL_MSP
);
565 // XXX this function might help with adding support for MSP on more than one port, if not delete it.
566 const serialPortFunctionList_t
*serialPortFunctionList
= getSerialPortFunctionList();
567 UNUSED(serialPortFunctionList
);
570 void mspReleasePortIfAllocated(serialPort_t
*serialPort
)
573 for (portIndex
= 0; portIndex
< MAX_MSP_PORT_COUNT
; portIndex
++) {
574 mspPort_t
*candidateMspPort
= &mspPorts
[portIndex
];
575 if (candidateMspPort
->port
== serialPort
) {
576 endSerialPortFunction(serialPort
, FUNCTION_MSP
);
577 memset(candidateMspPort
, 0, sizeof(mspPort_t
));
582 void mspInit(serialConfig_t
*serialConfig
)
584 // calculate used boxes based on features and fill availableBoxes[] array
585 memset(activeBoxIds
, 0xFF, sizeof(activeBoxIds
));
587 activeBoxIdCount
= 0;
588 activeBoxIds
[activeBoxIdCount
++] = BOXARM
;
590 if (sensors(SENSOR_ACC
)) {
591 activeBoxIds
[activeBoxIdCount
++] = BOXANGLE
;
592 activeBoxIds
[activeBoxIdCount
++] = BOXHORIZON
;
595 if (sensors(SENSOR_BARO
)) {
596 activeBoxIds
[activeBoxIdCount
++] = BOXBARO
;
599 if (sensors(SENSOR_ACC
) || sensors(SENSOR_MAG
)) {
600 activeBoxIds
[activeBoxIdCount
++] = BOXMAG
;
601 activeBoxIds
[activeBoxIdCount
++] = BOXHEADFREE
;
602 activeBoxIds
[activeBoxIdCount
++] = BOXHEADADJ
;
605 if (feature(FEATURE_SERVO_TILT
))
606 activeBoxIds
[activeBoxIdCount
++] = BOXCAMSTAB
;
609 if (feature(FEATURE_GPS
)) {
610 activeBoxIds
[activeBoxIdCount
++] = BOXGPSHOME
;
611 activeBoxIds
[activeBoxIdCount
++] = BOXGPSHOLD
;
615 if (masterConfig
.mixerMode
== MIXER_FLYING_WING
|| masterConfig
.mixerMode
== MIXER_AIRPLANE
)
616 activeBoxIds
[activeBoxIdCount
++] = BOXPASSTHRU
;
618 activeBoxIds
[activeBoxIdCount
++] = BOXBEEPERON
;
621 if (feature(FEATURE_LED_STRIP
)) {
622 activeBoxIds
[activeBoxIdCount
++] = BOXLEDLOW
;
626 if (feature(FEATURE_INFLIGHT_ACC_CAL
))
627 activeBoxIds
[activeBoxIdCount
++] = BOXCALIB
;
629 activeBoxIds
[activeBoxIdCount
++] = BOXOSD
;
631 if (feature(FEATURE_TELEMETRY
&& masterConfig
.telemetryConfig
.telemetry_switch
))
632 activeBoxIds
[activeBoxIdCount
++] = BOXTELEMETRY
;
635 activeBoxIds
[activeBoxIdCount
++] = BOXAUTOTUNE
;
638 if (feature(FEATURE_SONAR
)){
639 activeBoxIds
[activeBoxIdCount
++] = BOXSONAR
;
642 memset(mspPorts
, 0x00, sizeof(mspPorts
));
643 mspAllocateSerialPorts(serialConfig
);
646 #define IS_ENABLED(mask) (mask == 0 ? 0 : 1)
648 static bool processOutCommand(uint8_t cmdMSP
)
650 uint32_t i
, tmp
, junk
;
655 int32_t lat
= 0, lon
= 0;
659 case MSP_API_VERSION
:
661 1 + // protocol version length
664 serialize8(MSP_PROTOCOL_VERSION
);
666 serialize8(API_VERSION_MAJOR
);
667 serialize8(API_VERSION_MINOR
);
671 headSerialReply(FLIGHT_CONTROLLER_IDENTIFIER_LENGTH
);
673 for (i
= 0; i
< FLIGHT_CONTROLLER_IDENTIFIER_LENGTH
; i
++) {
674 serialize8(flightControllerIdentifier
[i
]);
679 headSerialReply(FLIGHT_CONTROLLER_VERSION_LENGTH
);
681 serialize8(FC_VERSION_MAJOR
);
682 serialize8(FC_VERSION_MINOR
);
683 serialize8(FC_VERSION_PATCH_LEVEL
);
688 BOARD_IDENTIFIER_LENGTH
+
689 BOARD_HARDWARE_REVISION_LENGTH
691 for (i
= 0; i
< BOARD_IDENTIFIER_LENGTH
; i
++) {
692 serialize8(boardIdentifier
[i
]);
695 serialize16(hardwareRevision
);
697 serialize16(0); // No other build targets currently have hardware revision detection.
705 GIT_SHORT_REVISION_LENGTH
708 for (i
= 0; i
< BUILD_DATE_LENGTH
; i
++) {
709 serialize8(buildDate
[i
]);
711 for (i
= 0; i
< BUILD_TIME_LENGTH
; i
++) {
712 serialize8(buildTime
[i
]);
715 for (i
= 0; i
< GIT_SHORT_REVISION_LENGTH
; i
++) {
716 serialize8(shortGitRevision
[i
]);
720 // DEPRECATED - Use MSP_API_VERSION
723 serialize8(MW_VERSION
);
724 serialize8(masterConfig
.mixerMode
);
725 serialize8(MSP_PROTOCOL_VERSION
);
726 serialize32(CAP_DYNBALANCE
| (masterConfig
.airplaneConfig
.flaps_speed
? CAP_FLAPS
: 0)); // "capability"
731 serialize16(cycleTime
);
733 serialize16(i2cGetErrorCounter());
737 serialize16(sensors(SENSOR_ACC
) | sensors(SENSOR_BARO
) << 1 | sensors(SENSOR_MAG
) << 2 | sensors(SENSOR_GPS
) << 3 | sensors(SENSOR_SONAR
) << 4);
738 // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
739 // Requires new Multiwii protocol version to fix
740 // It would be preferable to setting the enabled bits based on BOXINDEX.
742 tmp
= IS_ENABLED(FLIGHT_MODE(ANGLE_MODE
)) << BOXANGLE
|
743 IS_ENABLED(FLIGHT_MODE(HORIZON_MODE
)) << BOXHORIZON
|
744 IS_ENABLED(FLIGHT_MODE(BARO_MODE
)) << BOXBARO
|
745 IS_ENABLED(FLIGHT_MODE(MAG_MODE
)) << BOXMAG
|
746 IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE
)) << BOXHEADFREE
|
747 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ
)) << BOXHEADADJ
|
748 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB
)) << BOXCAMSTAB
|
749 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMTRIG
)) << BOXCAMTRIG
|
750 IS_ENABLED(FLIGHT_MODE(GPS_HOME_MODE
)) << BOXGPSHOME
|
751 IS_ENABLED(FLIGHT_MODE(GPS_HOLD_MODE
)) << BOXGPSHOLD
|
752 IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE
)) << BOXPASSTHRU
|
753 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON
)) << BOXBEEPERON
|
754 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX
)) << BOXLEDMAX
|
755 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW
)) << BOXLEDLOW
|
756 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS
)) << BOXLLIGHTS
|
757 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB
)) << BOXCALIB
|
758 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV
)) << BOXGOV
|
759 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD
)) << BOXOSD
|
760 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY
)) << BOXTELEMETRY
|
761 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTUNE
)) << BOXAUTOTUNE
|
762 IS_ENABLED(FLIGHT_MODE(SONAR_MODE
)) << BOXSONAR
|
763 IS_ENABLED(ARMING_FLAG(ARMED
)) << BOXARM
;
764 for (i
= 0; i
< activeBoxIdCount
; i
++) {
765 int flag
= (tmp
& (1 << activeBoxIds
[i
]));
770 serialize8(masterConfig
.current_profile_index
);
774 // Hack due to choice of units for sensor data in multiwii
776 for (i
= 0; i
< 3; i
++)
777 serialize16(accSmooth
[i
] / 8);
779 for (i
= 0; i
< 3; i
++)
780 serialize16(accSmooth
[i
]);
782 for (i
= 0; i
< 3; i
++)
783 serialize16(gyroData
[i
]);
784 for (i
= 0; i
< 3; i
++)
785 serialize16(magADC
[i
]);
788 s_struct((uint8_t *)&servo
, 16);
792 for (i
= 0; i
< MAX_SUPPORTED_SERVOS
; i
++) {
793 serialize16(currentProfile
->servoConf
[i
].min
);
794 serialize16(currentProfile
->servoConf
[i
].max
);
795 serialize16(currentProfile
->servoConf
[i
].middle
);
796 serialize8(currentProfile
->servoConf
[i
].rate
);
799 case MSP_CHANNEL_FORWARDING
:
801 for (i
= 0; i
< MAX_SUPPORTED_SERVOS
; i
++) {
802 serialize8(currentProfile
->servoConf
[i
].forwardFromChannel
);
806 s_struct((uint8_t *)motor
, 16);
809 headSerialReply(2 * rxRuntimeConfig
.channelCount
);
810 for (i
= 0; i
< rxRuntimeConfig
.channelCount
; i
++)
811 serialize16(rcData
[i
]);
815 for (i
= 0; i
< 2; i
++)
816 serialize16(inclination
.raw
[i
]);
817 serialize16(heading
);
826 serialize8((uint8_t)constrain(vbat
, 0, 255));
827 serialize16((uint16_t)constrain(mAhDrawn
, 0, 0xFFFF)); // milliamphours drawn from battery
829 if(masterConfig
.batteryConfig
.multiwiiCurrentMeterOutput
) {
830 serialize16((uint16_t)constrain((abs(amperage
) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps
832 serialize16((uint16_t)constrain(abs(amperage
), 0, 0xFFFF)); // send amperage in 0.01 A steps
836 serialize8(currentControlRateProfile
->rcRate8
);
837 serialize8(currentControlRateProfile
->rcExpo8
);
838 serialize8(currentControlRateProfile
->rollPitchRate
);
839 serialize8(currentControlRateProfile
->yawRate
);
840 serialize8(currentControlRateProfile
->dynThrPID
);
841 serialize8(currentControlRateProfile
->thrMid8
);
842 serialize8(currentControlRateProfile
->thrExpo8
);
845 headSerialReply(3 * PID_ITEM_COUNT
);
846 if (currentProfile
->pidController
== 2) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid
847 for (i
= 0; i
< 3; i
++) {
848 serialize8(constrain(lrintf(currentProfile
->pidProfile
.P_f
[i
] * 10.0f
), 0, 250));
849 serialize8(constrain(lrintf(currentProfile
->pidProfile
.I_f
[i
] * 100.0f
), 0, 250));
850 serialize8(constrain(lrintf(currentProfile
->pidProfile
.D_f
[i
] * 1000.0f
), 0, 100));
852 for (i
= 3; i
< PID_ITEM_COUNT
; i
++) {
854 serialize8(constrain(lrintf(currentProfile
->pidProfile
.A_level
* 10.0f
), 0, 250));
855 serialize8(constrain(lrintf(currentProfile
->pidProfile
.H_level
* 10.0f
), 0, 250));
856 serialize8(constrain(lrintf(currentProfile
->pidProfile
.H_sensitivity
), 0, 250));
858 serialize8(currentProfile
->pidProfile
.P8
[i
]);
859 serialize8(currentProfile
->pidProfile
.I8
[i
]);
860 serialize8(currentProfile
->pidProfile
.D8
[i
]);
864 for (i
= 0; i
< PID_ITEM_COUNT
; i
++) {
865 serialize8(currentProfile
->pidProfile
.P8
[i
]);
866 serialize8(currentProfile
->pidProfile
.I8
[i
]);
867 serialize8(currentProfile
->pidProfile
.D8
[i
]);
872 headSerialReply(sizeof(pidnames
) - 1);
873 serializeNames(pidnames
);
875 case MSP_MODE_RANGES
:
876 headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT
);
877 for (i
= 0; i
< MAX_MODE_ACTIVATION_CONDITION_COUNT
; i
++) {
878 modeActivationCondition_t
*mac
= ¤tProfile
->modeActivationConditions
[i
];
879 const box_t
*box
= &boxes
[mac
->modeId
];
880 serialize8(box
->permanentId
);
881 serialize8(mac
->auxChannelIndex
);
882 serialize8(mac
->range
.startStep
);
883 serialize8(mac
->range
.endStep
);
886 case MSP_ADJUSTMENT_RANGES
:
887 headSerialReply(MAX_ADJUSTMENT_RANGE_COUNT
* (
888 1 + // adjustment index/slot
889 1 + // aux channel index
892 1 + // adjustment function
893 1 // aux switch channel index
895 for (i
= 0; i
< MAX_ADJUSTMENT_RANGE_COUNT
; i
++) {
896 adjustmentRange_t
*adjRange
= ¤tProfile
->adjustmentRanges
[i
];
897 serialize8(adjRange
->adjustmentIndex
);
898 serialize8(adjRange
->auxChannelIndex
);
899 serialize8(adjRange
->range
.startStep
);
900 serialize8(adjRange
->range
.endStep
);
901 serialize8(adjRange
->adjustmentFunction
);
902 serialize8(adjRange
->auxSwitchChannelIndex
);
906 serializeBoxNamesReply();
909 headSerialReply(activeBoxIdCount
);
910 for (i
= 0; i
< activeBoxIdCount
; i
++) {
911 const box_t
*box
= findBoxByActiveBoxId(activeBoxIds
[i
]);
915 serialize8(box
->permanentId
);
919 headSerialReply(2 * 6 + 4 + 2 + 4);
920 serialize16(masterConfig
.rxConfig
.midrc
);
922 serialize16(masterConfig
.escAndServoConfig
.minthrottle
);
923 serialize16(masterConfig
.escAndServoConfig
.maxthrottle
);
924 serialize16(masterConfig
.escAndServoConfig
.mincommand
);
926 serialize16(currentProfile
->failsafeConfig
.failsafe_throttle
);
929 serialize8(masterConfig
.gpsConfig
.provider
); // gps_type
930 serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
931 serialize8(masterConfig
.gpsConfig
.sbasMode
); // gps_ubx_sbas
933 serialize8(0); // gps_type
934 serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
935 serialize8(0); // gps_ubx_sbas
937 serialize8(masterConfig
.batteryConfig
.multiwiiCurrentMeterOutput
);
938 serialize8(masterConfig
.rxConfig
.rssi_channel
);
941 serialize16(currentProfile
->mag_declination
/ 10);
943 serialize8(masterConfig
.batteryConfig
.vbatscale
);
944 serialize8(masterConfig
.batteryConfig
.vbatmincellvoltage
);
945 serialize8(masterConfig
.batteryConfig
.vbatmaxcellvoltage
);
946 serialize8(masterConfig
.batteryConfig
.vbatwarningcellvoltage
);
950 for (i
= 0; i
< 8; i
++)
956 serialize8(STATE(GPS_FIX
));
957 serialize8(GPS_numSat
);
958 serialize32(GPS_coord
[LAT
]);
959 serialize32(GPS_coord
[LON
]);
960 serialize16(GPS_altitude
);
961 serialize16(GPS_speed
);
962 serialize16(GPS_ground_course
);
966 serialize16(GPS_distanceToHome
);
967 serialize16(GPS_directionToHome
);
968 serialize8(GPS_update
& 1);
971 wp_no
= read8(); // get the wp number
976 } else if (wp_no
== 16) {
983 serialize32(AltHold
); // altitude (cm) will come here -- temporary implementation to test feature with apps
984 serialize16(0); // heading will come here (deg)
985 serialize16(0); // time to stay (ms) will come here
986 serialize8(0); // nav flag will come here
989 headSerialReply(1 + (GPS_numCh
* 4));
990 serialize8(GPS_numCh
);
991 for (i
= 0; i
< GPS_numCh
; i
++){
992 serialize8(GPS_svinfo_chn
[i
]);
993 serialize8(GPS_svinfo_svid
[i
]);
994 serialize8(GPS_svinfo_quality
[i
]);
995 serialize8(GPS_svinfo_cno
[i
]);
1001 // make use of this crap, output some useful QA statistics
1002 //debug[3] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock]
1003 for (i
= 0; i
< 4; i
++)
1004 serialize16(debug
[i
]); // 4 variables are here for general monitoring purpose
1007 // Additional commands that are not compatible with MultiWii
1010 serialize16(currentProfile
->accelerometerTrims
.values
.pitch
);
1011 serialize16(currentProfile
->accelerometerTrims
.values
.roll
);
1015 headSerialReply(12);
1016 serialize32(U_ID_0
);
1017 serialize32(U_ID_1
);
1018 serialize32(U_ID_2
);
1023 serialize32(featureMask());
1026 case MSP_BOARD_ALIGNMENT
:
1028 serialize16(masterConfig
.boardAlignment
.rollDegrees
);
1029 serialize16(masterConfig
.boardAlignment
.pitchDegrees
);
1030 serialize16(masterConfig
.boardAlignment
.yawDegrees
);
1033 case MSP_CURRENT_METER_CONFIG
:
1035 serialize16(masterConfig
.batteryConfig
.currentMeterScale
);
1036 serialize16(masterConfig
.batteryConfig
.currentMeterOffset
);
1041 serialize8(masterConfig
.mixerMode
);
1046 serialize8(masterConfig
.rxConfig
.serialrx_provider
);
1047 serialize16(masterConfig
.rxConfig
.maxcheck
);
1048 serialize16(masterConfig
.rxConfig
.midrc
);
1049 serialize16(masterConfig
.rxConfig
.mincheck
);
1050 serialize8(masterConfig
.rxConfig
.spektrum_sat_bind
);
1053 case MSP_RSSI_CONFIG
:
1055 serialize8(masterConfig
.rxConfig
.rssi_channel
);
1059 headSerialReply(MAX_MAPPABLE_RX_INPUTS
);
1060 for (i
= 0; i
< MAX_MAPPABLE_RX_INPUTS
; i
++)
1061 serialize8(masterConfig
.rxConfig
.rcmap
[i
]);
1065 headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 2 + 2);
1066 serialize8(masterConfig
.mixerMode
);
1068 serialize32(featureMask());
1070 serialize8(masterConfig
.rxConfig
.serialrx_provider
);
1072 serialize16(masterConfig
.boardAlignment
.rollDegrees
);
1073 serialize16(masterConfig
.boardAlignment
.pitchDegrees
);
1074 serialize16(masterConfig
.boardAlignment
.yawDegrees
);
1076 serialize16(masterConfig
.batteryConfig
.currentMeterScale
);
1077 serialize16(masterConfig
.batteryConfig
.currentMeterOffset
);
1080 case MSP_CF_SERIAL_CONFIG
:
1082 ((sizeof(uint8_t) * 2) * SERIAL_PORT_COUNT
) +
1083 (sizeof(uint32_t) * 4)
1085 for (i
= 0; i
< SERIAL_PORT_COUNT
; i
++) {
1086 serialize8(serialPortConstraints
[i
].identifier
);
1087 serialize8(masterConfig
.serialConfig
.serial_port_scenario
[i
]);
1089 serialize32(masterConfig
.serialConfig
.msp_baudrate
);
1090 serialize32(masterConfig
.serialConfig
.cli_baudrate
);
1091 serialize32(masterConfig
.serialConfig
.gps_baudrate
);
1092 serialize32(masterConfig
.serialConfig
.gps_passthrough_baudrate
);
1096 case MSP_LED_COLORS
:
1097 headSerialReply(CONFIGURABLE_COLOR_COUNT
* 4);
1098 for (i
= 0; i
< CONFIGURABLE_COLOR_COUNT
; i
++) {
1099 hsvColor_t
*color
= &masterConfig
.colors
[i
];
1100 serialize16(color
->h
);
1101 serialize8(color
->s
);
1102 serialize8(color
->v
);
1106 case MSP_LED_STRIP_CONFIG
:
1107 headSerialReply(MAX_LED_STRIP_LENGTH
* 6);
1108 for (i
= 0; i
< MAX_LED_STRIP_LENGTH
; i
++) {
1109 ledConfig_t
*ledConfig
= &masterConfig
.ledConfigs
[i
];
1110 serialize16((ledConfig
->flags
& LED_DIRECTION_MASK
) >> LED_DIRECTION_BIT_OFFSET
);
1111 serialize16((ledConfig
->flags
& LED_FUNCTION_MASK
) >> LED_FUNCTION_BIT_OFFSET
);
1112 serialize8(GET_LED_X(ledConfig
));
1113 serialize8(GET_LED_Y(ledConfig
));
1117 case MSP_BF_BUILD_INFO
:
1118 headSerialReply(11 + 4 + 4);
1119 for (i
= 0; i
< 11; i
++)
1120 serialize8(buildDate
[i
]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
1121 serialize32(0); // future exp
1122 serialize32(0); // future exp
1131 static bool processInCommand(void)
1137 int32_t lat
= 0, lon
= 0, alt
= 0;
1140 switch (currentPort
->cmdMSP
) {
1141 case MSP_SELECT_SETTING
:
1142 if (!ARMING_FLAG(ARMED
)) {
1143 masterConfig
.current_profile_index
= read8();
1144 if (masterConfig
.current_profile_index
> 2) {
1145 masterConfig
.current_profile_index
= 0;
1154 case MSP_SET_RAW_RC
:
1156 uint8_t channelCount
= currentPort
->dataSize
/ sizeof(uint16_t);
1157 if (channelCount
> MAX_SUPPORTED_RC_CHANNEL_COUNT
) {
1160 for (i
= 0; i
< channelCount
; i
++)
1161 rcData
[i
] = read16();
1162 rxMspFrameRecieve();
1166 case MSP_SET_ACC_TRIM
:
1167 currentProfile
->accelerometerTrims
.values
.pitch
= read16();
1168 currentProfile
->accelerometerTrims
.values
.roll
= read16();
1171 if (currentProfile
->pidController
== 2) {
1172 for (i
= 0; i
< 3; i
++) {
1173 currentProfile
->pidProfile
.P_f
[i
] = (float)read8() / 10.0f
;
1174 currentProfile
->pidProfile
.I_f
[i
] = (float)read8() / 100.0f
;
1175 currentProfile
->pidProfile
.D_f
[i
] = (float)read8() / 1000.0f
;
1177 for (i
= 3; i
< PID_ITEM_COUNT
; i
++) {
1178 if (i
== PIDLEVEL
) {
1179 currentProfile
->pidProfile
.A_level
= (float)read8() / 10.0f
;
1180 currentProfile
->pidProfile
.H_level
= (float)read8() / 10.0f
;
1181 currentProfile
->pidProfile
.H_sensitivity
= read8();
1183 currentProfile
->pidProfile
.P8
[i
] = read8();
1184 currentProfile
->pidProfile
.I8
[i
] = read8();
1185 currentProfile
->pidProfile
.D8
[i
] = read8();
1189 for (i
= 0; i
< PID_ITEM_COUNT
; i
++) {
1190 currentProfile
->pidProfile
.P8
[i
] = read8();
1191 currentProfile
->pidProfile
.I8
[i
] = read8();
1192 currentProfile
->pidProfile
.D8
[i
] = read8();
1196 case MSP_SET_MODE_RANGE
:
1198 if (i
< MAX_MODE_ACTIVATION_CONDITION_COUNT
) {
1199 modeActivationCondition_t
*mac
= ¤tProfile
->modeActivationConditions
[i
];
1201 const box_t
*box
= findBoxByPermenantId(i
);
1203 mac
->modeId
= box
->boxId
;
1204 mac
->auxChannelIndex
= read8();
1205 mac
->range
.startStep
= read8();
1206 mac
->range
.endStep
= read8();
1208 useRcControlsConfig(currentProfile
->modeActivationConditions
, &masterConfig
.escAndServoConfig
, ¤tProfile
->pidProfile
);
1216 case MSP_SET_ADJUSTMENT_RANGE
:
1218 if (i
< MAX_ADJUSTMENT_RANGE_COUNT
) {
1219 adjustmentRange_t
*adjRange
= ¤tProfile
->adjustmentRanges
[i
];
1221 if (i
< MAX_SIMULTANEOUS_ADJUSTMENT_COUNT
) {
1222 adjRange
->adjustmentIndex
= i
;
1223 adjRange
->auxChannelIndex
= read8();
1224 adjRange
->range
.startStep
= read8();
1225 adjRange
->range
.endStep
= read8();
1226 adjRange
->adjustmentFunction
= read8();
1227 adjRange
->auxSwitchChannelIndex
= read8();
1236 case MSP_SET_RC_TUNING
:
1237 currentControlRateProfile
->rcRate8
= read8();
1238 currentControlRateProfile
->rcExpo8
= read8();
1239 currentControlRateProfile
->rollPitchRate
= read8();
1240 currentControlRateProfile
->yawRate
= read8();
1241 currentControlRateProfile
->dynThrPID
= read8();
1242 currentControlRateProfile
->thrMid8
= read8();
1243 currentControlRateProfile
->thrExpo8
= read8();
1247 if (tmp
< 1600 && tmp
> 1400)
1248 masterConfig
.rxConfig
.midrc
= tmp
;
1250 masterConfig
.escAndServoConfig
.minthrottle
= read16();
1251 masterConfig
.escAndServoConfig
.maxthrottle
= read16();
1252 masterConfig
.escAndServoConfig
.mincommand
= read16();
1254 currentProfile
->failsafeConfig
.failsafe_throttle
= read16();
1257 masterConfig
.gpsConfig
.provider
= read8(); // gps_type
1258 read8(); // gps_baudrate
1259 masterConfig
.gpsConfig
.sbasMode
= read8(); // gps_ubx_sbas
1261 read8(); // gps_type
1262 read8(); // gps_baudrate
1263 read8(); // gps_ubx_sbas
1265 masterConfig
.batteryConfig
.multiwiiCurrentMeterOutput
= read8();
1266 masterConfig
.rxConfig
.rssi_channel
= read8();
1269 currentProfile
->mag_declination
= read16() * 10;
1271 masterConfig
.batteryConfig
.vbatscale
= read8(); // actual vbatscale as intended
1272 masterConfig
.batteryConfig
.vbatmincellvoltage
= read8(); // vbatlevel_warn1 in MWC2.3 GUI
1273 masterConfig
.batteryConfig
.vbatmaxcellvoltage
= read8(); // vbatlevel_warn2 in MWC2.3 GUI
1274 masterConfig
.batteryConfig
.vbatwarningcellvoltage
= read8(); // vbatlevel when buzzer starts to alert
1277 for (i
= 0; i
< 8; i
++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8
1278 motor_disarmed
[i
] = read16();
1280 case MSP_SET_SERVO_CONF
:
1281 for (i
= 0; i
< MAX_SUPPORTED_SERVOS
; i
++) {
1282 currentProfile
->servoConf
[i
].min
= read16();
1283 currentProfile
->servoConf
[i
].max
= read16();
1284 // provide temporary support for old clients that try and send a channel index instead of a servo middle
1285 uint16_t potentialServoMiddleOrChannelToForward
= read16();
1286 if (potentialServoMiddleOrChannelToForward
< MAX_SUPPORTED_SERVOS
) {
1287 currentProfile
->servoConf
[i
].forwardFromChannel
= potentialServoMiddleOrChannelToForward
;
1289 if (potentialServoMiddleOrChannelToForward
>= PWM_RANGE_MIN
&& potentialServoMiddleOrChannelToForward
<= PWM_RANGE_MAX
) {
1290 currentProfile
->servoConf
[i
].middle
= potentialServoMiddleOrChannelToForward
;
1292 currentProfile
->servoConf
[i
].rate
= read8();
1295 case MSP_SET_CHANNEL_FORWARDING
:
1296 for (i
= 0; i
< MAX_SUPPORTED_SERVOS
; i
++) {
1297 currentProfile
->servoConf
[i
].forwardFromChannel
= read8();
1300 case MSP_RESET_CONF
:
1301 if (!ARMING_FLAG(ARMED
)) {
1306 case MSP_ACC_CALIBRATION
:
1307 if (!ARMING_FLAG(ARMED
))
1308 accSetCalibrationCycles(CALIBRATING_ACC_CYCLES
);
1310 case MSP_MAG_CALIBRATION
:
1311 if (!ARMING_FLAG(ARMED
))
1312 ENABLE_STATE(CALIBRATE_MAG
);
1314 case MSP_EEPROM_WRITE
:
1315 if (ARMING_FLAG(ARMED
)) {
1323 case MSP_SET_RAW_GPS
:
1325 ENABLE_STATE(GPS_FIX
);
1327 DISABLE_STATE(GPS_FIX
);
1329 GPS_numSat
= read8();
1330 GPS_coord
[LAT
] = read32();
1331 GPS_coord
[LON
] = read32();
1332 GPS_altitude
= read16();
1333 GPS_speed
= read16();
1334 GPS_update
|= 2; // New data signalisation to GPS functions // FIXME Magic Numbers
1337 wp_no
= read8(); //get the wp number
1340 alt
= read32(); // to set altitude (cm)
1341 read16(); // future: to set heading (deg)
1342 read16(); // future: to set time to stay (ms)
1343 read8(); // future: to set nav flag
1345 GPS_home
[LAT
] = lat
;
1346 GPS_home
[LON
] = lon
;
1347 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
1348 ENABLE_STATE(GPS_FIX_HOME
);
1350 AltHold
= alt
; // temporary implementation to test feature with apps
1351 } 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
1352 GPS_hold
[LAT
] = lat
;
1353 GPS_hold
[LON
] = lon
;
1355 AltHold
= alt
; // temporary implementation to test feature with apps
1356 nav_mode
= NAV_MODE_WP
;
1357 GPS_set_next_wp(&GPS_hold
[LAT
], &GPS_hold
[LON
]);
1361 case MSP_SET_FEATURE
:
1363 featureSet(read32()); // features bitmap
1366 case MSP_SET_BOARD_ALIGNMENT
:
1367 masterConfig
.boardAlignment
.rollDegrees
= read16();
1368 masterConfig
.boardAlignment
.pitchDegrees
= read16();
1369 masterConfig
.boardAlignment
.yawDegrees
= read16();
1372 case MSP_SET_CURRENT_METER_CONFIG
:
1373 masterConfig
.batteryConfig
.currentMeterScale
= read16();
1374 masterConfig
.batteryConfig
.currentMeterOffset
= read16();
1377 #ifndef USE_QUAD_MIXER_ONLY
1379 masterConfig
.mixerMode
= read8();
1383 case MSP_SET_RX_CONFIG
:
1384 masterConfig
.rxConfig
.serialrx_provider
= read8();
1385 masterConfig
.rxConfig
.maxcheck
= read16();
1386 masterConfig
.rxConfig
.midrc
= read16();
1387 masterConfig
.rxConfig
.mincheck
= read16();
1388 masterConfig
.rxConfig
.spektrum_sat_bind
= read8();
1391 case MSP_SET_RSSI_CONFIG
:
1392 masterConfig
.rxConfig
.rssi_channel
= read8();
1395 case MSP_SET_RX_MAP
:
1396 for (i
= 0; i
< MAX_MAPPABLE_RX_INPUTS
; i
++) {
1397 masterConfig
.rxConfig
.rcmap
[i
] = read8();
1401 case MSP_SET_BF_CONFIG
:
1403 #ifdef USE_QUAD_MIXER_ONLY
1404 read8(); // mixerMode ignored
1406 masterConfig
.mixerMode
= read8(); // mixerMode
1410 featureSet(read32()); // features bitmap
1412 masterConfig
.rxConfig
.serialrx_provider
= read8(); // serialrx_type
1414 masterConfig
.boardAlignment
.rollDegrees
= read16(); // board_align_roll
1415 masterConfig
.boardAlignment
.pitchDegrees
= read16(); // board_align_pitch
1416 masterConfig
.boardAlignment
.yawDegrees
= read16(); // board_align_yaw
1418 masterConfig
.batteryConfig
.currentMeterScale
= read16();
1419 masterConfig
.batteryConfig
.currentMeterOffset
= read16();
1422 case MSP_SET_CF_SERIAL_CONFIG
:
1424 uint8_t baudRateSize
= (sizeof(uint32_t) * 4);
1425 uint8_t serialPortCount
= currentPort
->dataSize
- baudRateSize
;
1426 if (serialPortCount
!= SERIAL_PORT_COUNT
) {
1430 for (i
= 0; i
< SERIAL_PORT_COUNT
; i
++) {
1431 masterConfig
.serialConfig
.serial_port_scenario
[i
] = read8();
1433 masterConfig
.serialConfig
.msp_baudrate
= read32();
1434 masterConfig
.serialConfig
.cli_baudrate
= read32();
1435 masterConfig
.serialConfig
.gps_baudrate
= read32();
1436 masterConfig
.serialConfig
.gps_passthrough_baudrate
= read32();
1441 case MSP_SET_LED_COLORS
:
1442 for (i
= 0; i
< CONFIGURABLE_COLOR_COUNT
; i
++) {
1443 hsvColor_t
*color
= &masterConfig
.colors
[i
];
1444 color
->h
= read16();
1450 case MSP_SET_LED_STRIP_CONFIG
:
1453 if (i
>= MAX_LED_STRIP_LENGTH
|| currentPort
->dataSize
!= 7) {
1457 ledConfig_t
*ledConfig
= &masterConfig
.ledConfigs
[i
];
1459 // currently we're storing directions and functions in a uint16 (flags)
1460 // the msp uses 2 x uint16_t to cater for future expansion
1462 ledConfig
->flags
= (mask
<< LED_DIRECTION_BIT_OFFSET
) & LED_DIRECTION_MASK
;
1465 ledConfig
->flags
|= (mask
<< LED_FUNCTION_BIT_OFFSET
) & LED_FUNCTION_MASK
;
1468 ledConfig
->xy
= CALCULATE_LED_X(mask
);
1471 ledConfig
->xy
|= CALCULATE_LED_Y(mask
);
1473 reevalulateLedConfig();
1478 isRebootScheduled
= true;
1482 // we do not know how to handle the (valid) message, indicate error MSP $M!
1489 static void mspProcessPort(void)
1493 while (serialTotalBytesWaiting(mspSerialPort
)) {
1494 c
= serialRead(mspSerialPort
);
1496 if (currentPort
->c_state
== IDLE
) {
1497 currentPort
->c_state
= (c
== '$') ? HEADER_START
: IDLE
;
1498 if (currentPort
->c_state
== IDLE
&& !ARMING_FLAG(ARMED
))
1499 evaluateOtherData(c
); // if not armed evaluate all other incoming serial data
1500 } else if (currentPort
->c_state
== HEADER_START
) {
1501 currentPort
->c_state
= (c
== 'M') ? HEADER_M
: IDLE
;
1502 } else if (currentPort
->c_state
== HEADER_M
) {
1503 currentPort
->c_state
= (c
== '<') ? HEADER_ARROW
: IDLE
;
1504 } else if (currentPort
->c_state
== HEADER_ARROW
) {
1505 if (c
> INBUF_SIZE
) { // now we are expecting the payload size
1506 currentPort
->c_state
= IDLE
;
1509 currentPort
->dataSize
= c
;
1510 currentPort
->offset
= 0;
1511 currentPort
->checksum
= 0;
1512 currentPort
->indRX
= 0;
1513 currentPort
->checksum
^= c
;
1514 currentPort
->c_state
= HEADER_SIZE
; // the command is to follow
1515 } else if (currentPort
->c_state
== HEADER_SIZE
) {
1516 currentPort
->cmdMSP
= c
;
1517 currentPort
->checksum
^= c
;
1518 currentPort
->c_state
= HEADER_CMD
;
1519 } else if (currentPort
->c_state
== HEADER_CMD
&& currentPort
->offset
< currentPort
->dataSize
) {
1520 currentPort
->checksum
^= c
;
1521 currentPort
->inBuf
[currentPort
->offset
++] = c
;
1522 } else if (currentPort
->c_state
== HEADER_CMD
&& currentPort
->offset
>= currentPort
->dataSize
) {
1523 if (currentPort
->checksum
== c
) { // compare calculated and transferred checksum
1524 // we got a valid packet, evaluate it
1525 if (!(processOutCommand(currentPort
->cmdMSP
) || processInCommand())) {
1530 currentPort
->c_state
= IDLE
;
1531 break; // process one command so as not to block.
1536 void setCurrentPort(mspPort_t
*port
)
1539 mspSerialPort
= currentPort
->port
;
1542 void mspProcess(void)
1545 mspPort_t
*candidatePort
;
1547 for (portIndex
= 0; portIndex
< MAX_MSP_PORT_COUNT
; portIndex
++) {
1548 candidatePort
= &mspPorts
[portIndex
];
1549 if (candidatePort
->mspPortUsage
!= FOR_GENERAL_MSP
) {
1553 setCurrentPort(candidatePort
);
1556 if (isRebootScheduled
) {
1557 // pause a little while to allow response to be sent
1558 while (!isSerialTransmitBufferEmpty(candidatePort
->port
)) {
1566 static const uint8_t mspTelemetryCommandSequence
[] = {
1567 MSP_BOXNAMES
, // repeat boxnames, in case the first transmission was lost or never received.
1579 #define MSP_TELEMETRY_COMMAND_SEQUENCE_ENTRY_COUNT (sizeof(mspTelemetryCommandSequence) / sizeof(mspTelemetryCommandSequence[0]))
1581 static mspPort_t
*mspTelemetryPort
= NULL
;
1583 void mspSetTelemetryPort(serialPort_t
*serialPort
)
1586 mspPort_t
*candidatePort
= NULL
;
1587 mspPort_t
*matchedPort
= NULL
;
1589 // find existing telemetry port
1590 for (portIndex
= 0; portIndex
< MAX_MSP_PORT_COUNT
; portIndex
++) {
1591 candidatePort
= &mspPorts
[portIndex
];
1592 if (candidatePort
->mspPortUsage
== FOR_TELEMETRY
) {
1593 matchedPort
= candidatePort
;
1600 for (portIndex
= 0; portIndex
< MAX_MSP_PORT_COUNT
; portIndex
++) {
1601 candidatePort
= &mspPorts
[portIndex
];
1602 if (candidatePort
->mspPortUsage
== UNUSED_PORT
) {
1603 matchedPort
= candidatePort
;
1608 mspTelemetryPort
= matchedPort
;
1609 if (!mspTelemetryPort
) {
1613 resetMspPort(mspTelemetryPort
, serialPort
, FOR_TELEMETRY
);
1616 void sendMspTelemetry(void)
1618 static uint32_t sequenceIndex
= 0;
1620 if (!mspTelemetryPort
) {
1624 setCurrentPort(mspTelemetryPort
);
1626 processOutCommand(mspTelemetryCommandSequence
[sequenceIndex
]);
1630 if (sequenceIndex
>= MSP_TELEMETRY_COMMAND_SEQUENCE_ENTRY_COUNT
) {