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
Comments NOTHING