serial_msp.c 63.7 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
/*
 * This file is part of Cleanflight.
 *
 * Cleanflight is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Cleanflight is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdbool.h>
#include <stdint.h>
20
#include <stdlib.h>
21
22
23
24
#include <string.h>
#include <math.h>

#include "build_config.h"
Dominic Clifton's avatar
Dominic Clifton committed
25
#include "debug.h"
26
27
28
29

#include "platform.h"

#include "common/axis.h"
30
31
#include "common/color.h"
#include "common/maths.h"
32
33

#include "drivers/system.h"
34
35

#include "drivers/sensor.h"
36
#include "drivers/accgyro.h"
37
38
#include "drivers/compass.h"

39
40
41
42
43
44
45
46
#include "drivers/serial.h"
#include "drivers/bus_i2c.h"
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"

#include "rx/rx.h"
#include "rx/msp.h"
47

48
49
50
51
52
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "io/gps.h"
#include "io/gimbal.h"
#include "io/serial.h"
53
#include "io/ledstrip.h"
54
#include "io/flashfs.h"
55

56
#include "telemetry/telemetry.h"
57

58
59
60
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"
#include "sensors/battery.h"
61
#include "sensors/sonar.h"
62
63
64
65
66
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"

67
#include "flight/mixer.h"
68
69
#include "flight/pid.h"
#include "flight/imu.h"
70
71
72
73
#include "flight/failsafe.h"
#include "flight/navigation.h"
#include "flight/altitudehold.h"

74
75
#include "mw.h"

76
77
78
79
80
81
#include "config/runtime_config.h"
#include "config/config.h"
#include "config/config_profile.h"
#include "config/config_master.h"

#include "version.h"
82
83
84
#ifdef NAZE
#include "hardware_revision.h"
#endif
85
86
87

#include "serial_msp.h"

88
static serialPort_t *mspSerialPort;
89
90
91
92

extern uint16_t cycleTime; // FIXME dependency on mw.c
extern uint16_t rssi; // FIXME dependency on mw.c

93
94
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse);

95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
/**
 * MSP Guidelines, emphasis is used to clarify.
 *
 * Each FlightController (FC, Server) MUST change the API version when any MSP command is added, deleted, or changed.
 *
 * If you fork the FC source code and release your own version, you MUST change the Flight Controller Identifier.
 *
 * NEVER release a modified copy of this code that shares the same Flight controller IDENT and API version
 * if the API doesn't match EXACTLY.
 *
 * Consumers of the API (API clients) SHOULD first attempt to get a response from the MSP_API_VERSION command.
 * If no response is obtained then client MAY try the legacy MSP_IDENT command.
 *
 * API consumers should ALWAYS handle communication failures gracefully and attempt to continue
 * without the information if possible.  Clients MAY log/display a suitable message.
 *
111
 * API clients should NOT attempt any communication if they can't handle the returned API MAJOR VERSION.
112
113
114
115
116
 *
 * API clients SHOULD attempt communication if the API MINOR VERSION has increased from the time
 * the API client was written and handle command failures gracefully.  Clients MAY disable
 * functionality that depends on the commands while still leaving other functionality intact.
 * Clients SHOULD operate in READ-ONLY mode and SHOULD present a warning to the user to state
117
 * that the newer API version may cause problems before using API commands that change FC state.
118
119
120
121
 *
 * It is for this reason that each MSP command should be specific as possible, such that changes
 * to commands break as little functionality as possible.
 *
122
 * API client authors MAY use a compatibility matrix/table when determining if they can support
123
124
125
126
127
128
129
130
131
132
133
 * a given command from a given flight controller at a given api version level.
 *
 * Developers MUST NOT create new MSP commands that do more than one thing.
 *
 * Failure to follow these guidelines will likely invoke the wrath of developers trying to write tools
 * that use the API and the users of those tools.
 */

#define MSP_PROTOCOL_VERSION                0

#define API_VERSION_MAJOR                   1 // increment when major changes are made
134
#define API_VERSION_MINOR                   10 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
135
136
137
138
139
140
141
142

#define API_VERSION_LENGTH                  2

#define MULTIWII_IDENTIFIER "MWII";
#define CLEANFLIGHT_IDENTIFIER "CLFL"
#define BASEFLIGHT_IDENTIFIER "BAFL";

#define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4
143
static const char * const flightControllerIdentifier = CLEANFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
144
145
146
147

#define FLIGHT_CONTROLLER_VERSION_LENGTH    3
#define FLIGHT_CONTROLLER_VERSION_MASK      0xFFF

148
static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
149
150
151
152
#define BOARD_IDENTIFIER_LENGTH             4 // 4 UPPER CASE alpha numeric characters that identify the board being used.
#define BOARD_HARDWARE_REVISION_LENGTH      2

// These are baseflight specific flags but they are useless now since MW 2.3 uses the upper 4 bits for the navigation version.
153
#define CAP_PLATFORM_32BIT          ((uint32_t)1 << 31)
154
#define CAP_BASEFLIGHT_CONFIG       ((uint32_t)1 << 30)
155
156
157
158
159
160
161

// MW 2.3 stores NAVI_VERSION in the top 4 bits of the capability mask.
#define CAP_NAVI_VERSION_BIT_4_MSB  ((uint32_t)1 << 31)
#define CAP_NAVI_VERSION_BIT_3      ((uint32_t)1 << 30)
#define CAP_NAVI_VERSION_BIT_2      ((uint32_t)1 << 29)
#define CAP_NAVI_VERSION_BIT_1_LSB  ((uint32_t)1 << 28)

162
163
#define CAP_DYNBALANCE              ((uint32_t)1 << 2)
#define CAP_FLAPS                   ((uint32_t)1 << 3)
164
165
166
167
#define CAP_NAVCAP                  ((uint32_t)1 << 4)
#define CAP_EXTAUX                  ((uint32_t)1 << 5)

#define MSP_API_VERSION                 1    //out message
168
169
170
171
#define MSP_FC_VARIANT                  2    //out message
#define MSP_FC_VERSION                  3    //out message
#define MSP_BOARD_INFO                  4    //out message
#define MSP_BUILD_INFO                  5    //out message
172
173

//
174
// MSP commands for Cleanflight original features
175
//
176
177
#define MSP_CHANNEL_FORWARDING          32    //out message         Returns channel forwarding settings
#define MSP_SET_CHANNEL_FORWARDING      33    //in message          Channel forwarding settings
178

179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
#define MSP_MODE_RANGES                 34    //out message         Returns all mode ranges
#define MSP_SET_MODE_RANGE              35    //in message          Sets a single mode range

#define MSP_FEATURE                     36
#define MSP_SET_FEATURE                 37

#define MSP_BOARD_ALIGNMENT             38
#define MSP_SET_BOARD_ALIGNMENT         39

#define MSP_CURRENT_METER_CONFIG        40
#define MSP_SET_CURRENT_METER_CONFIG    41

#define MSP_MIXER                       42
#define MSP_SET_MIXER                   43

#define MSP_RX_CONFIG                   44
#define MSP_SET_RX_CONFIG               45

#define MSP_LED_COLORS                  46
#define MSP_SET_LED_COLORS              47

#define MSP_LED_STRIP_CONFIG            48
#define MSP_SET_LED_STRIP_CONFIG        49

#define MSP_RSSI_CONFIG                 50
#define MSP_SET_RSSI_CONFIG             51

206
207
208
#define MSP_ADJUSTMENT_RANGES           52
#define MSP_SET_ADJUSTMENT_RANGE        53

209
210
211
212
// private - only to be used by the configurator, the commands are likely to change
#define MSP_CF_SERIAL_CONFIG            54
#define MSP_SET_CF_SERIAL_CONFIG        55

213
214
215
#define MSP_VOLTAGE_METER_CONFIG        56
#define MSP_SET_VOLTAGE_METER_CONFIG    57

216
217
#define MSP_SONAR_ALTITUDE              58 //out message get sonar altitude [cm]

218
219
#define MSP_PID_CONTROLLER              59
#define MSP_SET_PID_CONTROLLER          60
220

Dominic Clifton's avatar
Dominic Clifton committed
221
222
#define MSP_ARMING_CONFIG               61 //out message         Returns auto_disarm_delay and disarm_kill_switch parameters
#define MSP_SET_ARMING_CONFIG           62 //in message          Sets auto_disarm_delay and disarm_kill_switch parameters
223

Dominic Clifton's avatar
Dominic Clifton committed
224
225
226
227
228
229
#define MSP_DATAFLASH_SUMMARY           70 //out message - get description of dataflash chip
#define MSP_DATAFLASH_READ              71 //out message - get content of dataflash chip
#define MSP_DATAFLASH_ERASE             72 //in message - erase dataflash chip

#define MSP_LOOP_TIME                   73 //out message         Returns FC cycle time i.e looptime parameter
#define MSP_SET_LOOP_TIME               74 //in message          Sets FC cycle time i.e looptime parameter
230

231
232
#define MSP_FAILSAFE_CONFIG             75 //out message         Returns FC Fail-Safe settings
#define MSP_SET_FAILSAFE_CONFIG         76 //in message          Sets FC Fail-Safe settings
233
//
234
// Baseflight MSP commands (if enabled they exist in Cleanflight)
235
//
236
237
238
#define MSP_RX_MAP                      64 //out message get channel map (also returns number of channels total)
#define MSP_SET_RX_MAP                  65 //in message set rx map, numchannels to set comes from MSP_RX_MAP

239
// FIXME - Provided for backwards compatibility with configurator code until configurator is updated.
240
// DEPRECATED - DO NOT USE "MSP_BF_CONFIG" and MSP_SET_BF_CONFIG.  In Cleanflight, isolated commands already exist and should be used instead.
Dominic Clifton's avatar
Dominic Clifton committed
241
242
#define MSP_BF_CONFIG                   66 //out message baseflight-specific settings that aren't covered elsewhere
#define MSP_SET_BF_CONFIG               67 //in message baseflight-specific settings save
243
244
245

#define MSP_REBOOT                      68 //in message reboot settings

246
247
// DEPRECATED - Use MSP_BUILD_INFO instead
#define MSP_BF_BUILD_INFO               69 //out message build date as well as some space for future expansion
248
249

//
250
// Multwii original MSP commands
251
252
253
//

// DEPRECATED - See MSP_API_VERSION and MSP_MIXER
254
#define MSP_IDENT                100    //out message         mixerMode + multiwii version + protocol version + capability variable
255
256


257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
#define MSP_STATUS               101    //out message         cycletime & errors_count & sensor present & box activation & current setting number
#define MSP_RAW_IMU              102    //out message         9 DOF
#define MSP_SERVO                103    //out message         8 servos
#define MSP_MOTOR                104    //out message         8 motors
#define MSP_RC                   105    //out message         8 rc chan and more
#define MSP_RAW_GPS              106    //out message         fix, numsat, lat, lon, alt, speed, ground course
#define MSP_COMP_GPS             107    //out message         distance home, direction home
#define MSP_ATTITUDE             108    //out message         2 angles 1 heading
#define MSP_ALTITUDE             109    //out message         altitude, variometer
#define MSP_ANALOG               110    //out message         vbat, powermetersum, rssi if available on RX
#define MSP_RC_TUNING            111    //out message         rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
#define MSP_PID                  112    //out message         P I D coeff (9 are used currently)
#define MSP_BOX                  113    //out message         BOX setup (number is dependant of your setup)
#define MSP_MISC                 114    //out message         powermeter trig
#define MSP_MOTOR_PINS           115    //out message         which pins are in use for motors & servos, for GUI
#define MSP_BOXNAMES             116    //out message         the aux switch names
#define MSP_PIDNAMES             117    //out message         the PID names
#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
#define MSP_BOXIDS               119    //out message         get the permanent IDs associated to BOXes
#define MSP_SERVO_CONF           120    //out message         Servo settings
#define MSP_NAV_STATUS           121    //out message         Returns navigation status
#define MSP_NAV_CONFIG           122    //out message         Returns navigation parameters

#define MSP_SET_RAW_RC           200    //in message          8 rc chan
#define MSP_SET_RAW_GPS          201    //in message          fix, numsat, lat, lon, alt, speed
#define MSP_SET_PID              202    //in message          P I D coeff (9 are used currently)
#define MSP_SET_BOX              203    //in message          BOX setup (number is dependant of your setup)
borisbstyle's avatar
borisbstyle committed
284
#define MSP_SET_RC_TUNING        204    //in message          rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
#define MSP_ACC_CALIBRATION      205    //in message          no param
#define MSP_MAG_CALIBRATION      206    //in message          no param
#define MSP_SET_MISC             207    //in message          powermeter trig + 8 free for future use
#define MSP_RESET_CONF           208    //in message          no param
#define MSP_SET_WP               209    //in message          sets a given WP (WP#,lat, lon, alt, flags)
#define MSP_SELECT_SETTING       210    //in message          Select Setting Number (0-2)
#define MSP_SET_HEAD             211    //in message          define a new heading hold direction
#define MSP_SET_SERVO_CONF       212    //in message          Servo settings
#define MSP_SET_MOTOR            214    //in message          PropBalance function
#define MSP_SET_NAV_CONFIG       215    //in message          Sets nav config parameters - write to the eeprom

// #define MSP_BIND                 240    //in message          no param

#define MSP_EEPROM_WRITE         250    //in message          no param

#define MSP_DEBUGMSG             253    //out message         debug string buffer
#define MSP_DEBUG                254    //out message         debug1,debug2,debug3,debug4

// Additional commands that are not compatible with MultiWii
#define MSP_UID                  160    //out message         Unique device ID
#define MSP_ACC_TRIM             240    //out message         get acc angle trim values
#define MSP_SET_ACC_TRIM         239    //in message          set acc angle trim values
#define MSP_GPSSVINFO            164    //out message         get Signal Strength (only U-Blox)

#define INBUF_SIZE 64

311
312
#define SERVO_CHUNK_SIZE 7

313
typedef struct box_e {
314
    const uint8_t boxId;         // see boxId_e
315
316
    const char *boxName;            // GUI-readable box name
    const uint8_t permanentId;      //
317
318
} box_t;

319
// FIXME remove ;'s
320
static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
    { BOXARM, "ARM;", 0 },
    { BOXANGLE, "ANGLE;", 1 },
    { BOXHORIZON, "HORIZON;", 2 },
    { BOXBARO, "BARO;", 3 },
    //{ BOXVARIO, "VARIO;", 4 },
    { BOXMAG, "MAG;", 5 },
    { BOXHEADFREE, "HEADFREE;", 6 },
    { BOXHEADADJ, "HEADADJ;", 7 },
    { BOXCAMSTAB, "CAMSTAB;", 8 },
    { BOXCAMTRIG, "CAMTRIG;", 9 },
    { BOXGPSHOME, "GPS HOME;", 10 },
    { BOXGPSHOLD, "GPS HOLD;", 11 },
    { BOXPASSTHRU, "PASSTHRU;", 12 },
    { BOXBEEPERON, "BEEPER;", 13 },
    { BOXLEDMAX, "LEDMAX;", 14 },
    { BOXLEDLOW, "LEDLOW;", 15 },
    { BOXLLIGHTS, "LLIGHTS;", 16 },
    { BOXCALIB, "CALIB;", 17 },
    { BOXGOV, "GOVERNOR;", 18 },
    { BOXOSD, "OSD SW;", 19 },
    { BOXTELEMETRY, "TELEMETRY;", 20 },
    { BOXAUTOTUNE, "AUTOTUNE;", 21 },
343
    { BOXSONAR, "SONAR;", 22 },
344
345
346
347
    { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};

// this is calculated at startup based on enabled features.
348
static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT];
349
// this is the number of filled indexes in above array
350
static uint8_t activeBoxIdCount = 0;
351
352
353
// from mixer.c
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];

354
355
356
// cause reboot after MSP processing complete
static bool isRebootScheduled = false;

357
358
359
360
361
362
363
364
365
366
367
368
static const char pidnames[] =
    "ROLL;"
    "PITCH;"
    "YAW;"
    "ALT;"
    "Pos;"
    "PosR;"
    "NavR;"
    "LEVEL;"
    "MAG;"
    "VEL;";

369
370
371
372
373
374
375
typedef enum {
    IDLE,
    HEADER_START,
    HEADER_M,
    HEADER_ARROW,
    HEADER_SIZE,
    HEADER_CMD,
376
    COMMAND_RECEIVED
377
378
} mspState_e;

379
380
381
382
383
384
typedef enum {
    UNUSED_PORT = 0,
    FOR_GENERAL_MSP,
    FOR_TELEMETRY
} mspPortUsage_e;

385
386
387
388
389
390
391
392
393
typedef struct mspPort_s {
    serialPort_t *port;
    uint8_t offset;
    uint8_t dataSize;
    uint8_t checksum;
    uint8_t indRX;
    uint8_t inBuf[INBUF_SIZE];
    mspState_e c_state;
    uint8_t cmdMSP;
394
    mspPortUsage_e mspPortUsage;
395
396
} mspPort_t;

397
static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
398
399

static mspPort_t *currentPort;
400

401
static void serialize8(uint8_t a)
402
{
403
404
    serialWrite(mspSerialPort, a);
    currentPort->checksum ^= a;
405
406
}

407
static void serialize16(uint16_t a)
408
{
409
410
    serialize8((uint8_t)(a >> 0));
    serialize8((uint8_t)(a >> 8));
411
412
}

413
static void serialize32(uint32_t a)
414
{
415
416
    serialize16((uint16_t)(a >> 0));
    serialize16((uint16_t)(a >> 16));
417
418
}

419
static uint8_t read8(void)
420
{
421
    return currentPort->inBuf[currentPort->indRX++] & 0xff;
422
423
}

424
static uint16_t read16(void)
425
426
427
428
429
430
{
    uint16_t t = read8();
    t += (uint16_t)read8() << 8;
    return t;
}

431
static uint32_t read32(void)
432
433
434
435
436
437
{
    uint32_t t = read16();
    t += (uint32_t)read16() << 16;
    return t;
}

438
static void headSerialResponse(uint8_t err, uint8_t responseBodySize)
439
440
441
442
{
    serialize8('$');
    serialize8('M');
    serialize8(err ? '!' : '>');
443
    currentPort->checksum = 0;               // start calculating a new checksum
444
    serialize8(responseBodySize);
445
    serialize8(currentPort->cmdMSP);
446
447
}

448
static void headSerialReply(uint8_t responseBodySize)
449
{
450
    headSerialResponse(0, responseBodySize);
451
452
}

453
static void headSerialError(uint8_t responseBodySize)
454
{
455
    headSerialResponse(1, responseBodySize);
456
457
}

458
static void tailSerialReply(void)
459
{
460
    serialize8(currentPort->checksum);
461
462
}

463
static void s_struct(uint8_t *cb, uint8_t siz)
464
465
466
467
468
469
{
    headSerialReply(siz);
    while (siz--)
        serialize8(*cb++);
}

470
static void serializeNames(const char *s)
471
472
473
474
475
476
{
    const char *c;
    for (c = s; *c; c++)
        serialize8(*c);
}

477
static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
478
479
480
481
482
{
    uint8_t boxIndex;
    const box_t *candidate;
    for (boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
        candidate = &boxes[boxIndex];
483
484
485
486
487
488
489
        if (candidate->boxId == activeBoxId) {
            return candidate;
        }
    }
    return NULL;
}

490
static const box_t *findBoxByPermenantId(uint8_t permenantId)
491
492
493
494
495
496
{
    uint8_t boxIndex;
    const box_t *candidate;
    for (boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
        candidate = &boxes[boxIndex];
        if (candidate->permanentId == permenantId) {
497
498
499
500
501
502
            return candidate;
        }
    }
    return NULL;
}

503
static void serializeBoxNamesReply(void)
504
{
505
    int i, activeBoxId, j, flag = 1, count = 0, len;
506
    const box_t *box;
507
508
509
510

reset:
    // in first run of the loop, we grab total size of junk to be sent
    // then come back and actually send it
511
    for (i = 0; i < activeBoxIdCount; i++) {
512
        activeBoxId = activeBoxIds[i];
513

514
        box = findBoxByActiveBoxId(activeBoxId);
515
516
517
518
519
        if (!box) {
            continue;
        }

        len = strlen(box->boxName);
520
521
522
523
        if (flag) {
            count += len;
        } else {
            for (j = 0; j < len; j++)
524
                serialize8(box->boxName[j]);
525
526
527
528
529
530
531
532
533
534
        }
    }

    if (flag) {
        headSerialReply(count);
        flag = 0;
        goto reset;
    }
}

535
static void serializeDataflashSummaryReply(void)
536
{
537
    headSerialReply(1 + 3 * 4);
538
#ifdef USE_FLASHFS
539
    const flashGeometry_t *geometry = flashfsGetGeometry();
540
    serialize8(flashfsIsReady() ? 1 : 0);
541
542
    serialize32(geometry->sectors);
    serialize32(geometry->totalSize);
543
    serialize32(flashfsGetOffset()); // Effectively the current number of bytes stored on the volume
544
#else
545
    serialize8(0);
546
    serialize32(0);
547
548
549
550
551
    serialize32(0);
    serialize32(0);
#endif
}

552
#ifdef USE_FLASHFS
553
static void serializeDataflashReadReply(uint32_t address, uint8_t size)
554
{
555
    uint8_t buffer[128];
556
    int bytesRead;
557

558
559
    if (size > sizeof(buffer)) {
        size = sizeof(buffer);
560
561
562
563
564
565
    }

    headSerialReply(4 + size);

    serialize32(address);

566
    // bytesRead will be lower than that requested if we reach end of volume
567
    bytesRead = flashfsReadAbs(address, buffer, size);
568

569
    for (int i = 0; i < bytesRead; i++) {
570
571
572
573
574
        serialize8(buffer[i]);
    }
}
#endif

575
576
577
578
579
580
581
582
static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort, mspPortUsage_e usage)
{
    memset(mspPortToReset, 0, sizeof(mspPort_t));

    mspPortToReset->port = serialPort;
    mspPortToReset->mspPortUsage = usage;
}

583
void mspAllocateSerialPorts(serialConfig_t *serialConfig)
584
{
585
    UNUSED(serialConfig);
586

587
    serialPort_t *serialPort;
588

589
590
591
592
593
    uint8_t portIndex = 0;

    serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP);

    while (portConfig && portIndex < MAX_MSP_PORT_COUNT) {
594
595
        mspPort_t *mspPort = &mspPorts[portIndex];
        if (mspPort->mspPortUsage != UNUSED_PORT) {
596
            portIndex++;
597
598
            continue;
        }
599

600
        serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
601
602
603
        if (serialPort) {
            resetMspPort(mspPort, serialPort, FOR_GENERAL_MSP);
            portIndex++;
604
        }
605

606
607
        portConfig = findNextSerialPortConfig(FUNCTION_MSP);
    }
608
609
}

610
611
612
613
void mspReleasePortIfAllocated(serialPort_t *serialPort)
{
    uint8_t portIndex;
    for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
614
        mspPort_t *candidateMspPort = &mspPorts[portIndex];
615
        if (candidateMspPort->port == serialPort) {
616
            closeSerialPort(serialPort);
617
618
619
620
621
            memset(candidateMspPort, 0, sizeof(mspPort_t));
        }
    }
}

622
623
void mspInit(serialConfig_t *serialConfig)
{
624
625
    BUILD_BUG_ON((SERVO_CHUNK_SIZE * MAX_SUPPORTED_SERVOS) > INBUF_SIZE);

626
    // calculate used boxes based on features and fill availableBoxes[] array
627
    memset(activeBoxIds, 0xFF, sizeof(activeBoxIds));
628

629
630
    activeBoxIdCount = 0;
    activeBoxIds[activeBoxIdCount++] = BOXARM;
631
632

    if (sensors(SENSOR_ACC)) {
633
634
        activeBoxIds[activeBoxIdCount++] = BOXANGLE;
        activeBoxIds[activeBoxIdCount++] = BOXHORIZON;
635
636
637
    }

    if (sensors(SENSOR_BARO)) {
638
        activeBoxIds[activeBoxIdCount++] = BOXBARO;
639
640
641
    }

    if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
642
643
644
        activeBoxIds[activeBoxIdCount++] = BOXMAG;
        activeBoxIds[activeBoxIdCount++] = BOXHEADFREE;
        activeBoxIds[activeBoxIdCount++] = BOXHEADADJ;
645
646
647
    }

    if (feature(FEATURE_SERVO_TILT))
648
        activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB;
649
650
651

#ifdef GPS
    if (feature(FEATURE_GPS)) {
652
653
        activeBoxIds[activeBoxIdCount++] = BOXGPSHOME;
        activeBoxIds[activeBoxIdCount++] = BOXGPSHOLD;
654
655
656
    }
#endif

657
    if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE)
658
        activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
659

660
    activeBoxIds[activeBoxIdCount++] = BOXBEEPERON;
661

Chris Penny's avatar
Chris Penny committed
662
663
664
665
666
#ifdef LED_STRIP
    if (feature(FEATURE_LED_STRIP)) {
        activeBoxIds[activeBoxIdCount++] = BOXLEDLOW;
    }
#endif
667

668
    if (feature(FEATURE_INFLIGHT_ACC_CAL))
669
        activeBoxIds[activeBoxIdCount++] = BOXCALIB;
670

671
    activeBoxIds[activeBoxIdCount++] = BOXOSD;
672

673
    if (feature(FEATURE_TELEMETRY) && masterConfig.telemetryConfig.telemetry_switch)
674
        activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY;
675

676
677
678
#ifdef AUTOTUNE
    activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE;
#endif
679

680
681
682
683
    if (feature(FEATURE_SONAR)){
        activeBoxIds[activeBoxIdCount++] = BOXSONAR;
    }

684
    memset(mspPorts, 0x00, sizeof(mspPorts));
685
    mspAllocateSerialPorts(serialConfig);
686
687
}

688
689
#define IS_ENABLED(mask) (mask == 0 ? 0 : 1)

690
static bool processOutCommand(uint8_t cmdMSP)
691
692
{
    uint32_t i, tmp, junk;
693

694
695
#ifdef GPS
    uint8_t wp_no;
696
    int32_t lat = 0, lon = 0;
697
698
699
#endif

    switch (cmdMSP) {
700
701
702
    case MSP_API_VERSION:
        headSerialReply(
            1 + // protocol version length
703
            API_VERSION_LENGTH
704
705
706
707
708
        );
        serialize8(MSP_PROTOCOL_VERSION);

        serialize8(API_VERSION_MAJOR);
        serialize8(API_VERSION_MINOR);
709
710
711
712
        break;

    case MSP_FC_VARIANT:
        headSerialReply(FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
713
714
715
716

        for (i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) {
            serialize8(flightControllerIdentifier[i]);
        }
717
718
719
720
        break;

    case MSP_FC_VERSION:
        headSerialReply(FLIGHT_CONTROLLER_VERSION_LENGTH);
721
722
723
724

        serialize8(FC_VERSION_MAJOR);
        serialize8(FC_VERSION_MINOR);
        serialize8(FC_VERSION_PATCH_LEVEL);
725
        break;
726

727
728
729
730
731
    case MSP_BOARD_INFO:
        headSerialReply(
            BOARD_IDENTIFIER_LENGTH +
            BOARD_HARDWARE_REVISION_LENGTH
        );
732
733
734
735
736
737
738
739
        for (i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) {
            serialize8(boardIdentifier[i]);
        }
#ifdef NAZE
        serialize16(hardwareRevision);
#else
        serialize16(0); // No other build targets currently have hardware revision detection.
#endif
740
741
742
743
744
745
746
747
        break;

    case MSP_BUILD_INFO:
        headSerialReply(
                BUILD_DATE_LENGTH +
                BUILD_TIME_LENGTH +
                GIT_SHORT_REVISION_LENGTH
        );
748
749
750
751
752
753
754
755
756
757
758
759
760
761

        for (i = 0; i < BUILD_DATE_LENGTH; i++) {
            serialize8(buildDate[i]);
        }
        for (i = 0; i < BUILD_TIME_LENGTH; i++) {
            serialize8(buildTime[i]);
        }

        for (i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) {
            serialize8(shortGitRevision[i]);
        }
        break;

    // DEPRECATED - Use MSP_API_VERSION
762
763
764
    case MSP_IDENT:
        headSerialReply(7);
        serialize8(MW_VERSION);
765
        serialize8(masterConfig.mixerMode);
766
767
        serialize8(MSP_PROTOCOL_VERSION);
        serialize32(CAP_DYNBALANCE | (masterConfig.airplaneConfig.flaps_speed ? CAP_FLAPS : 0)); // "capability"
768
        break;
769

770
771
772
    case MSP_STATUS:
        headSerialReply(11);
        serialize16(cycleTime);
773
#ifdef USE_I2C
774
        serialize16(i2cGetErrorCounter());
775
776
777
#else
        serialize16(0);
#endif
778
        serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
Dominic Clifton's avatar
Dominic Clifton committed
779
780
781
        // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
        // Requires new Multiwii protocol version to fix
        // It would be preferable to setting the enabled bits based on BOXINDEX.
782
        junk = 0;
783
784
785
786
787
        tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE |
            IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON |
            IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << BOXBARO |
            IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG |
            IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE |
788
789
790
            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ |
            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB |
            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMTRIG)) << BOXCAMTRIG |
791
792
793
            IS_ENABLED(FLIGHT_MODE(GPS_HOME_MODE)) << BOXGPSHOME |
            IS_ENABLED(FLIGHT_MODE(GPS_HOLD_MODE)) << BOXGPSHOLD |
            IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU |
794
795
            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON |
            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX)) << BOXLEDMAX |
Chris Penny's avatar
Chris Penny committed
796
            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW |
797
798
799
800
801
802
            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS |
            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB)) << BOXCALIB |
            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) << BOXAUTOTUNE |
803
            IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
804
            IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM;
805
806
        for (i = 0; i < activeBoxIdCount; i++) {
            int flag = (tmp & (1 << activeBoxIds[i]));
807
808
809
810
811
812
813
814
            if (flag)
                junk |= 1 << i;
        }
        serialize32(junk);
        serialize8(masterConfig.current_profile_index);
        break;
    case MSP_RAW_IMU:
        headSerialReply(18);
815
        // Hack due to choice of units for sensor data in multiwii
816
817
818
819
820
821
822
823
        if (acc_1G > 1024) {
            for (i = 0; i < 3; i++)
                serialize16(accSmooth[i] / 8);
        } else {
            for (i = 0; i < 3; i++)
                serialize16(accSmooth[i]);
        }
        for (i = 0; i < 3; i++)
824
            serialize16(gyroADC[i]);
825
826
827
        for (i = 0; i < 3; i++)
            serialize16(magADC[i]);
        break;
828
#ifdef USE_SERVOS
829
830
831
832
    case MSP_SERVO:
        s_struct((uint8_t *)&servo, 16);
        break;
    case MSP_SERVO_CONF:
833
        headSerialReply(MAX_SUPPORTED_SERVOS * 7);
834
        for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
835
836
837
838
            serialize16(currentProfile->servoConf[i].min);
            serialize16(currentProfile->servoConf[i].max);
            serialize16(currentProfile->servoConf[i].middle);
            serialize8(currentProfile->servoConf[i].rate);
839
840
841
        }
        break;
    case MSP_CHANNEL_FORWARDING:
842
        headSerialReply(MAX_SUPPORTED_SERVOS);
843
        for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
844
            serialize8(currentProfile->servoConf[i].forwardFromChannel);
845
846
        }
        break;
847
#endif
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
    case MSP_MOTOR:
        s_struct((uint8_t *)motor, 16);
        break;
    case MSP_RC:
        headSerialReply(2 * rxRuntimeConfig.channelCount);
        for (i = 0; i < rxRuntimeConfig.channelCount; i++)
            serialize16(rcData[i]);
        break;
    case MSP_ATTITUDE:
        headSerialReply(6);
        for (i = 0; i < 2; i++)
            serialize16(inclination.raw[i]);
        serialize16(heading);
        break;
    case MSP_ALTITUDE:
        headSerialReply(6);
864
865
866
867
868
#if defined(BARO) || defined(SONAR)
        serialize32(altitudeHoldGetEstimatedAltitude());
#else
        serialize32(0);
#endif
869
870
        serialize16(vario);
        break;
871
872
873
874
875
876
877
878
    case MSP_SONAR_ALTITUDE:
        headSerialReply(4);
#if defined(SONAR)
        serialize32(sonarGetLatestAltitude());
#else
        serialize32(0);
#endif
        break;
879
880
881
    case MSP_ANALOG:
        headSerialReply(7);
        serialize8((uint8_t)constrain(vbat, 0, 255));
882
        serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
883
884
        serialize16(rssi);
        if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) {
885
            serialize16((uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
886
        } else
887
            serialize16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
888
        break;
889
    case MSP_ARMING_CONFIG:
890
891
892
893
894
895
896
897
        headSerialReply(2);
        serialize8(masterConfig.auto_disarm_delay); 
        serialize8(masterConfig.disarm_kill_switch);
        break;
    case MSP_LOOP_TIME:
        headSerialReply(2);
        serialize16(masterConfig.looptime);
        break;
898
    case MSP_RC_TUNING:
899
        headSerialReply(11);
900
901
        serialize8(currentControlRateProfile->rcRate8);
        serialize8(currentControlRateProfile->rcExpo8);
902
903
904
        for (i = 0 ; i < 3; i++) {
            serialize8(currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
        }
905
906
907
        serialize8(currentControlRateProfile->dynThrPID);
        serialize8(currentControlRateProfile->thrMid8);
        serialize8(currentControlRateProfile->thrExpo8);
tricopterY's avatar
tricopterY committed
908
        serialize16(currentControlRateProfile->tpa_breakpoint);
borisbstyle's avatar
borisbstyle committed
909
        serialize8(currentControlRateProfile->rcYawExpo8);
910
911
912
        break;
    case MSP_PID:
        headSerialReply(3 * PID_ITEM_COUNT);
913
        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
914
            for (i = 0; i < 3; i++) {
915
916
917
                serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 250));
                serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 250));
                serialize8(constrain(lrintf(currentProfile->pidProfile.D_f[i] * 1000.0f), 0, 100));
918
919
920
            }
            for (i = 3; i < PID_ITEM_COUNT; i++) {
                if (i == PIDLEVEL) {
921
922
                    serialize8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 250));
                    serialize8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 250));
Ben Hitchcock's avatar
Ben Hitchcock committed
923
                    serialize8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity), 0, 250));
924
                } else {
925
926
927
                    serialize8(currentProfile->pidProfile.P8[i]);
                    serialize8(currentProfile->pidProfile.I8[i]);
                    serialize8(currentProfile->pidProfile.D8[i]);
928
929
930
931
                }
            }
        } else {
            for (i = 0; i < PID_ITEM_COUNT; i++) {
932
933
934
                serialize8(currentProfile->pidProfile.P8[i]);
                serialize8(currentProfile->pidProfile.I8[i]);
                serialize8(currentProfile->pidProfile.D8[i]);
935
936
937
938
939
940
941
            }
        }
        break;
    case MSP_PIDNAMES:
        headSerialReply(sizeof(pidnames) - 1);
        serializeNames(pidnames);
        break;
942
943
    case MSP_PID_CONTROLLER:
        headSerialReply(1);
944
        serialize8(currentProfile->pidProfile.pidController);
945
        break;
946
947
948
949
    case MSP_MODE_RANGES:
        headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT);
        for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
            modeActivationCondition_t *mac = &currentProfile->modeActivationConditions[i];
950
951
            const box_t *box = &boxes[mac->modeId];
            serialize8(box->permanentId);
952
            serialize8(mac->auxChannelIndex);
953
954
            serialize8(mac->range.startStep);
            serialize8(mac->range.endStep);
955
956
        }
        break;
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
    case MSP_ADJUSTMENT_RANGES:
        headSerialReply(MAX_ADJUSTMENT_RANGE_COUNT * (
                1 + // adjustment index/slot
                1 + // aux channel index
                1 + // start step
                1 + // end step
                1 + // adjustment function
                1   // aux switch channel index
        ));
        for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
            adjustmentRange_t *adjRange = &currentProfile->adjustmentRanges[i];
            serialize8(adjRange->adjustmentIndex);
            serialize8(adjRange->auxChannelIndex);
            serialize8(adjRange->range.startStep);
            serialize8(adjRange->range.endStep);
            serialize8(adjRange->adjustmentFunction);
            serialize8(adjRange->auxSwitchChannelIndex);
        }
        break;
976
977
978
979
    case MSP_BOXNAMES:
        serializeBoxNamesReply();
        break;
    case MSP_BOXIDS:
980
981
        headSerialReply(activeBoxIdCount);
        for (i = 0; i < activeBoxIdCount; i++) {
982
            const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]);
983
984
985
986
987
            if (!box) {
                continue;
            }
            serialize8(box->permanentId);
        }
988
989
        break;
    case MSP_MISC:
990
        headSerialReply(2 * 5 + 3 + 3 + 2 + 4);
991
992
        serialize16(masterConfig.rxConfig.midrc);

993
994
995
        serialize16(masterConfig.escAndServoConfig.minthrottle);
        serialize16(masterConfig.escAndServoConfig.maxthrottle);
        serialize16(masterConfig.escAndServoConfig.mincommand);
996

997
        serialize16(masterConfig.failsafeConfig.failsafe_throttle);
998

999
#ifdef GPS
1000
1001
1002
        serialize8(masterConfig.gpsConfig.provider); // gps_type
        serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
        serialize8(masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas
1003
1004
1005
1006
1007
#else
        serialize8(0); // gps_type
        serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
        serialize8(0); // gps_ubx_sbas
#endif
1008
1009
1010
1011
1012
1013
        serialize8(masterConfig.batteryConfig.multiwiiCurrentMeterOutput);
        serialize8(masterConfig.rxConfig.rssi_channel);
        serialize8(0);

        serialize16(currentProfile->mag_declination / 10);

1014
1015
1016
        serialize8(masterConfig.batteryConfig.vbatscale);
        serialize8(masterConfig.batteryConfig.vbatmincellvoltage);
        serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage);
1017
        serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage);
1018
1019
1020
1021
1022
1023
1024
        break;
    case MSP_MOTOR_PINS:
        headSerialReply(8);
        for (i = 0; i < 8; i++)
            serialize8(i + 1);
        break;
#ifdef GPS
1025
1026
    case MSP_RAW_GPS:
        headSerialReply(16);
1027
        serialize8(STATE(GPS_FIX));
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
        serialize8(GPS_numSat);
        serialize32(GPS_coord[LAT]);
        serialize32(GPS_coord[LON]);
        serialize16(GPS_altitude);
        serialize16(GPS_speed);
        serialize16(GPS_ground_course);
        break;
    case MSP_COMP_GPS:
        headSerialReply(5);
        serialize16(GPS_distanceToHome);
        serialize16(GPS_directionToHome);
        serialize8(GPS_update & 1);
        break;
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
    case MSP_WP:
        wp_no = read8();    // get the wp number
        headSerialReply(18);
        if (wp_no == 0) {
            lat = GPS_home[LAT];
            lon = GPS_home[LON];
        } else if (wp_no == 16) {
            lat = GPS_hold[LAT];
            lon = GPS_hold[LON];
        }
        serialize8(wp_no);
        serialize32(lat);
        serialize32(lon);
        serialize32(AltHold);           // altitude (cm) will come here -- temporary implementation to test feature with apps
        serialize16(0);                 // heading  will come here (deg)
        serialize16(0);                 // time to stay (ms) will come here
        serialize8(0);                  // nav flag will come here
        break;
1059
1060
1061
1062
1063
1064
1065
1066
    case MSP_GPSSVINFO:
        headSerialReply(1 + (GPS_numCh * 4));
        serialize8(GPS_numCh);
           for (i = 0; i < GPS_numCh; i++){
               serialize8(GPS_svinfo_chn[i]);
               serialize8(GPS_svinfo_svid[i]);
               serialize8(GPS_svinfo_quality[i]);
               serialize8(GPS_svinfo_cno[i]);
1067
           }
1068
        break;
1069
#endif
1070
    case MSP_DEBUG:
Dominic Clifton's avatar
Dominic Clifton committed
1071
1072
1073
1074
1075
1076
        headSerialReply(DEBUG16_VALUE_COUNT * sizeof(debug[0]));

        // output some useful QA statistics
        // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000);         // XX0YY [crystal clock : core clock]

        for (i = 0; i < DEBUG16_VALUE_COUNT; i++)
1077
1078
1079
1080
1081
1082
            serialize16(debug[i]);      // 4 variables are here for general monitoring purpose
        break;

    // Additional commands that are not compatible with MultiWii
    case MSP_ACC_TRIM:
        headSerialReply(4);
1083
1084
        serialize16(currentProfile->accelerometerTrims.values.pitch);
        serialize16(currentProfile->accelerometerTrims.values.roll);
1085
        break;
1086

1087
1088
1089
1090
1091
1092
    case MSP_UID:
        headSerialReply(12);
        serialize32(U_ID_0);
        serialize32(U_ID_1);
        serialize32(U_ID_2);
        break;
1093
1094
1095
1096
1097
1098
1099

    case MSP_FEATURE:
        headSerialReply(4);
        serialize32(featureMask());
        break;

    case MSP_BOARD_ALIGNMENT:
1100
        headSerialReply(6);
1101
1102
1103
1104
1105
        serialize16(masterConfig.boardAlignment.rollDegrees);
        serialize16(masterConfig.boardAlignment.pitchDegrees);
        serialize16(masterConfig.boardAlignment.yawDegrees);
        break;

1106
    case MSP_VOLTAGE_METER_CONFIG:
1107
        headSerialReply(4);
1108
1109
1110
1111
1112
1113
1114
1115
        serialize8(masterConfig.batteryConfig.vbatscale);
        serialize8(masterConfig.batteryConfig.vbatmincellvoltage);
        serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage);
        serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage);
        break;

    case MSP_CURRENT_METER_CONFIG:
        headSerialReply(7);
1116
1117
        serialize16(masterConfig.batteryConfig.currentMeterScale);