zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Better sensor cal 

- cancel support
- mag rotation images
- set board rotation before cal
- set compass rotation after mag cal
QGC4.4
Don Gagne 10 years ago
parent
6235763f8c
commit
ea433369ad
3 changed files with 556 additions and 240 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 348
      src/AutoPilotPlugins/PX4/SensorsComponent.qml
  2. 382
      src/AutoPilotPlugins/PX4/SensorsComponentController.cc
  3. 66
      src/AutoPilotPlugins/PX4/SensorsComponentController.h

348
src/AutoPilotPlugins/PX4/SensorsComponent.qml
Unescape View File

@ -24,6 +24,7 @@ @@ -24,6 +24,7 @@
import QtQuick 2.2
import QtQuick.Controls 1.2
import QtQuick.Controls.Styles 1.2
import QtQuick.Dialogs 1.2
import QGroundControl.FactSystem 1.0
import QGroundControl.FactControls 1.0
@ -66,6 +67,25 @@ Rectangle { @@ -66,6 +67,25 @@ Rectangle {
"ROTATION_ROLL_270_YAW_270"
]
readonly property string statusTextAreaDefaultText: "Sensor config is a work in progress. Not all visuals for all calibration types fully implemented.\n\n" +
"For Compass calibration you will need to rotate your vehicle through a number of positions. For this calibration is is best " +
"to be connected to you vehicle via radio instead of USB since the USB cable will likely get in the way.\n\n" +
"For Gyroscope calibration you will need to place your vehicle right side up on solid surface and leave it still.\n\n" +
"For Accelerometer calibration you will need to place your vehicle on all six sides and hold it there for a few seconds.\n\n" +
"For Airspeed calibration you will need to keep your airspeed sensor out of any wind.\n\n"
Fact { id: cal_mag0_id; name: "CAL_MAG0_ID" }
Fact { id: cal_mag1_id; name: "CAL_MAG1_ID" }
Fact { id: cal_mag2_id; name: "CAL_MAG2_ID" }
Fact { id: cal_mag0_rot; name: "CAL_MAG0_ROT" }
Fact { id: cal_mag1_rot; name: "CAL_MAG1_ROT" }
Fact { id: cal_mag2_rot; name: "CAL_MAG2_ROT" }
// Id > = signals compass available, rot < 0 signals internal compass
property bool showCompass0Rot: cal_mag0_id.value > 0 && cal_mag0_rot.value >= 0
property bool showCompass1Rot: cal_mag1_id.value > 0 && cal_mag1_rot.value >= 0
property bool showCompass2Rot: cal_mag2_id.value > 0 && cal_mag2_rot.value >= 0
color: qgcPal.window
// We use this bogus loader just so we can get an onLoaded signal to hook to in order to
@ -83,6 +103,227 @@ Rectangle { @@ -83,6 +103,227 @@ Rectangle {
controller.gyroButton = gyroButton
controller.accelButton = accelButton
controller.airspeedButton = airspeedButton
controller.cancelButton = cancelButton
controller.orientationCalAreaHelpText = orientationCalAreaHelpText
}
}
Connections {
target: controller
onResetStatusTextArea: statusTextArea.text = statusTextAreaDefaultText
onSetCompassRotations: showCompassRotationOverlay()
}
Rectangle {
id: overlay
anchors.fill: parent
color: qgcPal.window
opacity: 0.75
z: 100
visible: false
}
Rectangle {
width: 300
height: 100
anchors.verticalCenter: parent.verticalCenter
anchors.horizontalCenter: parent.horizontalCenter
color: qgcPal.window
border.width: 1
border.color: qgcPal.text
visible: controller.waitingForCancel
z: overlay.z + 1
onVisibleChanged: {
overlay.visible = visible
}
QGCLabel {
anchors.fill: parent
verticalAlignment: Text.AlignVCenter
horizontalAlignment: Text.AlignHCenter
text: "Waiting for Cancel (may take a few seconds)"
}
}
Rectangle {
property string calibrationType
id: boardRotationOverlay
width: 300
height: boardRotationOverlayColumn.height + 11
anchors.verticalCenter: parent.verticalCenter
anchors.horizontalCenter: parent.horizontalCenter
color: qgcPal.window
border.width: 1
border.color: qgcPal.text
visible: false
z: overlay.z + 1
Column {
id: boardRotationOverlayColumn
anchors.topMargin: 10
anchors.top: parent.top
width: parent.width
spacing: 10
Column {
anchors.leftMargin: 10
anchors.rightMargin: 10
anchors.left: parent.left
anchors.right: parent.right
spacing: 10
QGCLabel {
width: parent.width
wrapMode: Text.WordWrap
text: "Please check and/or update board rotation before calibrating"
}
FactComboBox {
width: rotationColumnWidth
model: rotations
fact: Fact { name: "SENS_BOARD_ROT" }
}
}
QGCButton {
x: 1
width: parent.width - 2
primary: true
text: "OK"
onClicked: {
boardRotationOverlay.visible = false
overlay.visible = false
if (boardRotationOverlay.calibrationType == "gyro") {
controller.calibrateGyro()
} else if (boardRotationOverlay.calibrationType == "accel") {
controller.calibrateAccel()
} else if (boardRotationOverlay.calibrationType == "compass") {
controller.calibrateCompass()
}
}
}
}
}
function showBoardRotationOverlay(calibrationType) {
boardRotationOverlay.calibrationType = calibrationType
boardRotationOverlay.visible = true
overlay.visible = true
}
Rectangle {
id: compassRotationOverlay
width: 300
height: compassRotationOverlayColumn.height + 11
anchors.verticalCenter: parent.verticalCenter
anchors.horizontalCenter: parent.horizontalCenter
color: qgcPal.window
border.width: 1
border.color: qgcPal.text
visible: false
z: overlay.z + 1
Column {
id: compassRotationOverlayColumn
anchors.topMargin: 10
anchors.top: parent.top
width: parent.width
spacing: 10
Column {
anchors.leftMargin: 10
anchors.rightMargin: 10
anchors.left: parent.left
anchors.right: parent.right
spacing: 10
QGCLabel {
width: parent.width
wrapMode: Text.WordWrap
text: "Please check and/or update compass rotation(s)"
}
// Compass 0 rotation
Component {
id: compass0ComponentLabel
QGCLabel { text: "Compass Orientation" }
}
Component {
id: compass0ComponentCombo
FactComboBox {
id: compass0RotationCombo
width: rotationColumnWidth
model: rotations
fact: Fact { name: "CAL_MAG0_ROT" }
}
}
Loader { sourceComponent: showCompass0Rot ? compass0ComponentLabel : null }
Loader { sourceComponent: showCompass0Rot ? compass0ComponentCombo : null }
// Compass 1 rotation
Component {
id: compass1ComponentLabel
QGCLabel { text: "Compass 1 Orientation" }
}
Component {
id: compass1ComponentCombo
FactComboBox {
id: compass1RotationCombo
width: rotationColumnWidth
model: rotations
fact: Fact { name: "CAL_MAG1_ROT" }
}
}
Loader { sourceComponent: showCompass1Rot ? compass1ComponentLabel : null }
Loader { sourceComponent: showCompass1Rot ? compass1ComponentCombo : null }
// Compass 2 rotation
Component {
id: compass2ComponentLabel
QGCLabel { text: "Compass 2 Orientation" }
}
Component {
id: compass2ComponentCombo
FactComboBox {
id: compass1RotationCombo
width: rotationColumnWidth
model: rotations
fact: Fact { name: "CAL_MAG2_ROT" }
}
}
Loader { sourceComponent: showCompass2Rot ? compass2ComponentLabel : null }
Loader { sourceComponent: showCompass2Rot ? compass2ComponentCombo : null }
}
QGCButton {
x: 1
width: parent.width - 2
primary: true
text: "OK"
onClicked: {
compassRotationOverlay.visible = false
overlay.visible = false
}
}
}
}
function showCompassRotationOverlay() {
if (showCompass0Rot || showCompass1Rot || showCompass2Rot) {
compassRotationOverlay.visible = true
overlay.visible = true
}
}
@ -110,7 +351,8 @@ Rectangle { @@ -110,7 +351,8 @@ Rectangle {
width: parent.buttonWidth
text: "Compass"
indicatorGreen: fact.value != 0
onClicked: controller.calibrateCompass()
onClicked: showBoardRotationOverlay("compass")
}
IndicatorButton {
@ -120,7 +362,8 @@ Rectangle { @@ -120,7 +362,8 @@ Rectangle {
width: parent.buttonWidth
text: "Gyroscope"
indicatorGreen: fact.value != 0
onClicked: controller.calibrateGyro()
onClicked: showBoardRotationOverlay("gyro")
}
IndicatorButton {
@ -130,7 +373,8 @@ Rectangle { @@ -130,7 +373,8 @@ Rectangle {
width: parent.buttonWidth
text: "Accelerometer"
indicatorGreen: fact.value != 0
onClicked: controller.calibrateAccel()
onClicked: showBoardRotationOverlay("accel")
}
IndicatorButton {
@ -143,6 +387,13 @@ Rectangle { @@ -143,6 +387,13 @@ Rectangle {
indicatorGreen: fact.value != 0
onClicked: controller.calibrateAirspeed()
}
QGCButton {
id: cancelButton
text: "Cancel"
enabled: false
onClicked: controller.cancelCalibration()
}
}
Item { height: 20; width: 10 } // spacer
@ -167,12 +418,7 @@ Rectangle { @@ -167,12 +418,7 @@ Rectangle {
height: parent.height
readOnly: true
frameVisible: false
text: "Sensor config is a work in progress. Not all visuals for all calibration types fully implemented.\n\n" +
"For Compass calibration you will need to rotate your vehicle through a number of positions. For this calibration is is best " +
"to be connected to you vehicle via radio instead of USB since the USB cable will likely get in the way.\n\n" +
"For Gyroscope calibration you will need to place your vehicle right side up on solid surface and leave it still.\n\n" +
"For Accelerometer calibration you will need to place your vehicle on all six sides and hold it there for a few seconds.\n\n" +
"For Airspeed calibration you will need to keep your airspeed sensor out of any wind.\n\n"
text: statusTextAreaDefaultText
style: TextAreaStyle {
textColor: qgcPal.text
@ -181,29 +427,6 @@ Rectangle { @@ -181,29 +427,6 @@ Rectangle {
}
Rectangle {
id: gyroCalArea
width: parent.calDisplayAreaWidth
height: parent.height
visible: controller.showGyroCalArea
color: qgcPal.windowShade
Column {
width: parent.width
QGCLabel {
text: "Place your vehicle upright on a solid surface and hold it still."
}
VehicleRotationCal {
calValid: true
calInProgress: controller.gyroCalInProgress
imageSource: "qrc:///qml/VehicleDown.png"
}
}
}
Rectangle {
id: orientationCalArea
width: parent.calDisplayAreaWidth
height: parent.height
@ -211,53 +434,58 @@ Rectangle { @@ -211,53 +434,58 @@ Rectangle {
color: qgcPal.windowShade
QGCLabel {
id: calAreaLabel
width: parent.width
wrapMode: Text.WordWrap
text: "Place your vehicle into each of the positions below and hold still. Once that position is completed you can move to another."
id: orientationCalAreaHelpText
width: parent.width
wrapMode: Text.WordWrap
font.pointSize: screenTools.fontPointFactor * (17);
}
Flow {
y: calAreaLabel.height
y: orientationCalAreaHelpText.height
width: parent.width
height: parent.height - calAreaLabel.implicitHeight
height: parent.height - orientationCalAreaHelpText.implicitHeight
spacing: 5
VehicleRotationCal {
visible: controller.orientationCalDownSideVisible
calValid: controller.orientationCalDownSideDone
calInProgress: controller.orientationCalDownSideInProgress
calInProgressText: controller.calInProgressText
imageSource: "qrc:///qml/VehicleDown.png"
calInProgressText: controller.orientationCalDownSideRotate ? "Rotate" : "Hold Still"
imageSource: controller.orientationCalDownSideRotate ? "qrc:///qml/VehicleDownRotate.png" : "qrc:///qml/VehicleDown.png"
}
VehicleRotationCal {
visible: controller.orientationCalUpsideDownSideVisible
calValid: controller.orientationCalUpsideDownSideDone
calInProgress: controller.orientationCalUpsideDownSideInProgress
calInProgressText: controller.calInProgressText
calInProgressText: "Hold Still"
imageSource: "qrc:///qml/VehicleUpsideDown.png"
}
VehicleRotationCal {
visible: controller.orientationCalNoseDownSideVisible
calValid: controller.orientationCalNoseDownSideDone
calInProgress: controller.orientationCalNoseDownSideInProgress
calInProgressText: controller.calInProgressText
imageSource: "qrc:///qml/VehicleNoseDown.png"
calInProgressText: controller.orientationCalNoseDownSideRotate ? "Rotate" : "Hold Still"
imageSource: controller.orientationCalNoseDownSideRotate ? "qrc:///qml/VehicleNoseDownRotate.png" : "qrc:///qml/VehicleNoseDown.png"
}
VehicleRotationCal {
visible: controller.orientationCalTailDownSideVisible
calValid: controller.orientationCalTailDownSideDone
calInProgress: controller.orientationCalTailDownSideInProgress
calInProgressText: controller.calInProgressText
calInProgressText: "Hold Still"
imageSource: "qrc:///qml/VehicleTailDown.png"
}
VehicleRotationCal {
visible: controller.orientationCalLeftSideVisible
calValid: controller.orientationCalLeftSideDone
calInProgress: controller.orientationCalLeftSideInProgress
calInProgressText: controller.calInProgressText
imageSource: "qrc:///qml/VehicleLeft.png"
calInProgressText: controller.orientationCalLeftSideRotate ? "Rotate" : "Hold Still"
imageSource: controller.orientationCalLeftSideRotate ? "qrc:///qml/VehicleLeftRotate.png" : "qrc:///qml/VehicleLeft.png"
}
VehicleRotationCal {
visible: controller.orientationCalRightSideVisible
calValid: controller.orientationCalRightSideDone
calInProgress: controller.orientationCalRightSideInProgress
calInProgressText: controller.calInProgressText
calInProgressText: "Hold Still"
imageSource: "qrc:///qml/VehicleRight.png"
}
}
@ -277,12 +505,12 @@ Rectangle { @@ -277,12 +505,12 @@ Rectangle {
// Compass 0 rotation
Component {
id: compass0ComponentLabel
id: compass0ComponentLabel2
QGCLabel { text: "Compass Orientation" }
}
Component {
id: compass0ComponentCombo
id: compass0ComponentCombo2
FactComboBox {
id: compass0RotationCombo
@ -291,17 +519,17 @@ Rectangle { @@ -291,17 +519,17 @@ Rectangle {
fact: Fact { name: "CAL_MAG0_ROT" }
}
}
Loader { sourceComponent: controller.showCompass0 ? compass0ComponentLabel : null }
Loader { sourceComponent: controller.showCompass0 ? compass0ComponentCombo : null }
Loader { sourceComponent: showCompass0Rot ? compass0ComponentLabel2 : null }
Loader { sourceComponent: showCompass0Rot ? compass0ComponentCombo2 : null }
// Compass 1 rotation
Component {
id: compass1ComponentLabel
id: compass1ComponentLabel2
QGCLabel { text: "Compass 1 Orientation" }
}
Component {
id: compass1ComponentCombo
id: compass1ComponentCombo2
FactComboBox {
id: compass1RotationCombo
@ -310,17 +538,17 @@ Rectangle { @@ -310,17 +538,17 @@ Rectangle {
fact: Fact { name: "CAL_MAG1_ROT" }
}
}
Loader { sourceComponent: controller.showCompass1 ? compass1ComponentLabel : null }
Loader { sourceComponent: controller.showCompass1 ? compass1ComponentCombo : null }
Loader { sourceComponent: showCompass1Rot ? compass1ComponentLabel2 : null }
Loader { sourceComponent: showCompass1Rot ? compass1ComponentCombo2 : null }
// Compass 2 rotation
Component {
id: compass2ComponentLabel
id: compass2ComponentLabel2
QGCLabel { text: "Compass 2 Orientation" }
}
Component {
id: compass2ComponentCombo
id: compass2ComponentCombo2
FactComboBox {
id: compass1RotationCombo
@ -329,8 +557,8 @@ Rectangle { @@ -329,8 +557,8 @@ Rectangle {
fact: Fact { name: "CAL_MAG2_ROT" }
}
}
Loader { sourceComponent: controller.showCompass2 ? compass2ComponentLabel : null }
Loader { sourceComponent: controller.showCompass2 ? compass2ComponentCombo : null }
Loader { sourceComponent: showCompass2Rot ? compass2ComponentLabel2 : null }
Loader { sourceComponent: showCompass2Rot ? compass2ComponentCombo2 : null }
}
}
}

382
src/AutoPilotPlugins/PX4/SensorsComponentController.cc
Unescape View File

@ -36,11 +36,12 @@ SensorsComponentController::SensorsComponentController(AutoPilotPlugin* autopilo @@ -36,11 +36,12 @@ SensorsComponentController::SensorsComponentController(AutoPilotPlugin* autopilo
QObject(parent),
_statusLog(NULL),
_progressBar(NULL),
_showGyroCalArea(false),
_compassButton(NULL),
_gyroButton(NULL),
_accelButton(NULL),
_airspeedButton(NULL),
_cancelButton(NULL),
_showOrientationCalArea(false),
_showCompass0(false),
_showCompass1(false),
_showCompass2(false),
_gyroCalInProgress(false),
_magCalInProgress(false),
_accelCalInProgress(false),
@ -50,18 +51,31 @@ SensorsComponentController::SensorsComponentController(AutoPilotPlugin* autopilo @@ -50,18 +51,31 @@ SensorsComponentController::SensorsComponentController(AutoPilotPlugin* autopilo
_orientationCalRightSideDone(false),
_orientationCalNoseDownSideDone(false),
_orientationCalTailDownSideDone(false),
_orientationCalDownSideVisible(false),
_orientationCalUpsideDownSideVisible(false),
_orientationCalLeftSideVisible(false),
_orientationCalRightSideVisible(false),
_orientationCalNoseDownSideVisible(false),
_orientationCalTailDownSideVisible(false),
_orientationCalDownSideInProgress(false),
_orientationCalUpsideDownSideInProgress(false),
_orientationCalLeftSideInProgress(false),
_orientationCalRightSideInProgress(false),
_orientationCalNoseDownSideInProgress(false),
_orientationCalTailDownSideInProgress(false),
_textLoggingStarted(false),
_orientationCalDownSideRotate(false),
_orientationCalLeftSideRotate(false),
_orientationCalNoseDownSideRotate(false),
_unknownFirmwareVersion(false),
_waitingForCancel(false),
_autopilot(autopilot)
{
Q_ASSERT(_autopilot);
Q_ASSERT(_autopilot->pluginReady());
_uas = _autopilot->uas();
Q_ASSERT(_uas);
// Mag rotation parameters are optional
_showCompass0 = _autopilot->parameterExists("CAL_MAG0_ROT") &&
_autopilot->getParameterFact("CAL_MAG0_ROT")->value().toInt() >= 0;
@ -84,89 +98,115 @@ void SensorsComponentController::_appendStatusLog(const QString& text) @@ -84,89 +98,115 @@ void SensorsComponentController::_appendStatusLog(const QString& text)
Q_ARG(QVariant, varText));
}
void SensorsComponentController::_startCalibration(void)
void SensorsComponentController::_startLogCalibration(void)
{
_beginTextLogging();
_unknownFirmwareVersion = false;
_hideAllCalAreas();
connect(_uas, &UASInterface::textMessageReceived, this, &SensorsComponentController::_handleUASTextMessage);
_cancelButton->setEnabled(false);
}
void SensorsComponentController::_startVisualCalibration(void)
{
_compassButton->setEnabled(false);
_gyroButton->setEnabled(false);
_accelButton->setEnabled(false);
_airspeedButton->setEnabled(false);
_cancelButton->setEnabled(true);
_progressBar->setProperty("value", 0);
}
void SensorsComponentController::_stopCalibration(bool failed)
void SensorsComponentController::_stopCalibration(SensorsComponentController::StopCalibrationCode code)
{
_magCalInProgress = false;
_accelCalInProgress = false;
disconnect(_uas, &UASInterface::textMessageReceived, this, &SensorsComponentController::_handleUASTextMessage);
_compassButton->setEnabled(true);
_gyroButton->setEnabled(true);
_accelButton->setEnabled(true);
_airspeedButton->setEnabled(true);
_cancelButton->setEnabled(false);
if (code == StopCalibrationSuccess) {
_orientationCalDownSideDone = true;
_orientationCalUpsideDownSideDone = true;
_orientationCalLeftSideDone = true;
_orientationCalRightSideDone = true;
_orientationCalTailDownSideDone = true;
_orientationCalNoseDownSideDone = true;
_orientationCalDownSideInProgress = false;
_orientationCalUpsideDownSideInProgress = false;
_orientationCalLeftSideInProgress = false;
_orientationCalRightSideInProgress = false;
_orientationCalNoseDownSideInProgress = false;
_orientationCalTailDownSideInProgress = false;
emit orientationCalSidesDoneChanged();
emit orientationCalSidesInProgressChanged();
_progressBar->setProperty("value", 1);
} else {
_progressBar->setProperty("value", 0);
}
_progressBar->setProperty("value", failed ? 0 : 1);
_updateAndEmitGyroCalInProgress(false);
_waitingForCancel = false;
emit waitingForCancelChanged();
_refreshParams();
if (failed) {
QGCMessageBox::warning("Calibration", "Calibration failed. Calibration log will be displayed.");
_hideAllCalAreas();
switch (code) {
case StopCalibrationSuccess:
_orientationCalAreaHelpText->setProperty("text", "Calibration complete");
emit resetStatusTextArea();
if (_magCalInProgress) {
emit setCompassRotations();
}
break;
case StopCalibrationCancelled:
emit resetStatusTextArea();
_hideAllCalAreas();
break;
default:
// Assume failed
_hideAllCalAreas();
QGCMessageBox::warning("Calibration", "Calibration failed. Calibration log will be displayed.");
break;
}
_magCalInProgress = false;
_accelCalInProgress = false;
_gyroCalInProgress = false;
}
void SensorsComponentController::calibrateGyro(void)
{
_startCalibration();
UASInterface* uas = _autopilot->uas();
Q_ASSERT(uas);
uas->executeCommand(MAV_CMD_PREFLIGHT_CALIBRATION, 1, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0);
_startLogCalibration();
_uas->startCalibration(UASInterface::StartCalibrationGyro);
}
void SensorsComponentController::calibrateCompass(void)
{
_startCalibration();
UASInterface* uas = _autopilot->uas();
Q_ASSERT(uas);
uas->executeCommand(MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0);
_startLogCalibration();
_uas->startCalibration(UASInterface::StartCalibrationMag);
}
void SensorsComponentController::calibrateAccel(void)
{
_startCalibration();
UASInterface* uas = _autopilot->uas();
Q_ASSERT(uas);
uas->executeCommand(MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0);
_startLogCalibration();
_uas->startCalibration(UASInterface::StartCalibrationAccel);
}
void SensorsComponentController::calibrateAirspeed(void)
{
_startCalibration();
UASInterface* uas = _autopilot->uas();
Q_ASSERT(uas);
uas->executeCommand(MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0);
}
void SensorsComponentController::_beginTextLogging(void)
{
if (!_textLoggingStarted) {
_textLoggingStarted = true;
UASInterface* uas = _autopilot->uas();
Q_ASSERT(uas);
connect(uas, &UASInterface::textMessageReceived, this, &SensorsComponentController::_handleUASTextMessage);
}
_startLogCalibration();
_uas->startCalibration(UASInterface::StartCalibrationAirspeed);
}
void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
{
QString startingSidePrefix("Hold still, starting to measure ");
QString sideDoneSuffix(" side done, rotate to a different side");
QString orientationDetectedSuffix(" orientation detected");
Q_UNUSED(compId);
Q_UNUSED(severity);
@ -176,14 +216,6 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in @@ -176,14 +216,6 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in
return;
}
QStringList ignorePrefixList;
ignorePrefixList << "[cmd]" << "[mavlink pm]" << "[ekf check]" << "[pm]" << "[inav]" << "IN AIR MODE" << "LANDED MODE";
foreach (QString ignorePrefix, ignorePrefixList) {
if (text.startsWith(ignorePrefix)) {
return;
}
}
if (text.contains("progress <")) {
QString percent = text.split("<").last().split(">").first();
bool ok;
@ -196,138 +228,179 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in @@ -196,138 +228,179 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in
}
_appendStatusLog(text);
qDebug() << 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());
if (text == "gyro calibration: started") {
_updateAndEmitShowGyroCalArea(true);
_updateAndEmitGyroCalInProgress(true);
} else if (text == "accel calibration: started" || text == "mag calibration: started") {
if (text == "accel calibration: started") {
_accelCalInProgress = true;
_updateAndEmitCalInProgressText("Hold Still");
} else {
_updateAndEmitCalInProgressText("Rotate");
_magCalInProgress = true;
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() != 1) {
_unknownFirmwareVersion = true;
qDebug() << "Unknown cal firmware version, using log";
return;
}
_orientationCalDownSideDone = false;
_orientationCalUpsideDownSideDone = false;
_orientationCalLeftSideDone = false;
_orientationCalRightSideDone = false;
_orientationCalTailDownSideDone = false;
_orientationCalNoseDownSideDone = false;
_orientationCalDownSideInProgress = false;
_orientationCalUpsideDownSideInProgress = false;
_orientationCalLeftSideInProgress = false;
_orientationCalRightSideInProgress = false;
_orientationCalNoseDownSideInProgress = false;
_orientationCalTailDownSideInProgress = false;
emit orientationCalSidesDoneChanged();
emit orientationCalSidesInProgressChanged();
_updateAndEmitShowOrientationCalArea(true);
} else if (text.startsWith(startingSidePrefix)) {
QString side = text.right(text.length() - startingSidePrefix.length()).section(" ", 0, 0);
qDebug() << "Side started" << side;
if (side == "down") {
_orientationCalDownSideInProgress = true;
} else if (side == "up") {
_orientationCalUpsideDownSideInProgress = true;
} else if (side == "left") {
_orientationCalLeftSideInProgress = true;
} else if (side == "right") {
_orientationCalRightSideInProgress = true;
} else if (side == "front") {
_orientationCalNoseDownSideInProgress = true;
} else if (side == "back") {
_orientationCalTailDownSideInProgress = true;
_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", "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") {
_magCalInProgress = true;
_orientationCalDownSideVisible = true;
_orientationCalLeftSideVisible = true;
_orientationCalNoseDownSideVisible = true;
} else if (text == "gyro") {
_gyroCalInProgress = true;
_orientationCalDownSideVisible = true;
} else {
Q_ASSERT(false);
}
emit orientationCalSidesDoneChanged();
emit orientationCalSidesVisibleChanged();
emit orientationCalSidesInProgressChanged();
_updateAndEmitShowOrientationCalArea(true);
}
emit orientationCalSidesInProgressChanged();
} else if (text.endsWith(orientationDetectedSuffix)) {
return;
}
if (text.endsWith("orientation detected")) {
QString side = text.section(" ", 0, 0);
qDebug() << "Side started" << side;
if (side == "down") {
_orientationCalDownSideInProgress = true;
if (_magCalInProgress) {
_orientationCalDownSideRotate = true;
}
} else if (side == "up") {
_orientationCalUpsideDownSideInProgress = true;
} else if (side == "left") {
_orientationCalLeftSideInProgress = true;
if (_magCalInProgress) {
_orientationCalLeftSideRotate = true;
}
} else if (side == "right") {
_orientationCalRightSideInProgress = true;
} else if (side == "front") {
_orientationCalNoseDownSideInProgress = true;
if (_magCalInProgress) {
_orientationCalNoseDownSideRotate = true;
}
} else if (side == "back") {
_orientationCalTailDownSideInProgress = true;
}
if (_magCalInProgress) {
_orientationCalAreaHelpText->setProperty("text", "Rotate the vehicle continuously as shown in the diagram until marked as Completed");
} else {
_orientationCalAreaHelpText->setProperty("text", "Hold still in the current orientation");
}
emit orientationCalSidesInProgressChanged();
} else if (text.endsWith(sideDoneSuffix)) {
emit orientationCalSidesRotateChanged();
return;
}
if (text.endsWith("side done, rotate to a different side")) {
QString side = text.section(" ", 0, 0);
qDebug() << "Side finished" << side;
if (side == "down") {
_orientationCalDownSideInProgress = false;
_orientationCalDownSideDone = true;
_orientationCalDownSideRotate = false;
} else if (side == "up") {
_orientationCalUpsideDownSideInProgress = false;
_orientationCalUpsideDownSideDone = true;
} else if (side == "left") {
_orientationCalLeftSideInProgress = false;
_orientationCalLeftSideDone = true;
_orientationCalLeftSideRotate = false;
} else if (side == "right") {
_orientationCalRightSideInProgress = false;
_orientationCalRightSideDone = true;
} else if (side == "front") {
_orientationCalNoseDownSideInProgress = false;
_orientationCalNoseDownSideDone = true;
_orientationCalNoseDownSideRotate = false;
} else if (side == "back") {
_orientationCalTailDownSideInProgress = false;
_orientationCalTailDownSideDone = true;
}
_orientationCalAreaHelpText->setProperty("text", "Place you vehicle into one of the orientations shown below and hold it still");
emit orientationCalSidesInProgressChanged();
emit orientationCalSidesDoneChanged();
} else if (text == "accel calibration: done" || text == "mag calibration: done") {
_progressBar->setProperty("value", 1);
_orientationCalDownSideDone = true;
_orientationCalUpsideDownSideDone = true;
_orientationCalLeftSideDone = true;
_orientationCalRightSideDone = true;
_orientationCalTailDownSideDone = true;
_orientationCalNoseDownSideDone = true;
_orientationCalDownSideInProgress = false;
_orientationCalUpsideDownSideInProgress = false;
_orientationCalLeftSideInProgress = false;
_orientationCalRightSideInProgress = false;
_orientationCalNoseDownSideInProgress = false;
_orientationCalTailDownSideInProgress = false;
emit orientationCalSidesDoneChanged();
emit orientationCalSidesInProgressChanged();
_stopCalibration(false /* success */);
} else if (text == "gyro calibration: done") {
_stopCalibration(false /* success */);
} else if (text == "dpress calibration: done") {
_stopCalibration(false /* success */);
} else if (text.endsWith(" calibration: failed")) {
_stopCalibration(true /* failed */);
emit orientationCalSidesRotateChanged();
return;
}
}
void SensorsComponentController::_refreshParams(void)
{
// Pull specified params first so red/green indicators update quickly
_autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_MAG0_ID");
_autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_GYRO0_ID");
_autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_ACC0_ID");
_autopilot->refreshParameter(FactSystem::defaultComponentId, "SENS_DPRES_OFF");
_autopilot->refreshParameter(FactSystem::defaultComponentId, "SENS_BOARD_ROT");
// Mag rotation parameters are optional
if (_autopilot->parameterExists("CAL_MAG0_ROT")) {
_autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_MAG0_ROT");
}
if (_autopilot->parameterExists("CAL_MAG1_ROT")) {
_autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_MAG1_ROT");
QString calCompletePrefix("calibration done:");
if (text.startsWith(calCompletePrefix)) {
_stopCalibration(StopCalibrationSuccess);
return;
}
if (_autopilot->parameterExists("CAL_MAG2_ROT")) {
_autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_MAG2_ROT");
if (text.startsWith("calibration cancelled")) {
_stopCalibration(_waitingForCancel ? StopCalibrationCancelled : StopCalibrationFailed);
return;
}
if (text.startsWith("calibration failed")) {
_stopCalibration(StopCalibrationFailed);
return;
}
}
void SensorsComponentController::_refreshParams(void)
{
// Pull full set in order to get all cal values back
_autopilot->refreshAllParameters();
}
@ -339,18 +412,6 @@ bool SensorsComponentController::fixedWing(void) @@ -339,18 +412,6 @@ bool SensorsComponentController::fixedWing(void)
return uas->getSystemType() == MAV_TYPE_FIXED_WING;
}
void SensorsComponentController::_updateAndEmitGyroCalInProgress(bool inProgress)
{
_gyroCalInProgress = inProgress;
emit gyroCalInProgressChanged();
}
void SensorsComponentController::_updateAndEmitShowGyroCalArea(bool show)
{
_showGyroCalArea = show;
emit showGyroCalAreaChanged();
}
void SensorsComponentController::_updateAndEmitShowOrientationCalArea(bool show)
{
_showOrientationCalArea = show;
@ -359,12 +420,15 @@ void SensorsComponentController::_updateAndEmitShowOrientationCalArea(bool show) @@ -359,12 +420,15 @@ void SensorsComponentController::_updateAndEmitShowOrientationCalArea(bool show)
void SensorsComponentController::_hideAllCalAreas(void)
{
_updateAndEmitShowGyroCalArea(false);
_updateAndEmitShowOrientationCalArea(false);
}
void SensorsComponentController::_updateAndEmitCalInProgressText(const QString& text)
void SensorsComponentController::cancelCalibration(void)
{
_calInProgressText = text;
emit calInProgressTextChanged(text);
}
// The firmware doesn't allow us to cancel calibration. The best we can do is wait
// for it to timeout.
_waitingForCancel = true;
emit waitingForCancelChanged();
_cancelButton->setEnabled(false);
_uas->stopCalibration();
}

66
src/AutoPilotPlugins/PX4/SensorsComponentController.h
Unescape View File

@ -50,18 +50,11 @@ public: @@ -50,18 +50,11 @@ public:
Q_PROPERTY(QQuickItem* gyroButton MEMBER _gyroButton)
Q_PROPERTY(QQuickItem* accelButton MEMBER _accelButton)
Q_PROPERTY(QQuickItem* airspeedButton MEMBER _airspeedButton)
Q_PROPERTY(QQuickItem* cancelButton MEMBER _cancelButton)
Q_PROPERTY(QQuickItem* orientationCalAreaHelpText MEMBER _orientationCalAreaHelpText)
Q_PROPERTY(bool showCompass0 MEMBER _showCompass0 CONSTANT)
Q_PROPERTY(bool showCompass1 MEMBER _showCompass1 CONSTANT)
Q_PROPERTY(bool showCompass2 MEMBER _showCompass2 CONSTANT)
Q_PROPERTY(bool showGyroCalArea MEMBER _showGyroCalArea NOTIFY showGyroCalAreaChanged)
Q_PROPERTY(bool showOrientationCalArea MEMBER _showOrientationCalArea NOTIFY showOrientationCalAreaChanged)
Q_PROPERTY(bool gyroCalInProgress MEMBER _gyroCalInProgress NOTIFY gyroCalInProgressChanged)
Q_PROPERTY(QString calInProgressText MEMBER _calInProgressText NOTIFY calInProgressTextChanged)
Q_PROPERTY(bool orientationCalDownSideDone MEMBER _orientationCalDownSideDone NOTIFY orientationCalSidesDoneChanged)
Q_PROPERTY(bool orientationCalUpsideDownSideDone MEMBER _orientationCalUpsideDownSideDone NOTIFY orientationCalSidesDoneChanged)
Q_PROPERTY(bool orientationCalLeftSideDone MEMBER _orientationCalLeftSideDone NOTIFY orientationCalSidesDoneChanged)
@ -69,6 +62,13 @@ public: @@ -69,6 +62,13 @@ public:
Q_PROPERTY(bool orientationCalNoseDownSideDone MEMBER _orientationCalNoseDownSideDone NOTIFY orientationCalSidesDoneChanged)
Q_PROPERTY(bool orientationCalTailDownSideDone MEMBER _orientationCalTailDownSideDone NOTIFY orientationCalSidesDoneChanged)
Q_PROPERTY(bool orientationCalDownSideVisible MEMBER _orientationCalDownSideVisible NOTIFY orientationCalSidesVisibleChanged)
Q_PROPERTY(bool orientationCalUpsideDownSideVisible MEMBER _orientationCalUpsideDownSideVisible NOTIFY orientationCalSidesVisibleChanged)
Q_PROPERTY(bool orientationCalLeftSideVisible MEMBER _orientationCalLeftSideVisible NOTIFY orientationCalSidesVisibleChanged)
Q_PROPERTY(bool orientationCalRightSideVisible MEMBER _orientationCalRightSideVisible NOTIFY orientationCalSidesVisibleChanged)
Q_PROPERTY(bool orientationCalNoseDownSideVisible MEMBER _orientationCalNoseDownSideVisible NOTIFY orientationCalSidesVisibleChanged)
Q_PROPERTY(bool orientationCalTailDownSideVisible MEMBER _orientationCalTailDownSideVisible NOTIFY orientationCalSidesVisibleChanged)
Q_PROPERTY(bool orientationCalDownSideInProgress MEMBER _orientationCalDownSideInProgress NOTIFY orientationCalSidesInProgressChanged)
Q_PROPERTY(bool orientationCalUpsideDownSideInProgress MEMBER _orientationCalUpsideDownSideInProgress NOTIFY orientationCalSidesInProgressChanged)
Q_PROPERTY(bool orientationCalLeftSideInProgress MEMBER _orientationCalLeftSideInProgress NOTIFY orientationCalSidesInProgressChanged)
@ -76,37 +76,48 @@ public: @@ -76,37 +76,48 @@ public:
Q_PROPERTY(bool orientationCalNoseDownSideInProgress MEMBER _orientationCalNoseDownSideInProgress NOTIFY orientationCalSidesInProgressChanged)
Q_PROPERTY(bool orientationCalTailDownSideInProgress MEMBER _orientationCalTailDownSideInProgress NOTIFY orientationCalSidesInProgressChanged)
Q_PROPERTY(bool orientationCalDownSideRotate MEMBER _orientationCalDownSideRotate NOTIFY orientationCalSidesRotateChanged)
Q_PROPERTY(bool orientationCalLeftSideRotate MEMBER _orientationCalLeftSideRotate NOTIFY orientationCalSidesRotateChanged)
Q_PROPERTY(bool orientationCalNoseDownSideRotate MEMBER _orientationCalNoseDownSideRotate NOTIFY orientationCalSidesRotateChanged)
Q_PROPERTY(bool waitingForCancel MEMBER _waitingForCancel NOTIFY waitingForCancelChanged)
Q_INVOKABLE void calibrateCompass(void);
Q_INVOKABLE void calibrateGyro(void);
Q_INVOKABLE void calibrateAccel(void);
Q_INVOKABLE void calibrateAirspeed(void);
Q_INVOKABLE void cancelCalibration(void);
bool fixedWing(void);
signals:
void showGyroCalAreaChanged(void);
void showOrientationCalAreaChanged(void);
void gyroCalInProgressChanged(void);
void orientationCalSidesDoneChanged(void);
void orientationCalSidesVisibleChanged(void);
void orientationCalSidesInProgressChanged(void);
void calInProgressTextChanged(const QString& newText);
void orientationCalSidesRotateChanged(void);
void resetStatusTextArea(void);
void waitingForCancelChanged(void);
void setCompassRotations(void);
private slots:
void _handleUASTextMessage(int uasId, int compId, int severity, QString text);
private:
void _startCalibration(void);
void _stopCalibration(bool failed);
void _beginTextLogging(void);
void _startLogCalibration(void);
void _startVisualCalibration(void);
void _appendStatusLog(const QString& text);
void _refreshParams(void);
void _hideAllCalAreas(void);
void _updateAndEmitGyroCalInProgress(bool inProgress);
void _updateAndEmitCalInProgressText(const QString& text);
enum StopCalibrationCode {
StopCalibrationSuccess,
StopCalibrationFailed,
StopCalibrationCancelled
};
void _stopCalibration(StopCalibrationCode code);
void _updateAndEmitShowGyroCalArea(bool show);
void _updateAndEmitShowOrientationCalArea(bool show);
QQuickItem* _statusLog;
@ -115,6 +126,8 @@ private: @@ -115,6 +126,8 @@ private:
QQuickItem* _gyroButton;
QQuickItem* _accelButton;
QQuickItem* _airspeedButton;
QQuickItem* _cancelButton;
QQuickItem* _orientationCalAreaHelpText;
bool _showGyroCalArea;
bool _showOrientationCalArea;
@ -127,8 +140,6 @@ private: @@ -127,8 +140,6 @@ private:
bool _magCalInProgress;
bool _accelCalInProgress;
QString _calInProgressText;
bool _orientationCalDownSideDone;
bool _orientationCalUpsideDownSideDone;
bool _orientationCalLeftSideDone;
@ -136,6 +147,13 @@ private: @@ -136,6 +147,13 @@ private:
bool _orientationCalNoseDownSideDone;
bool _orientationCalTailDownSideDone;
bool _orientationCalDownSideVisible;
bool _orientationCalUpsideDownSideVisible;
bool _orientationCalLeftSideVisible;
bool _orientationCalRightSideVisible;
bool _orientationCalNoseDownSideVisible;
bool _orientationCalTailDownSideVisible;
bool _orientationCalDownSideInProgress;
bool _orientationCalUpsideDownSideInProgress;
bool _orientationCalLeftSideInProgress;
@ -143,9 +161,15 @@ private: @@ -143,9 +161,15 @@ private:
bool _orientationCalNoseDownSideInProgress;
bool _orientationCalTailDownSideInProgress;
bool _textLoggingStarted;
bool _orientationCalDownSideRotate;
bool _orientationCalLeftSideRotate;
bool _orientationCalNoseDownSideRotate;
bool _unknownFirmwareVersion;
bool _waitingForCancel;
AutoPilotPlugin* _autopilot;
UASInterface* _uas;
};
#endif

Write Preview
Loading…
Cancel
Save