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"
27 #include "scheduler.h"
29 #include "common/axis.h"
30 #include "common/color.h"
31 #include "common/maths.h"
33 #include "drivers/system.h"
35 #include "drivers/sensor.h"
36 #include "drivers/accgyro.h"
37 #include "drivers/compass.h"
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 #include "drivers/sdcard.h"
46 #include "drivers/buf_writer.h"
50 #include "io/beeper.h"
51 #include "io/escservo.h"
52 #include "io/rc_controls.h"
54 #include "io/gimbal.h"
55 #include "io/serial.h"
56 #include "io/ledstrip.h"
57 #include "io/flashfs.h"
58 #include "io/transponder_ir.h"
59 #include "io/asyncfatfs/asyncfatfs.h"
61 #include "telemetry/telemetry.h"
63 #include "sensors/boardalignment.h"
64 #include "sensors/sensors.h"
65 #include "sensors/battery.h"
66 #include "sensors/sonar.h"
67 #include "sensors/acceleration.h"
68 #include "sensors/barometer.h"
69 #include "sensors/compass.h"
70 #include "sensors/gyro.h"
72 #include "flight/mixer.h"
73 #include "flight/pid.h"
74 #include "flight/imu.h"
75 #include "flight/failsafe.h"
76 #include "flight/navigation.h"
77 #include "flight/altitudehold.h"
79 #include "blackbox/blackbox.h"
83 #include "config/runtime_config.h"
84 #include "config/config.h"
85 #include "config/config_profile.h"
86 #include "config/config_master.h"
89 #ifdef USE_HARDWARE_REVISION_DETECTION
90 #include "hardware_revision.h"
93 #include "serial_msp.h"
95 #ifdef USE_SERIAL_1WIRE
96 #include "io/serial_1wire.h"
98 static serialPort_t
*mspSerialPort
;
100 extern uint16_t cycleTime
; // FIXME dependency on mw.c
101 extern uint16_t rssi
; // FIXME dependency on mw.c
102 extern void resetPidProfile(pidProfile_t
*pidProfile
);
105 void setGyroSamplingSpeed(uint16_t looptime
) {
106 uint16_t gyroSampleRate
= 1000;
107 uint8_t maxDivider
= 1;
109 if (looptime
!= targetLooptime
|| looptime
== 0) {
110 if (looptime
== 0) looptime
= targetLooptime
; // needed for pid controller changes
112 if (looptime
< 1000) {
113 masterConfig
.gyro_lpf
= 0;
114 gyroSampleRate
= 125;
116 masterConfig
.pid_process_denom
= 1;
117 masterConfig
.acc_hardware
= 0;
118 masterConfig
.baro_hardware
= 0;
119 masterConfig
.mag_hardware
= 0;
120 if (looptime
< 250) {
121 masterConfig
.acc_hardware
= 1;
122 masterConfig
.baro_hardware
= 1;
123 masterConfig
.mag_hardware
= 1;
124 masterConfig
.pid_process_denom
= 2;
125 } else if (looptime
< 375) {
126 #if defined(LUX_RACE) || defined(COLIBRI_RACE) || defined(MOTOLAB) || defined(ALIENFLIGHTF3)
127 masterConfig
.acc_hardware
= 0;
129 masterConfig
.acc_hardware
= 1;
131 masterConfig
.baro_hardware
= 1;
132 masterConfig
.mag_hardware
= 1;
133 masterConfig
.pid_process_denom
= 2;
136 masterConfig
.gyro_lpf
= 0;
137 masterConfig
.gyro_sync_denom
= 8;
138 masterConfig
.pid_process_denom
= 1;
139 masterConfig
.acc_hardware
= 0;
140 masterConfig
.baro_hardware
= 0;
141 masterConfig
.mag_hardware
= 0;
144 if (looptime
< 1000) {
145 masterConfig
.gyro_lpf
= 0;
146 masterConfig
.acc_hardware
= 1;
147 masterConfig
.baro_hardware
= 1;
148 masterConfig
.mag_hardware
= 1;
149 gyroSampleRate
= 125;
151 masterConfig
.pid_process_denom
= 1;
152 if (currentProfile
->pidProfile
.pidController
== 2) masterConfig
.pid_process_denom
= 2;
153 if (looptime
< 250) {
154 masterConfig
.pid_process_denom
= 3;
155 } else if (looptime
< 375) {
156 if (currentProfile
->pidProfile
.pidController
== 2) {
157 masterConfig
.pid_process_denom
= 3;
159 masterConfig
.pid_process_denom
= 2;
163 masterConfig
.gyro_lpf
= 0;
164 masterConfig
.gyro_sync_denom
= 8;
165 masterConfig
.acc_hardware
= 0;
166 masterConfig
.baro_hardware
= 0;
167 masterConfig
.mag_hardware
= 0;
168 masterConfig
.pid_process_denom
= 1;
171 masterConfig
.gyro_sync_denom
= constrain(looptime
/ gyroSampleRate
, 1, maxDivider
);
173 if (!(masterConfig
.use_multiShot
|| masterConfig
.use_oneshot42
) && ((masterConfig
.gyro_sync_denom
* gyroSampleRate
) == 125)) masterConfig
.pid_process_denom
= 3;
177 void useRcControlsConfig(modeActivationCondition_t
*modeActivationConditions
, escAndServoConfig_t
*escAndServoConfigToUse
, pidProfile_t
*pidProfileToUse
);
179 const char * const flightControllerIdentifier
= BETAFLIGHT_IDENTIFIER
; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
181 typedef struct box_e
{
182 const uint8_t boxId
; // see boxId_e
183 const char *boxName
; // GUI-readable box name
184 const uint8_t permanentId
; //
188 static const box_t boxes
[CHECKBOX_ITEM_COUNT
+ 1] = {
189 { BOXARM
, "ARM;", 0 },
190 { BOXANGLE
, "ANGLE;", 1 },
191 { BOXHORIZON
, "HORIZON;", 2 },
192 { BOXBARO
, "BARO;", 3 },
193 //{ BOXVARIO, "VARIO;", 4 },
194 { BOXMAG
, "MAG;", 5 },
195 { BOXHEADFREE
, "HEADFREE;", 6 },
196 { BOXHEADADJ
, "HEADADJ;", 7 },
197 { BOXCAMSTAB
, "CAMSTAB;", 8 },
198 { BOXCAMTRIG
, "CAMTRIG;", 9 },
199 { BOXGPSHOME
, "GPS HOME;", 10 },
200 { BOXGPSHOLD
, "GPS HOLD;", 11 },
201 { BOXPASSTHRU
, "PASSTHRU;", 12 },
202 { BOXBEEPERON
, "BEEPER;", 13 },
203 { BOXLEDMAX
, "LEDMAX;", 14 },
204 { BOXLEDLOW
, "LEDLOW;", 15 },
205 { BOXLLIGHTS
, "LLIGHTS;", 16 },
206 { BOXCALIB
, "CALIB;", 17 },
207 { BOXGOV
, "GOVERNOR;", 18 },
208 { BOXOSD
, "OSD SW;", 19 },
209 { BOXTELEMETRY
, "TELEMETRY;", 20 },
210 { BOXGTUNE
, "GTUNE;", 21 },
211 { BOXSONAR
, "SONAR;", 22 },
212 { BOXSERVO1
, "SERVO1;", 23 },
213 { BOXSERVO2
, "SERVO2;", 24 },
214 { BOXSERVO3
, "SERVO3;", 25 },
215 { BOXBLACKBOX
, "BLACKBOX;", 26 },
216 { BOXFAILSAFE
, "FAILSAFE;", 27 },
217 { BOXAIRMODE
, "AIR MODE;", 28 },
218 { BOXACROPLUS
, "ACRO PLUS;", 29 },
219 { CHECKBOX_ITEM_COUNT
, NULL
, 0xFF }
222 // this is calculated at startup based on enabled features.
223 static uint8_t activeBoxIds
[CHECKBOX_ITEM_COUNT
];
224 // this is the number of filled indexes in above array
225 static uint8_t activeBoxIdCount
= 0;
227 extern int16_t motor_disarmed
[MAX_SUPPORTED_MOTORS
];
229 // cause reboot after MSP processing complete
230 static bool isRebootScheduled
= false;
232 static const char pidnames
[] =
245 MSP_SDCARD_STATE_NOT_PRESENT
= 0,
246 MSP_SDCARD_STATE_FATAL
= 1,
247 MSP_SDCARD_STATE_CARD_INIT
= 2,
248 MSP_SDCARD_STATE_FS_INIT
= 3,
249 MSP_SDCARD_STATE_READY
= 4
253 STATIC_UNIT_TESTED mspPort_t mspPorts
[MAX_MSP_PORT_COUNT
];
255 STATIC_UNIT_TESTED mspPort_t
*currentPort
;
256 STATIC_UNIT_TESTED bufWriter_t
*writer
;
258 static void serialize8(uint8_t a
)
260 bufWriterAppend(writer
, a
);
261 currentPort
->checksum
^= a
;
264 static void serialize16(uint16_t a
)
266 serialize8((uint8_t)(a
>> 0));
267 serialize8((uint8_t)(a
>> 8));
270 static void serialize32(uint32_t a
)
272 serialize16((uint16_t)(a
>> 0));
273 serialize16((uint16_t)(a
>> 16));
276 static uint8_t read8(void)
278 return currentPort
->inBuf
[currentPort
->indRX
++] & 0xff;
281 static uint16_t read16(void)
283 uint16_t t
= read8();
284 t
+= (uint16_t)read8() << 8;
288 static uint32_t read32(void)
290 uint32_t t
= read16();
291 t
+= (uint32_t)read16() << 16;
295 static void headSerialResponse(uint8_t err
, uint8_t responseBodySize
)
297 serialBeginWrite(mspSerialPort
);
301 serialize8(err
? '!' : '>');
302 currentPort
->checksum
= 0; // start calculating a new checksum
303 serialize8(responseBodySize
);
304 serialize8(currentPort
->cmdMSP
);
307 static void headSerialReply(uint8_t responseBodySize
)
309 headSerialResponse(0, responseBodySize
);
312 static void headSerialError(uint8_t responseBodySize
)
314 headSerialResponse(1, responseBodySize
);
317 static void tailSerialReply(void)
319 serialize8(currentPort
->checksum
);
320 serialEndWrite(mspSerialPort
);
323 static void s_struct(uint8_t *cb
, uint8_t siz
)
325 headSerialReply(siz
);
330 static void serializeNames(const char *s
)
337 static const box_t
*findBoxByActiveBoxId(uint8_t activeBoxId
)
340 const box_t
*candidate
;
341 for (boxIndex
= 0; boxIndex
< sizeof(boxes
) / sizeof(box_t
); boxIndex
++) {
342 candidate
= &boxes
[boxIndex
];
343 if (candidate
->boxId
== activeBoxId
) {
350 static const box_t
*findBoxByPermenantId(uint8_t permenantId
)
353 const box_t
*candidate
;
354 for (boxIndex
= 0; boxIndex
< sizeof(boxes
) / sizeof(box_t
); boxIndex
++) {
355 candidate
= &boxes
[boxIndex
];
356 if (candidate
->permanentId
== permenantId
) {
363 static void serializeBoxNamesReply(void)
365 int i
, activeBoxId
, j
, flag
= 1, count
= 0, len
;
369 // in first run of the loop, we grab total size of junk to be sent
370 // then come back and actually send it
371 for (i
= 0; i
< activeBoxIdCount
; i
++) {
372 activeBoxId
= activeBoxIds
[i
];
374 box
= findBoxByActiveBoxId(activeBoxId
);
379 len
= strlen(box
->boxName
);
383 for (j
= 0; j
< len
; j
++)
384 serialize8(box
->boxName
[j
]);
389 headSerialReply(count
);
395 static void serializeSDCardSummaryReply(void)
397 headSerialReply(3 + 4 + 4);
400 uint8_t flags
= 1 /* SD card supported */ ;
405 // Merge the card and filesystem states together
406 if (!sdcard_isInserted()) {
407 state
= MSP_SDCARD_STATE_NOT_PRESENT
;
408 } else if (!sdcard_isFunctional()) {
409 state
= MSP_SDCARD_STATE_FATAL
;
411 switch (afatfs_getFilesystemState()) {
412 case AFATFS_FILESYSTEM_STATE_READY
:
413 state
= MSP_SDCARD_STATE_READY
;
415 case AFATFS_FILESYSTEM_STATE_INITIALIZATION
:
416 if (sdcard_isInitialized()) {
417 state
= MSP_SDCARD_STATE_FS_INIT
;
419 state
= MSP_SDCARD_STATE_CARD_INIT
;
422 case AFATFS_FILESYSTEM_STATE_FATAL
:
423 case AFATFS_FILESYSTEM_STATE_UNKNOWN
:
424 state
= MSP_SDCARD_STATE_FATAL
;
430 serialize8(afatfs_getLastError());
431 // Write free space and total space in kilobytes
432 serialize32(afatfs_getContiguousFreeSpace() / 1024);
433 serialize32(sdcard_getMetadata()->numBlocks
/ 2); // Block size is half a kilobyte
443 static void serializeDataflashSummaryReply(void)
445 headSerialReply(1 + 3 * 4);
447 const flashGeometry_t
*geometry
= flashfsGetGeometry();
448 uint8_t flags
= (flashfsIsReady() ? 1 : 0) | 2 /* FlashFS is supported */;
451 serialize32(geometry
->sectors
);
452 serialize32(geometry
->totalSize
);
453 serialize32(flashfsGetOffset()); // Effectively the current number of bytes stored on the volume
455 serialize8(0); // FlashFS is neither ready nor supported
463 static void serializeDataflashReadReply(uint32_t address
, uint8_t size
)
468 if (size
> sizeof(buffer
)) {
469 size
= sizeof(buffer
);
472 headSerialReply(4 + size
);
474 serialize32(address
);
476 // bytesRead will be lower than that requested if we reach end of volume
477 bytesRead
= flashfsReadAbs(address
, buffer
, size
);
479 for (int i
= 0; i
< bytesRead
; i
++) {
480 serialize8(buffer
[i
]);
485 static void resetMspPort(mspPort_t
*mspPortToReset
, serialPort_t
*serialPort
)
487 memset(mspPortToReset
, 0, sizeof(mspPort_t
));
489 mspPortToReset
->port
= serialPort
;
492 void mspAllocateSerialPorts(serialConfig_t
*serialConfig
)
494 UNUSED(serialConfig
);
496 serialPort_t
*serialPort
;
498 uint8_t portIndex
= 0;
500 serialPortConfig_t
*portConfig
= findSerialPortConfig(FUNCTION_MSP
);
502 while (portConfig
&& portIndex
< MAX_MSP_PORT_COUNT
) {
503 mspPort_t
*mspPort
= &mspPorts
[portIndex
];
509 serialPort
= openSerialPort(portConfig
->identifier
, FUNCTION_MSP
, NULL
, baudRates
[portConfig
->msp_baudrateIndex
], MODE_RXTX
, SERIAL_NOT_INVERTED
);
511 resetMspPort(mspPort
, serialPort
);
515 portConfig
= findNextSerialPortConfig(FUNCTION_MSP
);
519 void mspReleasePortIfAllocated(serialPort_t
*serialPort
)
522 for (portIndex
= 0; portIndex
< MAX_MSP_PORT_COUNT
; portIndex
++) {
523 mspPort_t
*candidateMspPort
= &mspPorts
[portIndex
];
524 if (candidateMspPort
->port
== serialPort
) {
525 closeSerialPort(serialPort
);
526 memset(candidateMspPort
, 0, sizeof(mspPort_t
));
531 void mspInit(serialConfig_t
*serialConfig
)
533 // calculate used boxes based on features and fill availableBoxes[] array
534 memset(activeBoxIds
, 0xFF, sizeof(activeBoxIds
));
536 activeBoxIdCount
= 0;
537 activeBoxIds
[activeBoxIdCount
++] = BOXARM
;
539 if (sensors(SENSOR_ACC
)) {
540 activeBoxIds
[activeBoxIdCount
++] = BOXANGLE
;
541 activeBoxIds
[activeBoxIdCount
++] = BOXHORIZON
;
544 activeBoxIds
[activeBoxIdCount
++] = BOXAIRMODE
;
545 activeBoxIds
[activeBoxIdCount
++] = BOXACROPLUS
;
548 if (sensors(SENSOR_BARO
)) {
549 activeBoxIds
[activeBoxIdCount
++] = BOXBARO
;
552 if (sensors(SENSOR_ACC
) || sensors(SENSOR_MAG
)) {
553 activeBoxIds
[activeBoxIdCount
++] = BOXMAG
;
554 activeBoxIds
[activeBoxIdCount
++] = BOXHEADFREE
;
555 activeBoxIds
[activeBoxIdCount
++] = BOXHEADADJ
;
558 if (feature(FEATURE_SERVO_TILT
))
559 activeBoxIds
[activeBoxIdCount
++] = BOXCAMSTAB
;
562 if (feature(FEATURE_GPS
)) {
563 activeBoxIds
[activeBoxIdCount
++] = BOXGPSHOME
;
564 activeBoxIds
[activeBoxIdCount
++] = BOXGPSHOLD
;
568 if (masterConfig
.mixerMode
== MIXER_FLYING_WING
|| masterConfig
.mixerMode
== MIXER_AIRPLANE
)
569 activeBoxIds
[activeBoxIdCount
++] = BOXPASSTHRU
;
571 activeBoxIds
[activeBoxIdCount
++] = BOXBEEPERON
;
574 if (feature(FEATURE_LED_STRIP
)) {
575 activeBoxIds
[activeBoxIdCount
++] = BOXLEDLOW
;
579 if (feature(FEATURE_INFLIGHT_ACC_CAL
))
580 activeBoxIds
[activeBoxIdCount
++] = BOXCALIB
;
582 activeBoxIds
[activeBoxIdCount
++] = BOXOSD
;
584 if (feature(FEATURE_TELEMETRY
) && masterConfig
.telemetryConfig
.telemetry_switch
)
585 activeBoxIds
[activeBoxIdCount
++] = BOXTELEMETRY
;
587 if (feature(FEATURE_SONAR
)){
588 activeBoxIds
[activeBoxIdCount
++] = BOXSONAR
;
592 if (masterConfig
.mixerMode
== MIXER_CUSTOM_AIRPLANE
) {
593 activeBoxIds
[activeBoxIdCount
++] = BOXSERVO1
;
594 activeBoxIds
[activeBoxIdCount
++] = BOXSERVO2
;
595 activeBoxIds
[activeBoxIdCount
++] = BOXSERVO3
;
600 if (feature(FEATURE_BLACKBOX
)){
601 activeBoxIds
[activeBoxIdCount
++] = BOXBLACKBOX
;
605 if (feature(FEATURE_FAILSAFE
)){
606 activeBoxIds
[activeBoxIdCount
++] = BOXFAILSAFE
;
610 activeBoxIds
[activeBoxIdCount
++] = BOXGTUNE
;
613 memset(mspPorts
, 0x00, sizeof(mspPorts
));
614 mspAllocateSerialPorts(serialConfig
);
617 #define IS_ENABLED(mask) (mask == 0 ? 0 : 1)
619 static uint32_t packFlightModeFlags(void)
621 uint32_t i
, junk
, tmp
;
623 // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
624 // Requires new Multiwii protocol version to fix
625 // It would be preferable to setting the enabled bits based on BOXINDEX.
627 tmp
= IS_ENABLED(FLIGHT_MODE(ANGLE_MODE
)) << BOXANGLE
|
628 IS_ENABLED(FLIGHT_MODE(HORIZON_MODE
)) << BOXHORIZON
|
629 IS_ENABLED(FLIGHT_MODE(BARO_MODE
)) << BOXBARO
|
630 IS_ENABLED(FLIGHT_MODE(MAG_MODE
)) << BOXMAG
|
631 IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE
)) << BOXHEADFREE
|
632 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ
)) << BOXHEADADJ
|
633 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB
)) << BOXCAMSTAB
|
634 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMTRIG
)) << BOXCAMTRIG
|
635 IS_ENABLED(FLIGHT_MODE(GPS_HOME_MODE
)) << BOXGPSHOME
|
636 IS_ENABLED(FLIGHT_MODE(GPS_HOLD_MODE
)) << BOXGPSHOLD
|
637 IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE
)) << BOXPASSTHRU
|
638 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON
)) << BOXBEEPERON
|
639 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX
)) << BOXLEDMAX
|
640 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW
)) << BOXLEDLOW
|
641 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS
)) << BOXLLIGHTS
|
642 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB
)) << BOXCALIB
|
643 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV
)) << BOXGOV
|
644 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD
)) << BOXOSD
|
645 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY
)) << BOXTELEMETRY
|
646 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE
)) << BOXGTUNE
|
647 IS_ENABLED(FLIGHT_MODE(SONAR_MODE
)) << BOXSONAR
|
648 IS_ENABLED(ARMING_FLAG(ARMED
)) << BOXARM
|
649 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX
)) << BOXBLACKBOX
|
650 IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE
)) << BOXFAILSAFE
|
651 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE
)) << BOXAIRMODE
|
652 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXACROPLUS
)) << BOXACROPLUS
;
654 for (i
= 0; i
< activeBoxIdCount
; i
++) {
655 int flag
= (tmp
& (1 << activeBoxIds
[i
]));
663 static bool processOutCommand(uint8_t cmdMSP
)
669 int32_t lat
= 0, lon
= 0;
673 case MSP_API_VERSION
:
675 1 + // protocol version length
678 serialize8(MSP_PROTOCOL_VERSION
);
680 serialize8(API_VERSION_MAJOR
);
681 serialize8(API_VERSION_MINOR
);
685 headSerialReply(FLIGHT_CONTROLLER_IDENTIFIER_LENGTH
);
687 for (i
= 0; i
< FLIGHT_CONTROLLER_IDENTIFIER_LENGTH
; i
++) {
688 serialize8(flightControllerIdentifier
[i
]);
693 headSerialReply(FLIGHT_CONTROLLER_VERSION_LENGTH
);
695 serialize8(FC_VERSION_MAJOR
);
696 serialize8(FC_VERSION_MINOR
);
697 serialize8(FC_VERSION_PATCH_LEVEL
);
702 BOARD_IDENTIFIER_LENGTH
+
703 BOARD_HARDWARE_REVISION_LENGTH
705 for (i
= 0; i
< BOARD_IDENTIFIER_LENGTH
; i
++) {
706 serialize8(boardIdentifier
[i
]);
708 #ifdef USE_HARDWARE_REVISION_DETECTION
709 serialize16(hardwareRevision
);
711 serialize16(0); // No other build targets currently have hardware revision detection.
719 GIT_SHORT_REVISION_LENGTH
722 for (i
= 0; i
< BUILD_DATE_LENGTH
; i
++) {
723 serialize8(buildDate
[i
]);
725 for (i
= 0; i
< BUILD_TIME_LENGTH
; i
++) {
726 serialize8(buildTime
[i
]);
729 for (i
= 0; i
< GIT_SHORT_REVISION_LENGTH
; i
++) {
730 serialize8(shortGitRevision
[i
]);
734 // DEPRECATED - Use MSP_API_VERSION
737 serialize8(MW_VERSION
);
738 serialize8(masterConfig
.mixerMode
);
739 serialize8(MSP_PROTOCOL_VERSION
);
740 serialize32(CAP_DYNBALANCE
); // "capability"
745 serialize16(cycleTime
);
747 serialize16(i2cGetErrorCounter());
751 serialize16(sensors(SENSOR_ACC
) | sensors(SENSOR_BARO
) << 1 | sensors(SENSOR_MAG
) << 2 | sensors(SENSOR_GPS
) << 3 | sensors(SENSOR_SONAR
) << 4);
752 serialize32(packFlightModeFlags());
753 serialize8(masterConfig
.current_profile_index
);
754 //serialize16(averageSystemLoadPercent);
759 serialize16(cycleTime
);
761 serialize16(i2cGetErrorCounter());
765 serialize16(sensors(SENSOR_ACC
) | sensors(SENSOR_BARO
) << 1 | sensors(SENSOR_MAG
) << 2 | sensors(SENSOR_GPS
) << 3 | sensors(SENSOR_SONAR
) << 4);
766 serialize32(packFlightModeFlags());
767 serialize8(masterConfig
.current_profile_index
);
772 // Hack scale due to choice of units for sensor data in multiwii
773 uint8_t scale
= (acc_1G
> 1024) ? 8 : 1;
775 for (i
= 0; i
< 3; i
++)
776 serialize16(accSmooth
[i
] / scale
);
777 for (i
= 0; i
< 3; i
++)
778 serialize16(gyroADC
[i
]);
779 for (i
= 0; i
< 3; i
++)
780 serialize16(magADC
[i
]);
784 s_struct((uint8_t *)&servo
, MAX_SUPPORTED_SERVOS
* 2);
786 case MSP_SERVO_CONFIGURATIONS
:
787 headSerialReply(MAX_SUPPORTED_SERVOS
* sizeof(servoParam_t
));
788 for (i
= 0; i
< MAX_SUPPORTED_SERVOS
; i
++) {
789 serialize16(masterConfig
.servoConf
[i
].min
);
790 serialize16(masterConfig
.servoConf
[i
].max
);
791 serialize16(masterConfig
.servoConf
[i
].middle
);
792 serialize8(masterConfig
.servoConf
[i
].rate
);
793 serialize8(masterConfig
.servoConf
[i
].angleAtMin
);
794 serialize8(masterConfig
.servoConf
[i
].angleAtMax
);
795 serialize8(masterConfig
.servoConf
[i
].forwardFromChannel
);
796 serialize32(masterConfig
.servoConf
[i
].reversedSources
);
799 case MSP_SERVO_MIX_RULES
:
800 headSerialReply(MAX_SERVO_RULES
* sizeof(servoMixer_t
));
801 for (i
= 0; i
< MAX_SERVO_RULES
; i
++) {
802 serialize8(masterConfig
.customServoMixer
[i
].targetChannel
);
803 serialize8(masterConfig
.customServoMixer
[i
].inputSource
);
804 serialize8(masterConfig
.customServoMixer
[i
].rate
);
805 serialize8(masterConfig
.customServoMixer
[i
].speed
);
806 serialize8(masterConfig
.customServoMixer
[i
].min
);
807 serialize8(masterConfig
.customServoMixer
[i
].max
);
808 serialize8(masterConfig
.customServoMixer
[i
].box
);
813 s_struct((uint8_t *)motor
, 16);
816 headSerialReply(2 * rxRuntimeConfig
.channelCount
);
817 for (i
= 0; i
< rxRuntimeConfig
.channelCount
; i
++)
818 serialize16(rcData
[i
]);
822 serialize16(attitude
.values
.roll
);
823 serialize16(attitude
.values
.pitch
);
824 serialize16(DECIDEGREES_TO_DEGREES(attitude
.values
.yaw
));
828 #if defined(BARO) || defined(SONAR)
829 serialize32(altitudeHoldGetEstimatedAltitude());
835 case MSP_SONAR_ALTITUDE
:
838 serialize32(sonarGetLatestAltitude());
845 serialize8((uint8_t)constrain(vbat
, 0, 255));
846 serialize16((uint16_t)constrain(mAhDrawn
, 0, 0xFFFF)); // milliamp hours drawn from battery
848 if(masterConfig
.batteryConfig
.multiwiiCurrentMeterOutput
) {
849 serialize16((uint16_t)constrain(amperage
* 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
851 serialize16((int16_t)constrain(amperage
, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
853 case MSP_ARMING_CONFIG
:
855 serialize8(masterConfig
.auto_disarm_delay
);
856 serialize8(masterConfig
.disarm_kill_switch
);
860 serialize16((uint16_t)targetLooptime
);
864 serialize8(currentControlRateProfile
->rcRate8
);
865 serialize8(currentControlRateProfile
->rcExpo8
);
866 for (i
= 0 ; i
< 3; i
++) {
867 serialize8(currentControlRateProfile
->rates
[i
]); // R,P,Y see flight_dynamics_index_t
869 serialize8(currentControlRateProfile
->dynThrPID
);
870 serialize8(currentControlRateProfile
->thrMid8
);
871 serialize8(currentControlRateProfile
->thrExpo8
);
872 serialize16(currentControlRateProfile
->tpa_breakpoint
);
873 serialize8(currentControlRateProfile
->rcYawExpo8
);
876 headSerialReply(3 * PID_ITEM_COUNT
);
877 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
878 for (i
= 0; i
< 3; i
++) {
879 serialize8(constrain(lrintf(currentProfile
->pidProfile
.P_f
[i
] * 10.0f
), 0, 255));
880 serialize8(constrain(lrintf(currentProfile
->pidProfile
.I_f
[i
] * 100.0f
), 0, 255));
881 serialize8(constrain(lrintf(currentProfile
->pidProfile
.D_f
[i
] * 1000.0f
), 0, 255));
883 for (i
= 3; i
< PID_ITEM_COUNT
; i
++) {
885 serialize8(constrain(lrintf(currentProfile
->pidProfile
.A_level
* 10.0f
), 0, 255));
886 serialize8(constrain(lrintf(currentProfile
->pidProfile
.H_level
* 10.0f
), 0, 255));
887 serialize8(constrain(lrintf(currentProfile
->pidProfile
.H_sensitivity
), 0, 255));
889 serialize8(currentProfile
->pidProfile
.P8
[i
]);
890 serialize8(currentProfile
->pidProfile
.I8
[i
]);
891 serialize8(currentProfile
->pidProfile
.D8
[i
]);
895 for (i
= 0; i
< PID_ITEM_COUNT
; i
++) {
896 serialize8(currentProfile
->pidProfile
.P8
[i
]);
897 serialize8(currentProfile
->pidProfile
.I8
[i
]);
898 serialize8(currentProfile
->pidProfile
.D8
[i
]);
903 headSerialReply(sizeof(pidnames
) - 1);
904 serializeNames(pidnames
);
906 case MSP_PID_CONTROLLER
:
908 serialize8(currentProfile
->pidProfile
.pidController
);
910 case MSP_MODE_RANGES
:
911 headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT
);
912 for (i
= 0; i
< MAX_MODE_ACTIVATION_CONDITION_COUNT
; i
++) {
913 modeActivationCondition_t
*mac
= &masterConfig
.modeActivationConditions
[i
];
914 const box_t
*box
= &boxes
[mac
->modeId
];
915 serialize8(box
->permanentId
);
916 serialize8(mac
->auxChannelIndex
);
917 serialize8(mac
->range
.startStep
);
918 serialize8(mac
->range
.endStep
);
921 case MSP_ADJUSTMENT_RANGES
:
922 headSerialReply(MAX_ADJUSTMENT_RANGE_COUNT
* (
923 1 + // adjustment index/slot
924 1 + // aux channel index
927 1 + // adjustment function
928 1 // aux switch channel index
930 for (i
= 0; i
< MAX_ADJUSTMENT_RANGE_COUNT
; i
++) {
931 adjustmentRange_t
*adjRange
= &masterConfig
.adjustmentRanges
[i
];
932 serialize8(adjRange
->adjustmentIndex
);
933 serialize8(adjRange
->auxChannelIndex
);
934 serialize8(adjRange
->range
.startStep
);
935 serialize8(adjRange
->range
.endStep
);
936 serialize8(adjRange
->adjustmentFunction
);
937 serialize8(adjRange
->auxSwitchChannelIndex
);
941 serializeBoxNamesReply();
944 headSerialReply(activeBoxIdCount
);
945 for (i
= 0; i
< activeBoxIdCount
; i
++) {
946 const box_t
*box
= findBoxByActiveBoxId(activeBoxIds
[i
]);
950 serialize8(box
->permanentId
);
954 headSerialReply(2 * 5 + 3 + 3 + 2 + 4);
955 serialize16(masterConfig
.rxConfig
.midrc
);
957 serialize16(masterConfig
.escAndServoConfig
.minthrottle
);
958 serialize16(masterConfig
.escAndServoConfig
.maxthrottle
);
959 serialize16(masterConfig
.escAndServoConfig
.mincommand
);
961 serialize16(masterConfig
.failsafeConfig
.failsafe_throttle
);
964 serialize8(masterConfig
.gpsConfig
.provider
); // gps_type
965 serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
966 serialize8(masterConfig
.gpsConfig
.sbasMode
); // gps_ubx_sbas
968 serialize8(0); // gps_type
969 serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
970 serialize8(0); // gps_ubx_sbas
972 serialize8(masterConfig
.batteryConfig
.multiwiiCurrentMeterOutput
);
973 serialize8(masterConfig
.rxConfig
.rssi_channel
);
976 serialize16(masterConfig
.mag_declination
/ 10);
978 serialize8(masterConfig
.batteryConfig
.vbatscale
);
979 serialize8(masterConfig
.batteryConfig
.vbatmincellvoltage
);
980 serialize8(masterConfig
.batteryConfig
.vbatmaxcellvoltage
);
981 serialize8(masterConfig
.batteryConfig
.vbatwarningcellvoltage
);
985 // FIXME This is hardcoded and should not be.
987 for (i
= 0; i
< 8; i
++)
993 serialize8(STATE(GPS_FIX
));
994 serialize8(GPS_numSat
);
995 serialize32(GPS_coord
[LAT
]);
996 serialize32(GPS_coord
[LON
]);
997 serialize16(GPS_altitude
);
998 serialize16(GPS_speed
);
999 serialize16(GPS_ground_course
);
1003 serialize16(GPS_distanceToHome
);
1004 serialize16(GPS_directionToHome
);
1005 serialize8(GPS_update
& 1);
1008 wp_no
= read8(); // get the wp number
1009 headSerialReply(18);
1011 lat
= GPS_home
[LAT
];
1012 lon
= GPS_home
[LON
];
1013 } else if (wp_no
== 16) {
1014 lat
= GPS_hold
[LAT
];
1015 lon
= GPS_hold
[LON
];
1020 serialize32(AltHold
); // altitude (cm) will come here -- temporary implementation to test feature with apps
1021 serialize16(0); // heading will come here (deg)
1022 serialize16(0); // time to stay (ms) will come here
1023 serialize8(0); // nav flag will come here
1026 headSerialReply(1 + (GPS_numCh
* 4));
1027 serialize8(GPS_numCh
);
1028 for (i
= 0; i
< GPS_numCh
; i
++){
1029 serialize8(GPS_svinfo_chn
[i
]);
1030 serialize8(GPS_svinfo_svid
[i
]);
1031 serialize8(GPS_svinfo_quality
[i
]);
1032 serialize8(GPS_svinfo_cno
[i
]);
1037 headSerialReply(DEBUG16_VALUE_COUNT
* sizeof(debug
[0]));
1039 // output some useful QA statistics
1040 // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock]
1042 for (i
= 0; i
< DEBUG16_VALUE_COUNT
; i
++)
1043 serialize16(debug
[i
]); // 4 variables are here for general monitoring purpose
1046 // Additional commands that are not compatible with MultiWii
1049 serialize16(masterConfig
.accelerometerTrims
.values
.pitch
);
1050 serialize16(masterConfig
.accelerometerTrims
.values
.roll
);
1054 headSerialReply(12);
1055 serialize32(U_ID_0
);
1056 serialize32(U_ID_1
);
1057 serialize32(U_ID_2
);
1062 serialize32(featureMask());
1065 case MSP_BOARD_ALIGNMENT
:
1067 serialize16(masterConfig
.boardAlignment
.rollDegrees
);
1068 serialize16(masterConfig
.boardAlignment
.pitchDegrees
);
1069 serialize16(masterConfig
.boardAlignment
.yawDegrees
);
1072 case MSP_VOLTAGE_METER_CONFIG
:
1074 serialize8(masterConfig
.batteryConfig
.vbatscale
);
1075 serialize8(masterConfig
.batteryConfig
.vbatmincellvoltage
);
1076 serialize8(masterConfig
.batteryConfig
.vbatmaxcellvoltage
);
1077 serialize8(masterConfig
.batteryConfig
.vbatwarningcellvoltage
);
1080 case MSP_CURRENT_METER_CONFIG
:
1082 serialize16(masterConfig
.batteryConfig
.currentMeterScale
);
1083 serialize16(masterConfig
.batteryConfig
.currentMeterOffset
);
1084 serialize8(masterConfig
.batteryConfig
.currentMeterType
);
1085 serialize16(masterConfig
.batteryConfig
.batteryCapacity
);
1090 serialize8(masterConfig
.mixerMode
);
1094 headSerialReply(12);
1095 serialize8(masterConfig
.rxConfig
.serialrx_provider
);
1096 serialize16(masterConfig
.rxConfig
.maxcheck
);
1097 serialize16(masterConfig
.rxConfig
.midrc
);
1098 serialize16(masterConfig
.rxConfig
.mincheck
);
1099 serialize8(masterConfig
.rxConfig
.spektrum_sat_bind
);
1100 serialize16(masterConfig
.rxConfig
.rx_min_usec
);
1101 serialize16(masterConfig
.rxConfig
.rx_max_usec
);
1104 case MSP_FAILSAFE_CONFIG
:
1106 serialize8(masterConfig
.failsafeConfig
.failsafe_delay
);
1107 serialize8(masterConfig
.failsafeConfig
.failsafe_off_delay
);
1108 serialize16(masterConfig
.failsafeConfig
.failsafe_throttle
);
1109 serialize8(masterConfig
.failsafeConfig
.failsafe_kill_switch
);
1110 serialize16(masterConfig
.failsafeConfig
.failsafe_throttle_low_delay
);
1111 serialize8(masterConfig
.failsafeConfig
.failsafe_procedure
);
1114 case MSP_RXFAIL_CONFIG
:
1115 headSerialReply(3 * (rxRuntimeConfig
.channelCount
));
1116 for (i
= 0; i
< rxRuntimeConfig
.channelCount
; i
++) {
1117 serialize8(masterConfig
.rxConfig
.failsafe_channel_configurations
[i
].mode
);
1118 serialize16(RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig
.rxConfig
.failsafe_channel_configurations
[i
].step
));
1122 case MSP_RSSI_CONFIG
:
1124 serialize8(masterConfig
.rxConfig
.rssi_channel
);
1128 headSerialReply(MAX_MAPPABLE_RX_INPUTS
);
1129 for (i
= 0; i
< MAX_MAPPABLE_RX_INPUTS
; i
++)
1130 serialize8(masterConfig
.rxConfig
.rcmap
[i
]);
1134 headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 2 + 2);
1135 serialize8(masterConfig
.mixerMode
);
1137 serialize32(featureMask());
1139 serialize8(masterConfig
.rxConfig
.serialrx_provider
);
1141 serialize16(masterConfig
.boardAlignment
.rollDegrees
);
1142 serialize16(masterConfig
.boardAlignment
.pitchDegrees
);
1143 serialize16(masterConfig
.boardAlignment
.yawDegrees
);
1145 serialize16(masterConfig
.batteryConfig
.currentMeterScale
);
1146 serialize16(masterConfig
.batteryConfig
.currentMeterOffset
);
1149 case MSP_CF_SERIAL_CONFIG
:
1151 ((sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4)) * serialGetAvailablePortCount())
1153 for (i
= 0; i
< SERIAL_PORT_COUNT
; i
++) {
1154 if (!serialIsPortAvailable(masterConfig
.serialConfig
.portConfigs
[i
].identifier
)) {
1157 serialize8(masterConfig
.serialConfig
.portConfigs
[i
].identifier
);
1158 serialize16(masterConfig
.serialConfig
.portConfigs
[i
].functionMask
);
1159 serialize8(masterConfig
.serialConfig
.portConfigs
[i
].msp_baudrateIndex
);
1160 serialize8(masterConfig
.serialConfig
.portConfigs
[i
].gps_baudrateIndex
);
1161 serialize8(masterConfig
.serialConfig
.portConfigs
[i
].telemetry_baudrateIndex
);
1162 serialize8(masterConfig
.serialConfig
.portConfigs
[i
].blackbox_baudrateIndex
);
1167 case MSP_LED_COLORS
:
1168 headSerialReply(CONFIGURABLE_COLOR_COUNT
* 4);
1169 for (i
= 0; i
< CONFIGURABLE_COLOR_COUNT
; i
++) {
1170 hsvColor_t
*color
= &masterConfig
.colors
[i
];
1171 serialize16(color
->h
);
1172 serialize8(color
->s
);
1173 serialize8(color
->v
);
1177 case MSP_LED_STRIP_CONFIG
:
1178 headSerialReply(MAX_LED_STRIP_LENGTH
* 7);
1179 for (i
= 0; i
< MAX_LED_STRIP_LENGTH
; i
++) {
1180 ledConfig_t
*ledConfig
= &masterConfig
.ledConfigs
[i
];
1181 serialize16((ledConfig
->flags
& LED_DIRECTION_MASK
) >> LED_DIRECTION_BIT_OFFSET
);
1182 serialize16((ledConfig
->flags
& LED_FUNCTION_MASK
) >> LED_FUNCTION_BIT_OFFSET
);
1183 serialize8(GET_LED_X(ledConfig
));
1184 serialize8(GET_LED_Y(ledConfig
));
1185 serialize8(ledConfig
->color
);
1190 case MSP_DATAFLASH_SUMMARY
:
1191 serializeDataflashSummaryReply();
1195 case MSP_DATAFLASH_READ
:
1197 uint32_t readAddress
= read32();
1199 serializeDataflashReadReply(readAddress
, 128);
1204 case MSP_BLACKBOX_CONFIG
:
1208 serialize8(1); //Blackbox supported
1209 serialize8(masterConfig
.blackbox_device
);
1210 serialize8(masterConfig
.blackbox_rate_num
);
1211 serialize8(masterConfig
.blackbox_rate_denom
);
1213 serialize8(0); // Blackbox not supported
1220 case MSP_SDCARD_SUMMARY
:
1221 serializeSDCardSummaryReply();
1224 case MSP_TRANSPONDER_CONFIG
:
1226 headSerialReply(1 + sizeof(masterConfig
.transponderData
));
1228 serialize8(1); //Transponder supported
1230 for (i
= 0; i
< sizeof(masterConfig
.transponderData
); i
++) {
1231 serialize8(masterConfig
.transponderData
[i
]);
1235 serialize8(0); // Transponder not supported
1239 case MSP_BF_BUILD_INFO
:
1240 headSerialReply(11 + 4 + 4);
1241 for (i
= 0; i
< 11; i
++)
1242 serialize8(buildDate
[i
]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
1243 serialize32(0); // future exp
1244 serialize32(0); // future exp
1248 headSerialReply(2 * 4);
1249 serialize16(masterConfig
.flight3DConfig
.deadband3d_low
);
1250 serialize16(masterConfig
.flight3DConfig
.deadband3d_high
);
1251 serialize16(masterConfig
.flight3DConfig
.neutral3d
);
1252 serialize16(masterConfig
.flight3DConfig
.deadband3d_throttle
);
1255 case MSP_RC_DEADBAND
:
1257 serialize8(masterConfig
.rcControlsConfig
.deadband
);
1258 serialize8(masterConfig
.rcControlsConfig
.yaw_deadband
);
1259 serialize8(masterConfig
.rcControlsConfig
.alt_hold_deadband
);
1261 case MSP_SENSOR_ALIGNMENT
:
1263 serialize8(masterConfig
.sensorAlignmentConfig
.gyro_align
);
1264 serialize8(masterConfig
.sensorAlignmentConfig
.acc_align
);
1265 serialize8(masterConfig
.sensorAlignmentConfig
.mag_align
);
1274 static bool processInCommand(void)
1282 int32_t lat
= 0, lon
= 0, alt
= 0;
1285 switch (currentPort
->cmdMSP
) {
1286 case MSP_SELECT_SETTING
:
1287 if (!ARMING_FLAG(ARMED
)) {
1288 masterConfig
.current_profile_index
= read8();
1289 if (masterConfig
.current_profile_index
> 1) {
1290 masterConfig
.current_profile_index
= 0;
1299 case MSP_SET_RAW_RC
:
1301 uint8_t channelCount
= currentPort
->dataSize
/ sizeof(uint16_t);
1302 if (channelCount
> MAX_SUPPORTED_RC_CHANNEL_COUNT
) {
1305 uint16_t frame
[MAX_SUPPORTED_RC_CHANNEL_COUNT
];
1307 for (i
= 0; i
< channelCount
; i
++) {
1308 frame
[i
] = read16();
1311 rxMspFrameReceive(frame
, channelCount
);
1315 case MSP_SET_ACC_TRIM
:
1316 masterConfig
.accelerometerTrims
.values
.pitch
= read16();
1317 masterConfig
.accelerometerTrims
.values
.roll
= read16();
1319 case MSP_SET_ARMING_CONFIG
:
1320 masterConfig
.auto_disarm_delay
= read8();
1321 masterConfig
.disarm_kill_switch
= read8();
1323 case MSP_SET_LOOP_TIME
:
1324 setGyroSamplingSpeed(read16());
1326 case MSP_SET_PID_CONTROLLER
:
1327 oldPid
= currentProfile
->pidProfile
.pidController
;
1328 currentProfile
->pidProfile
.pidController
= read8();
1329 pidSetController(currentProfile
->pidProfile
.pidController
);
1330 if (oldPid
!= currentProfile
->pidProfile
.pidController
) setGyroSamplingSpeed(0); // recalculate looptimes for new PID
1333 if (IS_PID_CONTROLLER_FP_BASED(currentProfile
->pidProfile
.pidController
)) {
1334 for (i
= 0; i
< 3; i
++) {
1335 currentProfile
->pidProfile
.P_f
[i
] = (float)read8() / 10.0f
;
1336 currentProfile
->pidProfile
.I_f
[i
] = (float)read8() / 100.0f
;
1337 currentProfile
->pidProfile
.D_f
[i
] = (float)read8() / 1000.0f
;
1339 for (i
= 3; i
< PID_ITEM_COUNT
; i
++) {
1340 if (i
== PIDLEVEL
) {
1341 currentProfile
->pidProfile
.A_level
= (float)read8() / 10.0f
;
1342 currentProfile
->pidProfile
.H_level
= (float)read8() / 10.0f
;
1343 currentProfile
->pidProfile
.H_sensitivity
= read8();
1345 currentProfile
->pidProfile
.P8
[i
] = read8();
1346 currentProfile
->pidProfile
.I8
[i
] = read8();
1347 currentProfile
->pidProfile
.D8
[i
] = read8();
1351 for (i
= 0; i
< PID_ITEM_COUNT
; i
++) {
1352 currentProfile
->pidProfile
.P8
[i
] = read8();
1353 currentProfile
->pidProfile
.I8
[i
] = read8();
1354 currentProfile
->pidProfile
.D8
[i
] = read8();
1358 case MSP_SET_MODE_RANGE
:
1360 if (i
< MAX_MODE_ACTIVATION_CONDITION_COUNT
) {
1361 modeActivationCondition_t
*mac
= &masterConfig
.modeActivationConditions
[i
];
1363 const box_t
*box
= findBoxByPermenantId(i
);
1365 mac
->modeId
= box
->boxId
;
1366 mac
->auxChannelIndex
= read8();
1367 mac
->range
.startStep
= read8();
1368 mac
->range
.endStep
= read8();
1370 useRcControlsConfig(masterConfig
.modeActivationConditions
, &masterConfig
.escAndServoConfig
, ¤tProfile
->pidProfile
);
1378 case MSP_SET_ADJUSTMENT_RANGE
:
1380 if (i
< MAX_ADJUSTMENT_RANGE_COUNT
) {
1381 adjustmentRange_t
*adjRange
= &masterConfig
.adjustmentRanges
[i
];
1383 if (i
< MAX_SIMULTANEOUS_ADJUSTMENT_COUNT
) {
1384 adjRange
->adjustmentIndex
= i
;
1385 adjRange
->auxChannelIndex
= read8();
1386 adjRange
->range
.startStep
= read8();
1387 adjRange
->range
.endStep
= read8();
1388 adjRange
->adjustmentFunction
= read8();
1389 adjRange
->auxSwitchChannelIndex
= read8();
1398 case MSP_SET_RC_TUNING
:
1399 if (currentPort
->dataSize
>= 10) {
1400 currentControlRateProfile
->rcRate8
= read8();
1401 currentControlRateProfile
->rcExpo8
= read8();
1402 for (i
= 0; i
< 3; i
++) {
1404 currentControlRateProfile
->rates
[i
] = MIN(rate
, i
== FD_YAW
? CONTROL_RATE_CONFIG_YAW_RATE_MAX
: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX
);
1407 currentControlRateProfile
->dynThrPID
= MIN(rate
, CONTROL_RATE_CONFIG_TPA_MAX
);
1408 currentControlRateProfile
->thrMid8
= read8();
1409 currentControlRateProfile
->thrExpo8
= read8();
1410 currentControlRateProfile
->tpa_breakpoint
= read16();
1411 if (currentPort
->dataSize
>= 11) {
1412 currentControlRateProfile
->rcYawExpo8
= read8();
1420 if (tmp
< 1600 && tmp
> 1400)
1421 masterConfig
.rxConfig
.midrc
= tmp
;
1423 masterConfig
.escAndServoConfig
.minthrottle
= read16();
1424 masterConfig
.escAndServoConfig
.maxthrottle
= read16();
1425 masterConfig
.escAndServoConfig
.mincommand
= read16();
1427 masterConfig
.failsafeConfig
.failsafe_throttle
= read16();
1430 masterConfig
.gpsConfig
.provider
= read8(); // gps_type
1431 read8(); // gps_baudrate
1432 masterConfig
.gpsConfig
.sbasMode
= read8(); // gps_ubx_sbas
1434 read8(); // gps_type
1435 read8(); // gps_baudrate
1436 read8(); // gps_ubx_sbas
1438 masterConfig
.batteryConfig
.multiwiiCurrentMeterOutput
= read8();
1439 masterConfig
.rxConfig
.rssi_channel
= read8();
1442 masterConfig
.mag_declination
= read16() * 10;
1444 masterConfig
.batteryConfig
.vbatscale
= read8(); // actual vbatscale as intended
1445 masterConfig
.batteryConfig
.vbatmincellvoltage
= read8(); // vbatlevel_warn1 in MWC2.3 GUI
1446 masterConfig
.batteryConfig
.vbatmaxcellvoltage
= read8(); // vbatlevel_warn2 in MWC2.3 GUI
1447 masterConfig
.batteryConfig
.vbatwarningcellvoltage
= read8(); // vbatlevel when buzzer starts to alert
1450 for (i
= 0; i
< 8; i
++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8
1451 motor_disarmed
[i
] = read16();
1453 case MSP_SET_SERVO_CONFIGURATION
:
1455 if (currentPort
->dataSize
!= 1 + sizeof(servoParam_t
)) {
1460 if (i
>= MAX_SUPPORTED_SERVOS
) {
1463 masterConfig
.servoConf
[i
].min
= read16();
1464 masterConfig
.servoConf
[i
].max
= read16();
1465 masterConfig
.servoConf
[i
].middle
= read16();
1466 masterConfig
.servoConf
[i
].rate
= read8();
1467 masterConfig
.servoConf
[i
].angleAtMin
= read8();
1468 masterConfig
.servoConf
[i
].angleAtMax
= read8();
1469 masterConfig
.servoConf
[i
].forwardFromChannel
= read8();
1470 masterConfig
.servoConf
[i
].reversedSources
= read32();
1475 case MSP_SET_SERVO_MIX_RULE
:
1478 if (i
>= MAX_SERVO_RULES
) {
1481 masterConfig
.customServoMixer
[i
].targetChannel
= read8();
1482 masterConfig
.customServoMixer
[i
].inputSource
= read8();
1483 masterConfig
.customServoMixer
[i
].rate
= read8();
1484 masterConfig
.customServoMixer
[i
].speed
= read8();
1485 masterConfig
.customServoMixer
[i
].min
= read8();
1486 masterConfig
.customServoMixer
[i
].max
= read8();
1487 masterConfig
.customServoMixer
[i
].box
= read8();
1488 loadCustomServoMixer();
1494 masterConfig
.flight3DConfig
.deadband3d_low
= read16();
1495 masterConfig
.flight3DConfig
.deadband3d_high
= read16();
1496 masterConfig
.flight3DConfig
.neutral3d
= read16();
1497 masterConfig
.flight3DConfig
.deadband3d_throttle
= read16();
1500 case MSP_SET_RC_DEADBAND
:
1501 masterConfig
.rcControlsConfig
.deadband
= read8();
1502 masterConfig
.rcControlsConfig
.yaw_deadband
= read8();
1503 masterConfig
.rcControlsConfig
.alt_hold_deadband
= read8();
1506 case MSP_SET_RESET_CURR_PID
:
1507 //resetPidProfile(¤tProfile->pidProfile);
1510 case MSP_SET_SENSOR_ALIGNMENT
:
1511 masterConfig
.sensorAlignmentConfig
.gyro_align
= read8();
1512 masterConfig
.sensorAlignmentConfig
.acc_align
= read8();
1513 masterConfig
.sensorAlignmentConfig
.mag_align
= read8();
1516 case MSP_RESET_CONF
:
1517 if (!ARMING_FLAG(ARMED
)) {
1522 case MSP_ACC_CALIBRATION
:
1523 if (!ARMING_FLAG(ARMED
))
1524 accSetCalibrationCycles(CALIBRATING_ACC_CYCLES
);
1526 case MSP_MAG_CALIBRATION
:
1527 if (!ARMING_FLAG(ARMED
))
1528 ENABLE_STATE(CALIBRATE_MAG
);
1530 case MSP_EEPROM_WRITE
:
1531 if (ARMING_FLAG(ARMED
)) {
1540 case MSP_SET_BLACKBOX_CONFIG
:
1541 // Don't allow config to be updated while Blackbox is logging
1542 if (blackboxMayEditConfig()) {
1543 masterConfig
.blackbox_device
= read8();
1544 masterConfig
.blackbox_rate_num
= read8();
1545 masterConfig
.blackbox_rate_denom
= read8();
1551 case MSP_SET_TRANSPONDER_CONFIG
:
1552 if (currentPort
->dataSize
!= sizeof(masterConfig
.transponderData
)) {
1557 for (i
= 0; i
< sizeof(masterConfig
.transponderData
); i
++) {
1558 masterConfig
.transponderData
[i
] = read8();
1561 transponderUpdateData(masterConfig
.transponderData
);
1566 case MSP_DATAFLASH_ERASE
:
1567 flashfsEraseCompletely();
1572 case MSP_SET_RAW_GPS
:
1574 ENABLE_STATE(GPS_FIX
);
1576 DISABLE_STATE(GPS_FIX
);
1578 GPS_numSat
= read8();
1579 GPS_coord
[LAT
] = read32();
1580 GPS_coord
[LON
] = read32();
1581 GPS_altitude
= read16();
1582 GPS_speed
= read16();
1583 GPS_update
|= 2; // New data signalisation to GPS functions // FIXME Magic Numbers
1586 wp_no
= read8(); //get the wp number
1589 alt
= read32(); // to set altitude (cm)
1590 read16(); // future: to set heading (deg)
1591 read16(); // future: to set time to stay (ms)
1592 read8(); // future: to set nav flag
1594 GPS_home
[LAT
] = lat
;
1595 GPS_home
[LON
] = lon
;
1596 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
1597 ENABLE_STATE(GPS_FIX_HOME
);
1599 AltHold
= alt
; // temporary implementation to test feature with apps
1600 } 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
1601 GPS_hold
[LAT
] = lat
;
1602 GPS_hold
[LON
] = lon
;
1604 AltHold
= alt
; // temporary implementation to test feature with apps
1605 nav_mode
= NAV_MODE_WP
;
1606 GPS_set_next_wp(&GPS_hold
[LAT
], &GPS_hold
[LON
]);
1610 case MSP_SET_FEATURE
:
1612 featureSet(read32()); // features bitmap
1615 case MSP_SET_BOARD_ALIGNMENT
:
1616 masterConfig
.boardAlignment
.rollDegrees
= read16();
1617 masterConfig
.boardAlignment
.pitchDegrees
= read16();
1618 masterConfig
.boardAlignment
.yawDegrees
= read16();
1621 case MSP_SET_VOLTAGE_METER_CONFIG
:
1622 masterConfig
.batteryConfig
.vbatscale
= read8(); // actual vbatscale as intended
1623 masterConfig
.batteryConfig
.vbatmincellvoltage
= read8(); // vbatlevel_warn1 in MWC2.3 GUI
1624 masterConfig
.batteryConfig
.vbatmaxcellvoltage
= read8(); // vbatlevel_warn2 in MWC2.3 GUI
1625 masterConfig
.batteryConfig
.vbatwarningcellvoltage
= read8(); // vbatlevel when buzzer starts to alert
1628 case MSP_SET_CURRENT_METER_CONFIG
:
1629 masterConfig
.batteryConfig
.currentMeterScale
= read16();
1630 masterConfig
.batteryConfig
.currentMeterOffset
= read16();
1631 masterConfig
.batteryConfig
.currentMeterType
= read8();
1632 masterConfig
.batteryConfig
.batteryCapacity
= read16();
1635 #ifndef USE_QUAD_MIXER_ONLY
1637 masterConfig
.mixerMode
= read8();
1641 case MSP_SET_RX_CONFIG
:
1642 masterConfig
.rxConfig
.serialrx_provider
= read8();
1643 masterConfig
.rxConfig
.maxcheck
= read16();
1644 masterConfig
.rxConfig
.midrc
= read16();
1645 masterConfig
.rxConfig
.mincheck
= read16();
1646 masterConfig
.rxConfig
.spektrum_sat_bind
= read8();
1647 if (currentPort
->dataSize
> 8) {
1648 masterConfig
.rxConfig
.rx_min_usec
= read16();
1649 masterConfig
.rxConfig
.rx_max_usec
= read16();
1653 case MSP_SET_FAILSAFE_CONFIG
:
1654 masterConfig
.failsafeConfig
.failsafe_delay
= read8();
1655 masterConfig
.failsafeConfig
.failsafe_off_delay
= read8();
1656 masterConfig
.failsafeConfig
.failsafe_throttle
= read16();
1657 masterConfig
.failsafeConfig
.failsafe_kill_switch
= read8();
1658 masterConfig
.failsafeConfig
.failsafe_throttle_low_delay
= read16();
1659 masterConfig
.failsafeConfig
.failsafe_procedure
= read8();
1662 case MSP_SET_RXFAIL_CONFIG
:
1664 if (i
< MAX_SUPPORTED_RC_CHANNEL_COUNT
) {
1665 masterConfig
.rxConfig
.failsafe_channel_configurations
[i
].mode
= read8();
1666 masterConfig
.rxConfig
.failsafe_channel_configurations
[i
].step
= CHANNEL_VALUE_TO_RXFAIL_STEP(read16());
1672 case MSP_SET_RSSI_CONFIG
:
1673 masterConfig
.rxConfig
.rssi_channel
= read8();
1676 case MSP_SET_RX_MAP
:
1677 for (i
= 0; i
< MAX_MAPPABLE_RX_INPUTS
; i
++) {
1678 masterConfig
.rxConfig
.rcmap
[i
] = read8();
1682 case MSP_SET_BF_CONFIG
:
1684 #ifdef USE_QUAD_MIXER_ONLY
1685 read8(); // mixerMode ignored
1687 masterConfig
.mixerMode
= read8(); // mixerMode
1691 featureSet(read32()); // features bitmap
1693 masterConfig
.rxConfig
.serialrx_provider
= read8(); // serialrx_type
1695 masterConfig
.boardAlignment
.rollDegrees
= read16(); // board_align_roll
1696 masterConfig
.boardAlignment
.pitchDegrees
= read16(); // board_align_pitch
1697 masterConfig
.boardAlignment
.yawDegrees
= read16(); // board_align_yaw
1699 masterConfig
.batteryConfig
.currentMeterScale
= read16();
1700 masterConfig
.batteryConfig
.currentMeterOffset
= read16();
1703 case MSP_SET_CF_SERIAL_CONFIG
:
1705 uint8_t portConfigSize
= sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4);
1707 if (currentPort
->dataSize
% portConfigSize
!= 0) {
1712 uint8_t remainingPortsInPacket
= currentPort
->dataSize
/ portConfigSize
;
1714 while (remainingPortsInPacket
--) {
1715 uint8_t identifier
= read8();
1717 serialPortConfig_t
*portConfig
= serialFindPortConfiguration(identifier
);
1723 portConfig
->identifier
= identifier
;
1724 portConfig
->functionMask
= read16();
1725 portConfig
->msp_baudrateIndex
= read8();
1726 portConfig
->gps_baudrateIndex
= read8();
1727 portConfig
->telemetry_baudrateIndex
= read8();
1728 portConfig
->blackbox_baudrateIndex
= read8();
1734 case MSP_SET_LED_COLORS
:
1735 for (i
= 0; i
< CONFIGURABLE_COLOR_COUNT
; i
++) {
1736 hsvColor_t
*color
= &masterConfig
.colors
[i
];
1737 color
->h
= read16();
1743 case MSP_SET_LED_STRIP_CONFIG
:
1746 if (i
>= MAX_LED_STRIP_LENGTH
|| currentPort
->dataSize
!= (1 + 7)) {
1750 ledConfig_t
*ledConfig
= &masterConfig
.ledConfigs
[i
];
1752 // currently we're storing directions and functions in a uint16 (flags)
1753 // the msp uses 2 x uint16_t to cater for future expansion
1755 ledConfig
->flags
= (mask
<< LED_DIRECTION_BIT_OFFSET
) & LED_DIRECTION_MASK
;
1758 ledConfig
->flags
|= (mask
<< LED_FUNCTION_BIT_OFFSET
) & LED_FUNCTION_MASK
;
1761 ledConfig
->xy
= CALCULATE_LED_X(mask
);
1764 ledConfig
->xy
|= CALCULATE_LED_Y(mask
);
1766 ledConfig
->color
= read8();
1768 reevalulateLedConfig();
1773 isRebootScheduled
= true;
1776 #ifdef USE_SERIAL_1WIRE
1778 // get channel number
1780 // we do not give any data back, assume channel number is transmitted OK
1782 // 0xFF -> preinitialize the Passthrough
1783 // switch all motor lines HI
1784 usb1WireInitialize();
1785 // reply the count of ESC found
1787 serialize8(escCount
);
1789 // and come back right afterwards
1790 // rem: App: Wait at least appx. 500 ms for BLHeli to jump into
1791 // bootloader mode before try to connect any ESC
1796 // Check for channel number 0..ESC_COUNT-1
1798 // because we do not come back after calling usb1WirePassthrough
1799 // proceed with a success reply first
1802 // flush the transmit buffer
1803 bufWriterFlush(writer
);
1804 // wait for all data to send
1805 waitForSerialPortToFinishTransmitting(currentPort
->port
);
1806 // Start to activate here
1807 // motor 1 => index 0
1809 // search currentPort portIndex
1810 /* next lines seems to be unnecessary, because the currentPort always point to the same mspPorts[portIndex]
1812 for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
1813 if (currentPort == &mspPorts[portIndex]) {
1818 mspReleasePortIfAllocated(mspSerialPort
); // CloseSerialPort also marks currentPort as UNUSED_PORT
1819 usb1WirePassthrough(i
);
1820 // Wait a bit more to let App read the 0 byte and switch baudrate
1821 // 2ms will most likely do the job, but give some grace time
1823 // rebuild/refill currentPort structure, does openSerialPort if marked UNUSED_PORT - used ports are skiped
1824 mspAllocateSerialPorts(&masterConfig
.serialConfig
);
1825 /* restore currentPort and mspSerialPort
1826 setCurrentPort(&mspPorts[portIndex]); // not needed same index will be restored
1828 // former used MSP uart is active again
1829 // restore MSP_SET_1WIRE as current command for correct headSerialReply(0)
1830 currentPort
->cmdMSP
= MSP_SET_1WIRE
;
1832 // ESC channel higher than max. allowed
1833 // rem: BLHeliSuite will not support more than 8
1836 // proceed as usual with MSP commands
1837 // and wait to switch to next channel
1838 // rem: App needs to call MSP_BOOT to deinitialize Passthrough
1843 // we do not know how to handle the (valid) message, indicate error MSP $M!
1850 STATIC_UNIT_TESTED
void mspProcessReceivedCommand() {
1851 if (!(processOutCommand(currentPort
->cmdMSP
) || processInCommand())) {
1855 currentPort
->c_state
= IDLE
;
1858 static bool mspProcessReceivedData(uint8_t c
)
1860 if (currentPort
->c_state
== IDLE
) {
1862 currentPort
->c_state
= HEADER_START
;
1866 } else if (currentPort
->c_state
== HEADER_START
) {
1867 currentPort
->c_state
= (c
== 'M') ? HEADER_M
: IDLE
;
1868 } else if (currentPort
->c_state
== HEADER_M
) {
1869 currentPort
->c_state
= (c
== '<') ? HEADER_ARROW
: IDLE
;
1870 } else if (currentPort
->c_state
== HEADER_ARROW
) {
1871 if (c
> MSP_PORT_INBUF_SIZE
) {
1872 currentPort
->c_state
= IDLE
;
1875 currentPort
->dataSize
= c
;
1876 currentPort
->offset
= 0;
1877 currentPort
->checksum
= 0;
1878 currentPort
->indRX
= 0;
1879 currentPort
->checksum
^= c
;
1880 currentPort
->c_state
= HEADER_SIZE
;
1882 } else if (currentPort
->c_state
== HEADER_SIZE
) {
1883 currentPort
->cmdMSP
= c
;
1884 currentPort
->checksum
^= c
;
1885 currentPort
->c_state
= HEADER_CMD
;
1886 } else if (currentPort
->c_state
== HEADER_CMD
&& currentPort
->offset
< currentPort
->dataSize
) {
1887 currentPort
->checksum
^= c
;
1888 currentPort
->inBuf
[currentPort
->offset
++] = c
;
1889 } else if (currentPort
->c_state
== HEADER_CMD
&& currentPort
->offset
>= currentPort
->dataSize
) {
1890 if (currentPort
->checksum
== c
) {
1891 currentPort
->c_state
= COMMAND_RECEIVED
;
1893 currentPort
->c_state
= IDLE
;
1899 STATIC_UNIT_TESTED
void setCurrentPort(mspPort_t
*port
)
1902 mspSerialPort
= currentPort
->port
;
1905 void mspProcess(void)
1908 mspPort_t
*candidatePort
;
1910 for (portIndex
= 0; portIndex
< MAX_MSP_PORT_COUNT
; portIndex
++) {
1911 candidatePort
= &mspPorts
[portIndex
];
1912 if (!candidatePort
->port
) {
1916 setCurrentPort(candidatePort
);
1917 // Big enough to fit a MSP_STATUS in one write.
1918 uint8_t buf
[sizeof(bufWriter_t
) + 20];
1919 writer
= bufWriterInit(buf
, sizeof(buf
),
1920 (bufWrite_t
)serialWriteBufShim
, currentPort
->port
);
1922 while (serialRxBytesWaiting(mspSerialPort
)) {
1924 uint8_t c
= serialRead(mspSerialPort
);
1925 bool consumed
= mspProcessReceivedData(c
);
1927 if (!consumed
&& !ARMING_FLAG(ARMED
)) {
1928 evaluateOtherData(mspSerialPort
, c
);
1931 if (currentPort
->c_state
== COMMAND_RECEIVED
) {
1932 mspProcessReceivedCommand();
1933 break; // process one command at a time so as not to block.
1937 bufWriterFlush(writer
);
1939 if (isRebootScheduled
) {
1940 waitForSerialPortToFinishTransmitting(candidatePort
->port
);
1942 handleOneshotFeatureChangeOnRestart();