通过串口读取mavlink数据

一，整体架构

2，代码

（1），子线程读取串口并打包成mavlink  消息，Qt emit出此消息

void MySerialLink::sl_readBytes()

{

    if(mPort&&mPort->isOpen())

    {

        const qint64 maxLength = 2048;

        char data[maxLength];

        _dataMutex.lock();

        qint64 numBytes = mPort->bytesAvailable();

        if (numBytes > 0)

        {

            /* Read as much data in buffer as possible without overflow */

            if(maxLength < numBytes)

                numBytes = maxLength;

            mPort->read(data, numBytes);

            QByteArray buffer(data, numBytes);

            //接收mavlink数据

            mavlink_message_t message;

            mavlink_status_t status;

            int mavlinkChannel=0;

            static int mavlink09Count = 0;

            static int nonmavlinkCount = 0;

            static bool decodedFirstPacket = false;

            static bool warnedUser = false;

            static bool checkedUserNonMavlink = false;

            static bool warnedUserNonMavlink = false;

            for (int position = 0; position < buffer.size(); position++)

            {

                unsigned int decodeState = mavlink_parse_char(mavlinkChannel, (uint8_t)(buffer[position]), &message, &status);

                if ((uint8_t)buffer[position] == 0x55)//解析到 枕头

                    mavlink09Count++;

                //根据枕头 判断 是否是 使用0.9版本的mavlink

                if ((mavlink09Count > 100) && !decodedFirstPacket && !warnedUser)

                {

                    warnedUser = true;

                    // Obviously the user tries to use a 0.9 autopilot

                    // with PVAssistant built for version 1.0

                    qDebug()<< tr("There is a MAVLink Version or Baud Rate Mismatch. "

                                  "Your MAVLink device seems to use the deprecated version 0.9, while PVAssistant only supports version 1.0+. "

                                  "Please upgrade the MAVLink version of your autopilot. "

                                  "If your autopilot is using version 1.0, check if the baud rates of PVAssistant and your autopilot are the same.");

                }

                if (decodeState == 0 && !decodedFirstPacket)

                {

                    nonmavlinkCount++;

                    if (nonmavlinkCount > 2000 && !warnedUserNonMavlink)

                    {

                        //2000 bytes with no mavlink message. Are we connected to a mavlink capable device?

                        if (!checkedUserNonMavlink)

                        {

                            this->requestReset();

                            checkedUserNonMavlink = true;

                        }

                        else

                        {

                            warnedUserNonMavlink = true;

                            qDebug()<<tr("There is a MAVLink Version or Baud Rate Mismatch. "

                                         "Please check if the baud rates of PVAssistant and your autopilot are the same.");

                        }

                    }

                }

                if (decodeState == 1)

                {

                    decodedFirstPacket = true;

                    // Increase receive counter

                    totalReceiveCounter[mavlinkChannel]++;

                    currReceiveCounter[mavlinkChannel]++;

                    // Determine what the next expected sequence number is, accounting for

                    // never having seen a message for this system/component pair.

                    int lastSeq = lastIndex[message.sysid][message.compid];

                    int expectedSeq = (lastSeq == -1) ? message.seq : (lastSeq + 1);

                    // And if we didn't encounter that sequence number, record the error

                    if (message.seq != expectedSeq)

                    {

                        // Determine how many messages were skipped

                        int lostMessages = message.seq - expectedSeq;

                        // Out of order messages or wraparound can cause this, but we just ignore these conditions for simplicity

                        if (lostMessages < 0)

                        {

                            lostMessages = 0;

                        }

                        // And log how many were lost for all time and just this timestep

                        totalLossCounter[mavlinkChannel] += lostMessages;

                        currLossCounter[mavlinkChannel] += lostMessages;

                    }

                    // And update the last sequence number for this system/component pair

                    lastIndex[message.sysid][message.compid] = expectedSeq;

                    // Update on every 32th packet

                    if ((totalReceiveCounter[mavlinkChannel] & 0x1F) == 0)

                    {

                        // Calculate new loss ratio

                        // Receive loss

                        float receiveLoss = (double)currLossCounter[mavlinkChannel]/(double)(currReceiveCounter[mavlinkChannel]+currLossCounter[mavlinkChannel]);

                        receiveLoss *= 100.0f;

                        currLossCounter[mavlinkChannel] = 0;

                        currReceiveCounter[mavlinkChannel] = 0;

                    }

                    emit bytesReceived(message);

                }

            }

        }

        _dataMutex.unlock();

    }

}

（2），主线程 槽函数 接受此消息，并解析出具体的结构体数据，当前 解析的为磁力计数据。

void Widget::sl_receiveStr(mavlink_message_t message)

{

    if(message.msgid== MAVLINK_MSG_ID_HIGHRES_IMU)

    {

        __mavlink_highres_imu_t state;

        mavlink_msg_highres_imu_decode(&message, &state);

        //磁

        float xmag = state.xmag;

        float ymag = state.ymag;

        float zmag = state.zmag;

    }

    link->clear(1);

}

（3），串口也可以更改成 tcp，接收tcp数据。