前言

前段时间购入了一块MATEK的传感器模块，上面包含了一个光流和一个激光传感器，该模块使用MSP通信协议与飞控进行串口通信，因此笔者尝试从INAV的开源飞控代码入手研究它的具体通信过程

协议介绍

官方文档地址：https://github.com/iNavFlight/inav/wiki/MSP-V2

协议简介

MSP is a request-response protocol. MSP defines the side sending the request as "MSP Master" role and the side generating a response as "MSP Slave" role.

A specific device may function as both "MSP Master" and "MSP Slave" or implement only one role, depending on requirements. Generally, a Groundstation software is an "MSP Master" and FC is an "MSP Slave", however in some use-cases a FC or any other device may be required to act as an "MSP Master" and "MSP Slave" concurrently.

简单浏览得知，飞控和外设之间通信是经典的主从关系

报文格式

根据官方给出的表格：和一般的通信报文一样，MSP的报文中也包含了固定的帧头以及末尾的校验和。第三到第六字节包含了一些与本次通信相关的信息，其中type根据官方文档一共有三种，分别是请求、应答和异常：

function包含了本次通信的功能信息，以传感器为例，传感器发出的MSP报文的function分为以下几类：

MSP2_SENSOR_RANGEFINDER     0x1F01                    //距离传感器
MSP2_SENSOR_OPTIC_FLOW      0x1F02                    //光流传感器
MSP2_SENSOR_GPS             0x1F03                    //GPS
MSP2_SENSOR_COMPASS         0x1F04                    //指南针
MSP2_SENSOR_BAROMETER       0x1F05                    //气压计
MSP2_SENSOR_AIRSPEED        0x1F06                    //空速计

程序框架

INAV飞控MSP处理程序的结构如下所示：

  • mspSerialProcess

    • mspSerialrocessOneport

      • serialRead
      • mspSerialProcessReceivedData
      • mspSerialProcessReceivedCommand

在fc_tasks.c中，串口的处理作为一个任务交给系统调度，其中进行MSP通信处理的是下面的函数，该函数的两个参数分别是非MSP报文的处理方式以及报文处理函数的函数指针（记住，后面要考）：

// Allow MSP processing even if in CLI mode
    mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand);

mspSerialProcess

void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn)
{
    for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
        mspPort_t * const mspPort = &mspPorts[portIndex];
        if (mspPort->port) {
            mspSerialProcessOnePort(mspPort, evaluateNonMspData, mspProcessCommandFn);
        }
    }
}

该函数按次序将所有可用的msp端口进行处理，可以观察到该函数将上一层的参数都传了进去，在此基础上增加了所用端口的信息。

mspSerialrocessOneport

void mspSerialProcessOnePort(mspPort_t * const mspPort, mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn)
{
    mspPostProcessFnPtr mspPostProcessFn = NULL;

    if (serialRxBytesWaiting(mspPort->port)) {
        // There are bytes incoming - abort pending request
        mspPort->lastActivityMs = millis();
        mspPort->pendingRequest = MSP_PENDING_NONE;

        // Process incoming bytes
        while (serialRxBytesWaiting(mspPort->port)) {
            const uint8_t c = serialRead(mspPort->port);
            const bool consumed = mspSerialProcessReceivedData(mspPort, c);

            if (!consumed && evaluateNonMspData == MSP_EVALUATE_NON_MSP_DATA) {
                mspEvaluateNonMspData(mspPort, c);
            }

            if (mspPort->c_state == MSP_COMMAND_RECEIVED) {
                mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort, mspProcessCommandFn);
                break; // process one command at a time so as not to block.
            }
        }

        if (mspPostProcessFn) {
            waitForSerialPortToFinishTransmitting(mspPort->port);
            mspPostProcessFn(mspPort->port);
        }
    }
    else {
        mspProcessPendingRequest(mspPort);
    }
}

可以观察到该函数主要完成三个工作，一是读取完整的报文，二是进行协议的解析，三是对报文内容进行处理，分别对应前文程序框架中提到的三个函数。

报文读取：serialRead

uint8_t serialRead(serialPort_t *instance)
{
    return instance->vTable->serialRead(instance);
}

实际上是调用了端口对应的读取函数，以使用了STM32的UART的端口的情况为例，这一操作调用的实际上是以下函数：

uint8_t uartRead(serialPort_t *instance)
{
    uint8_t ch;
    uartPort_t *s = (uartPort_t *)instance;


    ch = s->port.rxBuffer[s->port.rxBufferTail];
    if (s->port.rxBufferTail + 1 >= s->port.rxBufferSize) {
        s->port.rxBufferTail = 0;
    } else {
        s->port.rxBufferTail++;
    }

    return ch;
}

封装技巧：

开发者在这里使用了一种将函数指针封装在结构体内的方法，移植时只需要将对应的函数指针赋值即可，这样便于针对多种不同硬件同时使用不同配置，也避免了使用者修改c文件中的内容（大概）

struct serialPortVTable {
    void (*serialWrite)(serialPort_t *instance, uint8_t ch);

    uint32_t (*serialTotalRxWaiting)(const serialPort_t *instance);
    uint32_t (*serialTotalTxFree)(const serialPort_t *instance);

    uint8_t (*serialRead)(serialPort_t *instance);

    // Specified baud rate may not be allowed by an implementation, use serialGetBaudRate to determine actual baud rate in use.
    void (*serialSetBaudRate)(serialPort_t *instance, uint32_t baudRate);

    bool (*isSerialTransmitBufferEmpty)(const serialPort_t *instance);

    void (*setMode)(serialPort_t *instance, portMode_t mode);

    void (*writeBuf)(serialPort_t *instance, const void *data, int count);

    bool (*isConnected)(const serialPort_t *instance);

    bool (*isIdle)(serialPort_t *instance);

    // Optional functions used to buffer large writes.
    void (*beginWrite)(serialPort_t *instance);
    void (*endWrite)(serialPort_t *instance);
};

协议解析：mspSerialProcessReceivedData

INAV对于协议的解析是通过一个有限状态机来完成的，其中包括帧头的识别和接收完成后的校验，状态转移具体见下图：

完成接收后，该函数会将报文存储到对应端口的buffer，并且返回一个COMMAND_RECEIVED，表示接收完成，之后串口处理函数进入下一步操作。

由于这一部分源码实在是太大一坨，为了观看体验就不放在这里了，感兴趣的同学可以划到最底下看一看。

内容处理：mspSerialProcessReceivedCommand

static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn)
{
    uint8_t outBuf[MSP_PORT_OUTBUF_SIZE];

    mspPacket_t reply = {
        .buf = { .ptr = outBuf, .end = ARRAYEND(outBuf), },
        .cmd = -1,
        .flags = 0,
        .result = 0,
    };
    uint8_t *outBufHead = reply.buf.ptr;

    mspPacket_t command = {
        .buf = { .ptr = msp->inBuf, .end = msp->inBuf + msp->dataSize, },
        .cmd = msp->cmdMSP,
        .flags = msp->cmdFlags,
        .result = 0,
    };

    mspPostProcessFnPtr mspPostProcessFn = NULL;
    const mspResult_e status = mspProcessCommandFn(&command, &reply, &mspPostProcessFn);

    if (status != MSP_RESULT_NO_REPLY) {
        sbufSwitchToReader(&reply.buf, outBufHead); // change streambuf direction
        mspSerialEncode(msp, &reply, msp->mspVersion);
    }

    msp->c_state = MSP_IDLE;
    return mspPostProcessFn;
}

除去前面的一些初始化代码以及一些后续的处理，这段代码的核心其实就是mspProcessCommandFn这个函数，看到这里是不是觉得有点眼熟？没错，那个从一开始就出现的函数指针到这里终于被调用了，我们来康康里面到底是什么东西。

mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
{
    mspResult_e ret = MSP_RESULT_ACK;
    sbuf_t *dst = &reply->buf;
    sbuf_t *src = &cmd->buf;
    const uint16_t cmdMSP = cmd->cmd;
    // initialize reply by default
    reply->cmd = cmd->cmd;

    if (MSP2_IS_SENSOR_MESSAGE(cmdMSP)) {
        ret = mspProcessSensorCommand(cmdMSP, src);
    } else if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
        ret = MSP_RESULT_ACK;
    } else if (cmdMSP == MSP_SET_PASSTHROUGH) {
        mspFcSetPassthroughCommand(dst, src, mspPostProcessFn);
        ret = MSP_RESULT_ACK;
    } else {
        if (!mspFCProcessInOutCommand(cmdMSP, dst, src, &ret)) {
            ret = mspFcProcessInCommand(cmdMSP, src);
        }
    }

    // Process DONT_REPLY flag
    if (cmd->flags & MSP_FLAG_DONT_REPLY) {
        ret = MSP_RESULT_NO_REPLY;
    }

    reply->result = ret;
    return ret;
}

这个函数判定报文的类型，再根据报文类型作相应的处理。

报文类型判定

可以看到，报文类型判定时访问的是cmdMSP这个变量，我们来看看这个变量最初是在哪里被赋值的呢？

case MSP_HEADER_V2_NATIVE:
            mspPort->inBuf[mspPort->offset++] = c;
            mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
            if (mspPort->offset == sizeof(mspHeaderV2_t)) {
                mspHeaderV2_t * hdrv2 = (mspHeaderV2_t *)&mspPort->inBuf[0];

                // Check for potential buffer overflow
                if (hdrv2->size > MSP_PORT_INBUF_SIZE) {
                    mspPort->c_state = MSP_IDLE;
                }
                else {
                    mspPort->dataSize = hdrv2->size;
                    mspPort->cmdMSP = hdrv2->cmd;
                    mspPort->cmdFlags = hdrv2->flags;
                    mspPort->offset = 0;                // re-use buffer
                    mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V2_NATIVE : MSP_CHECKSUM_V2_NATIVE;
                }
            }
            break;

顺藤摸瓜，发现它的赋值是在mspSerialProcessReceivedData中完成的，赋值的内容实际上就是报文中那两个字节的function。由于这次使用的是传感器模块，我们来看一下传感器判断的宏里面是什么：

#define MSP2_IS_SENSOR_MESSAGE(x)   ((x) >= 0x1F00 && (x) <= 0x1FFF)

#define MSP2_SENSOR_RANGEFINDER     0x1F01
#define MSP2_SENSOR_OPTIC_FLOW      0x1F02
#define MSP2_SENSOR_GPS             0x1F03
#define MSP2_SENSOR_COMPASS         0x1F04
#define MSP2_SENSOR_BAROMETER       0x1F05
#define MSP2_SENSOR_AIRSPEED        0x1F06

对应上了我们之前提到的传感器报文类型。

在这之后就是许多不同类型报文的具体处理了，这里我们不继续展开。

封装技巧：

开发者在这里也利用了结构体来进行报文的解析，以上文代码中的hdrv2结构体为例：

typedef struct __attribute__((packed)) {
    uint8_t  flags;
    uint16_t cmd;
    uint16_t size;
} mspHeaderV2_t;

结合上文我们不难得知，将报文缓冲区的指针赋值给该结构体类型的指针后，我们就可以以访问结构体成员的方式读取报文对应位置的信息，增强了代码的可读性。与笔者平时惯用的移位后赋值给对应变量的写法相比也节省了部分内存，提高了程序的效率。

总结

通过对INAV飞控MSP协议通信程序的分析，笔者更加具体地了解了协议手册中未提及的内容，从中也学习了一些程序封装的技巧，最重要的是我终于可以把模块牛过来自己用了（雾）

附录

参考资料：

INAV飞控源码

MSP协议手册

BetaFlight模块设计之三十二：MSP协议模块分析

mspSerialProcessReceivedData源码：

static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
{
    switch (mspPort->c_state) {
        default:
        case MSP_IDLE:      // Waiting for '$' character
            if (c == '$') {
                mspPort->mspVersion = MSP_V1;
                mspPort->c_state = MSP_HEADER_START;
            }
            else {
                return false;
            }
            break;

        case MSP_HEADER_START:  // Waiting for 'M' (MSPv1 / MSPv2_over_v1) or 'X' (MSPv2 native)
            switch (c) {
                case 'M':
                    mspPort->c_state = MSP_HEADER_M;
                    break;
                case 'X':
                    mspPort->c_state = MSP_HEADER_X;
                    break;
                default:
                    mspPort->c_state = MSP_IDLE;
                    break;
            }
            break;

        case MSP_HEADER_M:      // Waiting for '<'
            if (c == '<') {
                mspPort->offset = 0;
                mspPort->checksum1 = 0;
                mspPort->checksum2 = 0;
                mspPort->c_state = MSP_HEADER_V1;
            }
            else {
                mspPort->c_state = MSP_IDLE;
            }
            break;

        case MSP_HEADER_X:
            if (c == '<') {
                mspPort->offset = 0;
                mspPort->checksum2 = 0;
                mspPort->mspVersion = MSP_V2_NATIVE;
                mspPort->c_state = MSP_HEADER_V2_NATIVE;
            }
            else {
                mspPort->c_state = MSP_IDLE;
            }
            break;

        case MSP_HEADER_V1:     // Now receive v1 header (size/cmd), this is already checksummable
            mspPort->inBuf[mspPort->offset++] = c;
            mspPort->checksum1 ^= c;
            if (mspPort->offset == sizeof(mspHeaderV1_t)) {
                mspHeaderV1_t * hdr = (mspHeaderV1_t *)&mspPort->inBuf[0];
                // Check incoming buffer size limit
                if (hdr->size > MSP_PORT_INBUF_SIZE) {
                    mspPort->c_state = MSP_IDLE;
                }
                else if (hdr->cmd == MSP_V2_FRAME_ID) {
                    // MSPv1 payload must be big enough to hold V2 header + extra checksum
                    if (hdr->size >= sizeof(mspHeaderV2_t) + 1) {
                        mspPort->mspVersion = MSP_V2_OVER_V1;
                        mspPort->c_state = MSP_HEADER_V2_OVER_V1;
                    }
                    else {
                        mspPort->c_state = MSP_IDLE;
                    }
                }
                else {
                    mspPort->dataSize = hdr->size;
                    mspPort->cmdMSP = hdr->cmd;
                    mspPort->cmdFlags = 0;
                    mspPort->offset = 0;                // re-use buffer
                    mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V1 : MSP_CHECKSUM_V1;    // If no payload - jump to checksum byte
                }
            }
            break;

        case MSP_PAYLOAD_V1:
            mspPort->inBuf[mspPort->offset++] = c;
            mspPort->checksum1 ^= c;
            if (mspPort->offset == mspPort->dataSize) {
                mspPort->c_state = MSP_CHECKSUM_V1;
            }
            break;

        case MSP_CHECKSUM_V1:
            if (mspPort->checksum1 == c) {
                mspPort->c_state = MSP_COMMAND_RECEIVED;
            } else {
                mspPort->c_state = MSP_IDLE;
            }
            break;

        case MSP_HEADER_V2_OVER_V1:     // V2 header is part of V1 payload - we need to calculate both checksums now
            mspPort->inBuf[mspPort->offset++] = c;
            mspPort->checksum1 ^= c;
            mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
            if (mspPort->offset == (sizeof(mspHeaderV2_t) + sizeof(mspHeaderV1_t))) {
                mspHeaderV2_t * hdrv2 = (mspHeaderV2_t *)&mspPort->inBuf[sizeof(mspHeaderV1_t)];
                mspPort->dataSize = hdrv2->size;

                // Check for potential buffer overflow
                if (hdrv2->size > MSP_PORT_INBUF_SIZE) {
                    mspPort->c_state = MSP_IDLE;
                }
                else {
                    mspPort->cmdMSP = hdrv2->cmd;
                    mspPort->cmdFlags = hdrv2->flags;
                    mspPort->offset = 0;                // re-use buffer
                    mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V2_OVER_V1 : MSP_CHECKSUM_V2_OVER_V1;
                }
            }
            break;

        case MSP_PAYLOAD_V2_OVER_V1:
            mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
            mspPort->checksum1 ^= c;
            mspPort->inBuf[mspPort->offset++] = c;

            if (mspPort->offset == mspPort->dataSize) {
                mspPort->c_state = MSP_CHECKSUM_V2_OVER_V1;
            }
            break;

        case MSP_CHECKSUM_V2_OVER_V1:
            mspPort->checksum1 ^= c;
            if (mspPort->checksum2 == c) {
                mspPort->c_state = MSP_CHECKSUM_V1; // Checksum 2 correct - verify v1 checksum
            } else {
                mspPort->c_state = MSP_IDLE;
            }
            break;

        case MSP_HEADER_V2_NATIVE:
            mspPort->inBuf[mspPort->offset++] = c;
            mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
            if (mspPort->offset == sizeof(mspHeaderV2_t)) {
                mspHeaderV2_t * hdrv2 = (mspHeaderV2_t *)&mspPort->inBuf[0];

                // Check for potential buffer overflow
                if (hdrv2->size > MSP_PORT_INBUF_SIZE) {
                    mspPort->c_state = MSP_IDLE;
                }
                else {
                    mspPort->dataSize = hdrv2->size;
                    mspPort->cmdMSP = hdrv2->cmd;
                    mspPort->cmdFlags = hdrv2->flags;
                    mspPort->offset = 0;                // re-use buffer
                    mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V2_NATIVE : MSP_CHECKSUM_V2_NATIVE;
                }
            }
            break;

        case MSP_PAYLOAD_V2_NATIVE:
            mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
            mspPort->inBuf[mspPort->offset++] = c;

            if (mspPort->offset == mspPort->dataSize) {
                mspPort->c_state = MSP_CHECKSUM_V2_NATIVE;
            }
            break;

        case MSP_CHECKSUM_V2_NATIVE:
            if (mspPort->checksum2 == c) {
                mspPort->c_state = MSP_COMMAND_RECEIVED;
            } else {
                mspPort->c_state = MSP_IDLE;
            }
            break;
    }

    return true;
}
最后修改：2023 年 08 月 13 日
© 允许规范转载
V我五十