PX4无人机传感器校准MavLink指令协议说明

发布于 2022-06-16  573 次阅读


PX4 是一款专业级飞控, 它由来自业界和学术界的世界级开发商开发,并得到活跃的全球社区的支持。

本文介绍传感器校准的协议说明。

传感器MavLink协议/消息。

一、标定触发与停止

未起飞前才能触发,且单次指令只能包含一个标定目标。
触发标定:

(MAV_CMD_PREFLIGHT_CALIBRATION,    // command id
param1, param2, param3, param4, param5, param6, param7);

参数分别对应:陀螺仪、磁力计、地面压力、遥控器、加速度计、电机/空速校准、电调/气压器,每个校准类型内还可以细分校准项,具体参数位可选参数如上图示。
停止触发,全部置0:

(MAV_CMD_PREFLIGHT_CALIBRATION,     // command id
                   0,                                 // gyro cal
                   0,                                 // mag cal
                   0,                                 // ground pressure
                   0,                                 // radio cal
                 0,                                 // accel cal
                   0,                                 // airspeed cal
                   0);                                 // esc cal

二、消息接收

系统状态为STATUSTEXT信息,进入标定后,标定的信息需要通过Text去判断当前标定的清空,如类型、进度等。

参数 说明 备注
Severity 信息的严重度 -
Text 无null中止符的信息描述文本 -
Id 此消息的唯一(不透明)标识符。可用于从一个块序列重新组合一个逻辑长文本状态消息。值为0表示这是序列中唯一的块,可以立即发出消息。 -
chunk_seq 这个块的序号;索引是从0开始的。文本字段中的任何空字符都表示这是最后一个块 -

Java:MavLink-> MagCalStatus类:标定状态
MessageText中,所有的标定信息起于[cal]标签,QGC处理示例:

void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
{
    Q_UNUSED(compId);
    Q_UNUSED(severity);

    if (uasId != _vehicle->id()) {
        return;
    }

    if (text.contains("progress <")) {
        QString percent = text.split("<").last().split(">").first();
        bool ok;
        int p = percent.toInt(&ok);
        if (ok) {
            if (_progressBar) {
                _progressBar->setProperty("value", (float)(p / 100.0));
            } else {
                qWarning() << "Internal error";
            }
        }
        return;
    }

    _appendStatusLog(text);
    qCDebug(SensorsComponentControllerLog) << text;

    if (_unknownFirmwareVersion) {
        // We don't know how to do visual cal with the version of firwmare
        return;
    }

    // All calibration messages start with [cal]
    QString calPrefix("[cal] ");
    if (!text.startsWith(calPrefix)) {
        return;
    }
    text = text.right(text.length() - calPrefix.length());

    QString calStartPrefix("calibration started: ");
    if (text.startsWith(calStartPrefix)) {
        text = text.right(text.length() - calStartPrefix.length());

        // Split version number and cal type
        QStringList parts = text.split(" ");
        if (parts.count() != 2 && parts[0].toInt() != _supportedFirmwareCalVersion) {
            _unknownFirmwareVersion = true;
            QString msg = tr("Unsupported calibration firmware version, using log");
            _appendStatusLog(msg);
            qDebug() << msg;
            return;
        }

        _startVisualCalibration();

        text = parts[1];
        if (text == "accel" || text == "mag" || text == "gyro") {
            // Reset all progress indication
            _orientationCalDownSideDone = false;
            _orientationCalUpsideDownSideDone = false;
            _orientationCalLeftSideDone = false;
            _orientationCalRightSideDone = false;
            _orientationCalTailDownSideDone = false;
            _orientationCalNoseDownSideDone = false;
            _orientationCalDownSideInProgress = false;
            _orientationCalUpsideDownSideInProgress = false;
            _orientationCalLeftSideInProgress = false;
            _orientationCalRightSideInProgress = false;
            _orientationCalNoseDownSideInProgress = false;
            _orientationCalTailDownSideInProgress = false;

            // Reset all visibility
            _orientationCalDownSideVisible = false;
            _orientationCalUpsideDownSideVisible = false;
            _orientationCalLeftSideVisible = false;
            _orientationCalRightSideVisible = false;
            _orientationCalTailDownSideVisible = false;
            _orientationCalNoseDownSideVisible = false;

            _orientationCalAreaHelpText->setProperty("text", tr("Place your vehicle into one of the Incomplete orientations shown below and hold it still"));

            if (text == "accel") {
                _accelCalInProgress = true;
                _orientationCalDownSideVisible = true;
                _orientationCalUpsideDownSideVisible = true;
                _orientationCalLeftSideVisible = true;
                _orientationCalRightSideVisible = true;
                _orientationCalTailDownSideVisible = true;
                _orientationCalNoseDownSideVisible = true;
            } else if (text == "mag") {

                // Work out what the autopilot is configured to
                int sides = 0;

                if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, "CAL_MAG_SIDES")) {
                    // Read the requested calibration directions off the system
                    sides = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, "CAL_MAG_SIDES")->rawValue().toFloat();
                } else {
                    // There is no valid setting, default to all six sides
                    sides = (1 << 5) | (1 << 4) | (1 << 3) | (1 << 2) | (1 << 1) | (1 << 0);
                }

                _magCalInProgress = true;
                _orientationCalTailDownSideVisible =   ((sides & (1 << 0)) > 0);
                _orientationCalNoseDownSideVisible =   ((sides & (1 << 1)) > 0);
                _orientationCalLeftSideVisible =       ((sides & (1 << 2)) > 0);
                _orientationCalRightSideVisible =      ((sides & (1 << 3)) > 0);
                _orientationCalUpsideDownSideVisible = ((sides & (1 << 4)) > 0);
                _orientationCalDownSideVisible =       ((sides & (1 << 5)) > 0);
            } else if (text == "gyro") {
                _gyroCalInProgress = true;
                _orientationCalDownSideVisible = true;
            } else {
                qWarning() << "Unknown calibration message type" << text;
            }
            emit orientationCalSidesDoneChanged();
            emit orientationCalSidesVisibleChanged();
            emit orientationCalSidesInProgressChanged();
            _updateAndEmitShowOrientationCalArea(true);
        } else if (text == "airspeed") {
            _airspeedCalInProgress = true;
        } else if (text == "level") {
            _levelCalInProgress = true;
        }
        return;
    }

    if (text.endsWith("orientation detected")) {
        QString side = text.section(" ", 0, 0);
        qCDebug(SensorsComponentControllerLog) << "Side started" << side;

        if (side == "down") {
            _orientationCalDownSideInProgress = true;
            if (_magCalInProgress) {
                _orientationCalDownSideRotate = true;
            }
        } else if (side == "up") {
            _orientationCalUpsideDownSideInProgress = true;
            if (_magCalInProgress) {
                _orientationCalUpsideDownSideRotate = true;
            }
        } else if (side == "left") {
            _orientationCalLeftSideInProgress = true;
            if (_magCalInProgress) {
                _orientationCalLeftSideRotate = true;
            }
        } else if (side == "right") {
            _orientationCalRightSideInProgress = true;
            if (_magCalInProgress) {
                _orientationCalRightSideRotate = true;
            }
        } else if (side == "front") {
            _orientationCalNoseDownSideInProgress = true;
            if (_magCalInProgress) {
                _orientationCalNoseDownSideRotate = true;
            }
        } else if (side == "back") {
            _orientationCalTailDownSideInProgress = true;
            if (_magCalInProgress) {
                _orientationCalTailDownSideRotate = true;
            }
        }

        if (_magCalInProgress) {
            _orientationCalAreaHelpText->setProperty("text", tr("Rotate the vehicle continuously as shown in the diagram until marked as Completed"));
        } else {
            _orientationCalAreaHelpText->setProperty("text", tr("Hold still in the current orientation"));
        }

        emit orientationCalSidesInProgressChanged();
        emit orientationCalSidesRotateChanged();
        return;
    }

    if (text.endsWith("side done, rotate to a different side")) {
        QString side = text.section(" ", 0, 0);
        qCDebug(SensorsComponentControllerLog) << "Side finished" << side;

        if (side == "down") {
            _orientationCalDownSideInProgress = false;
            _orientationCalDownSideDone = true;
            _orientationCalDownSideRotate = false;
        } else if (side == "up") {
            _orientationCalUpsideDownSideInProgress = false;
            _orientationCalUpsideDownSideDone = true;
            _orientationCalUpsideDownSideRotate = false;
        } else if (side == "left") {
            _orientationCalLeftSideInProgress = false;
            _orientationCalLeftSideDone = true;
            _orientationCalLeftSideRotate = false;
        } else if (side == "right") {
            _orientationCalRightSideInProgress = false;
            _orientationCalRightSideDone = true;
            _orientationCalRightSideRotate = false;
        } else if (side == "front") {
            _orientationCalNoseDownSideInProgress = false;
            _orientationCalNoseDownSideDone = true;
            _orientationCalNoseDownSideRotate = false;
        } else if (side == "back") {
            _orientationCalTailDownSideInProgress = false;
            _orientationCalTailDownSideDone = true;
            _orientationCalTailDownSideRotate = false;
        }

        _orientationCalAreaHelpText->setProperty("text", tr("Place you vehicle into one of the orientations shown below and hold it still"));

        emit orientationCalSidesInProgressChanged();
        emit orientationCalSidesDoneChanged();
        emit orientationCalSidesRotateChanged();
        return;
    }

    if (text.endsWith("side already completed")) {
        _orientationCalAreaHelpText->setProperty("text", tr("Orientation already completed, place you vehicle into one of the incomplete orientations shown below and hold it still"));
        return;
    }

    QString calCompletePrefix("calibration done:");
    if (text.startsWith(calCompletePrefix)) {
        _stopCalibration(StopCalibrationSuccess);
        return;
    }

    if (text.startsWith("calibration cancelled")) {
        _stopCalibration(_waitingForCancel ? StopCalibrationCancelled : StopCalibrationFailed);
        return;
    }

    if (text.startsWith("calibration failed")) {
        _stopCalibration(StopCalibrationFailed);
        return;
    }
}

校准的文本格式如下(根据调试信息):

[cal] calibration started: supportedFirmwareCalVersion {sensorsType} 

{sensorsType}为可变参,sensorsType可为accel/mag/gyro等,以上为开始校准的信息,进入校准检测到指定的旋转方向:

[cal] {direction} orientation detected

{direction}为可变参,为:down、up、left、right、front、back。
某一旋转方向完成:

[cal] {direction} side done, rotate to a different side

校准方向已完成:

[cal] {direction} side already completed

校准完成:

[cal] calibration done: {information}

取消校准:

[cal] calibration cancelled

校准失败:

[cal] calibration failed

QGC实测校准日志:

三、磁力计

MavLink-> MagCalProgress:指南针标定进度
MavLink->MagCalReport: 报告完成指南针校准的结果。收到 MAG_CAL_ACK才会发送,注:MAG_CAL_ACK消息指令未找到
此消息使用场景与StatusText使用场景的区别待定(待实测确认),QGC暂未找到使用的地方。
MavLink磁力计校准实测:

四、IMU

IMU的校准信息:

[cal] calibration started: supportedFirmwareCalVersion sensorsType

sensorsType为accel/gyro。
IMU由陀螺仪、加速度计共同组成,校准IMU即校准组成的传感器。
陀螺仪只需要静置即可完成校准,加速度计需要六向校准。

五、方向说明

以朝重力方向(以下简称朝下)为基准:
(1)Down :无人机正常水平放置,机身底部朝下
(2)Up :无人机翻转,机身上部朝下
(3)Front: 机头朝下
(4)Back :机尾朝下
(5)Left:机身左侧朝下
(6)Right:机身右侧朝下

六、参考

[1]MavLink指令集文档:http://mavlink.io/zh/messages/common.html
[2]QgroundControl源码:https://github.com/mavlink/qgroundcontrol