Skip to content

Q
qgroundcontrol
Unverified Commit b363fbe3 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #7418 from DonLakeFlyer/VisualCal 

ArduPilot: Visual accel cal
parents ada66ceb 083ae310
Hide whitespace changes
Inline Side-by-side
Showing with 143 additions and 31 deletions
src/AutoPilotPlugins/APM/APMSensorsComponent.qml
View file @ b363fbe3
...@@ -694,16 +694,25 @@ SetupPage { ...@@ -694,16 +694,25 @@ SetupPage {
calValid: controller.orientationCalDownSideDone calValid: controller.orientationCalDownSideDone
calInProgress: controller.orientationCalDownSideInProgress calInProgress: controller.orientationCalDownSideInProgress
calInProgressText: controller.orientationCalDownSideRotate ? qsTr("Rotate") : qsTr("Hold Still") calInProgressText: controller.orientationCalDownSideRotate ? qsTr("Rotate") : qsTr("Hold Still")
imageSource: controller.orientationCalDownSideRotate ? "qrc:///qmlimages/VehicleDownRotate.png" : "qrc:///qmlimages/VehicleDown.png" imageSource: "qrc:///qmlimages/VehicleDown.png"
} }
VehicleRotationCal { VehicleRotationCal {
width: parent.indicatorWidth width: parent.indicatorWidth
height: parent.indicatorHeight height: parent.indicatorHeight
visible: controller.orientationCalUpsideDownSideVisible visible: controller.orientationCalLeftSideVisible
calValid: controller.orientationCalUpsideDownSideDone calValid: controller.orientationCalLeftSideDone
calInProgress: controller.orientationCalUpsideDownSideInProgress calInProgress: controller.orientationCalLeftSideInProgress
calInProgressText: controller.orientationCalUpsideDownSideRotate ? qsTr("Rotate") : qsTr("Hold Still") calInProgressText: controller.orientationCalLeftSideRotate ? qsTr("Rotate") : qsTr("Hold Still")
imageSource: controller.orientationCalUpsideDownSideRotate ? "qrc:///qmlimages/VehicleUpsideDownRotate.png" : "qrc:///qmlimages/VehicleUpsideDown.png" imageSource: "qrc:///qmlimages/VehicleLeft.png"
}
VehicleRotationCal {
width: parent.indicatorWidth
height: parent.indicatorHeight
visible: controller.orientationCalRightSideVisible
calValid: controller.orientationCalRightSideDone
calInProgress: controller.orientationCalRightSideInProgress
calInProgressText: controller.orientationCalRightSideRotate ? qsTr("Rotate") : qsTr("Hold Still")
imageSource: "qrc:///qmlimages/VehicleRight.png"
} }
VehicleRotationCal { VehicleRotationCal {
width: parent.indicatorWidth width: parent.indicatorWidth
...@@ -712,7 +721,7 @@ SetupPage { ...@@ -712,7 +721,7 @@ SetupPage {
calValid: controller.orientationCalNoseDownSideDone calValid: controller.orientationCalNoseDownSideDone
calInProgress: controller.orientationCalNoseDownSideInProgress calInProgress: controller.orientationCalNoseDownSideInProgress
calInProgressText: controller.orientationCalNoseDownSideRotate ? qsTr("Rotate") : qsTr("Hold Still") calInProgressText: controller.orientationCalNoseDownSideRotate ? qsTr("Rotate") : qsTr("Hold Still")
imageSource: controller.orientationCalNoseDownSideRotate ? "qrc:///qmlimages/VehicleNoseDownRotate.png" : "qrc:///qmlimages/VehicleNoseDown.png" imageSource: "qrc:///qmlimages/VehicleNoseDown.png"
} }
VehicleRotationCal { VehicleRotationCal {
width: parent.indicatorWidth width: parent.indicatorWidth
...@@ -721,25 +730,16 @@ SetupPage { ...@@ -721,25 +730,16 @@ SetupPage {
calValid: controller.orientationCalTailDownSideDone calValid: controller.orientationCalTailDownSideDone
calInProgress: controller.orientationCalTailDownSideInProgress calInProgress: controller.orientationCalTailDownSideInProgress
calInProgressText: controller.orientationCalTailDownSideRotate ? qsTr("Rotate") : qsTr("Hold Still") calInProgressText: controller.orientationCalTailDownSideRotate ? qsTr("Rotate") : qsTr("Hold Still")
imageSource: controller.orientationCalTailDownSideRotate ? "qrc:///qmlimages/VehicleTailDownRotate.png" : "qrc:///qmlimages/VehicleTailDown.png" imageSource: "qrc:///qmlimages/VehicleTailDown.png"
} }
VehicleRotationCal { VehicleRotationCal {
width: parent.indicatorWidth width: parent.indicatorWidth
height: parent.indicatorHeight height: parent.indicatorHeight
visible: controller.orientationCalLeftSideVisible visible: controller.orientationCalUpsideDownSideVisible
calValid: controller.orientationCalLeftSideDone calValid: controller.orientationCalUpsideDownSideDone
calInProgress: controller.orientationCalLeftSideInProgress calInProgress: controller.orientationCalUpsideDownSideInProgress
calInProgressText: controller.orientationCalLeftSideRotate ? qsTr("Rotate") : qsTr("Hold Still") calInProgressText: controller.orientationCalUpsideDownSideRotate ? qsTr("Rotate") : qsTr("Hold Still")
imageSource: controller.orientationCalLeftSideRotate ? "qrc:///qmlimages/VehicleLeftRotate.png" : "qrc:///qmlimages/VehicleLeft.png" imageSource: "qrc:///qmlimages/VehicleUpsideDown.png"
}
VehicleRotationCal {
width: parent.indicatorWidth
height: parent.indicatorHeight
visible: controller.orientationCalRightSideVisible
calValid: controller.orientationCalRightSideDone
calInProgress: controller.orientationCalRightSideInProgress
calInProgressText: controller.orientationCalRightSideRotate ? qsTr("Rotate") : qsTr("Hold Still")
imageSource: controller.orientationCalRightSideRotate ? "qrc:///qmlimages/VehicleRightRotate.png" : "qrc:///qmlimages/VehicleRight.png"
} }
} }
} }
......
src/AutoPilotPlugins/APM/APMSensorsComponentController.cc
View file @ b363fbe3
...@@ -227,7 +227,7 @@ void APMSensorsComponentController::_mavCommandResult(int vehicleId, int compone ...@@ -227,7 +227,7 @@ void APMSensorsComponentController::_mavCommandResult(int vehicleId, int compone
_startLogCalibration(); _startLogCalibration();
uint8_t compassBits = 0; uint8_t compassBits = 0;
if (getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_DEV_ID"))->rawValue().toInt() > 0 && if (getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_DEV_ID"))->rawValue().toInt() > 0 &&
getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_USE"))->rawValue().toBool()) { getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_USE"))->rawValue().toBool()) {
compassBits |= 1 << 0; compassBits |= 1 << 0;
qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 1"; qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 1";
} else { } else {
...@@ -236,7 +236,7 @@ void APMSensorsComponentController::_mavCommandResult(int vehicleId, int compone ...@@ -236,7 +236,7 @@ void APMSensorsComponentController::_mavCommandResult(int vehicleId, int compone
_rgCompassCalFitness[0] = 0; _rgCompassCalFitness[0] = 0;
} }
if (getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_DEV_ID2"))->rawValue().toInt() > 0 && if (getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_DEV_ID2"))->rawValue().toInt() > 0 &&
getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_USE2"))->rawValue().toBool()) { getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_USE2"))->rawValue().toBool()) {
compassBits |= 1 << 1; compassBits |= 1 << 1;
qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 2"; qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 2";
} else { } else {
...@@ -245,7 +245,7 @@ void APMSensorsComponentController::_mavCommandResult(int vehicleId, int compone ...@@ -245,7 +245,7 @@ void APMSensorsComponentController::_mavCommandResult(int vehicleId, int compone
_rgCompassCalFitness[1] = 0; _rgCompassCalFitness[1] = 0;
} }
if (getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_DEV_ID3"))->rawValue().toInt() > 0 && if (getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_DEV_ID3"))->rawValue().toInt() > 0 &&
getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_USE3"))->rawValue().toBool()) { getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_USE3"))->rawValue().toBool()) {
compassBits |= 1 << 2; compassBits |= 1 << 2;
qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 3"; qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 3";
} else { } else {
...@@ -335,11 +335,118 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId, ...@@ -335,11 +335,118 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId,
return; return;
} }
if (text.startsWith(QLatin1Literal("PreArm:")) || text.startsWith(QLatin1Literal("EKF")) QString originalMessageText = text;
|| text.startsWith(QLatin1Literal("Arm")) || text.startsWith(QLatin1Literal("Initialising"))) { text = text.toLower();
QStringList hidePrefixList = { QStringLiteral("prearm:"), QStringLiteral("ekf"), QStringLiteral("arm"), QStringLiteral("initialising") };
for (const QString& hidePrefix: hidePrefixList) {
if (text.startsWith(hidePrefix)) {
return;
}
}
if (_calTypeInProgress == CalTypeAccel) {
if (text == QStringLiteral("place vehicle level and press any key.")) {
_startVisualCalibration();
_cancelButton->setEnabled(false);
// 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;
_calTypeInProgress = CalTypeAccel;
_orientationCalDownSideVisible = true;
_orientationCalUpsideDownSideVisible = true;
_orientationCalLeftSideVisible = true;
_orientationCalRightSideVisible = true;
_orientationCalTailDownSideVisible = true;
_orientationCalNoseDownSideVisible = true;
emit orientationCalSidesDoneChanged();
emit orientationCalSidesVisibleChanged();
emit orientationCalSidesInProgressChanged();
_updateAndEmitShowOrientationCalArea(true);
}
QString placeVehicle("place vehicle ");
if (_calTypeInProgress == CalTypeAccel && text.startsWith(placeVehicle)) {
text = text.right(text.length() - placeVehicle.length());
if (text.startsWith("level")) {
_orientationCalDownSideInProgress = true;
_nextButton->setEnabled(true);
} else if (text.startsWith("on its left")) {
_orientationCalDownSideDone = true;
_orientationCalDownSideInProgress = false;
_orientationCalLeftSideInProgress = true;
_progressBar->setProperty("value", (qreal)(17 / 100.0));
} else if (text.startsWith("on its right")) {
_orientationCalLeftSideDone = true;
_orientationCalLeftSideInProgress = false;
_orientationCalRightSideInProgress = true;
_progressBar->setProperty("value", (qreal)(34 / 100.0));
} else if (text.startsWith("nose down")) {
_orientationCalRightSideDone = true;
_orientationCalRightSideInProgress = false;
_orientationCalNoseDownSideInProgress = true;
_progressBar->setProperty("value", (qreal)(51 / 100.0));
} else if (text.startsWith("nose up")) {
_orientationCalNoseDownSideDone = true;
_orientationCalNoseDownSideInProgress = false;
_orientationCalTailDownSideInProgress = true;
_progressBar->setProperty("value", (qreal)(68 / 100.0));
} else if (text.startsWith("on its back")) {
_orientationCalTailDownSideDone = true;
_orientationCalTailDownSideInProgress = false;
_orientationCalUpsideDownSideInProgress = true;
_progressBar->setProperty("value", (qreal)(85 / 100.0));
}
_orientationCalAreaHelpText->setProperty("text", tr("Hold still in the current orientation and press Next when ready"));
emit orientationCalSidesDoneChanged();
emit orientationCalSidesInProgressChanged();
emit orientationCalSidesRotateChanged();
}
}
_appendStatusLog(originalMessageText);
qCDebug(APMSensorsComponentControllerLog) << originalMessageText << severity;
if (text.contains(QLatin1String("calibration successful"))) {
_stopCalibration(StopCalibrationSuccess);
return; return;
} }
if (text.startsWith(QStringLiteral("calibration cancelled"))) {
_stopCalibration(_waitingForCancel ? StopCalibrationCancelled : StopCalibrationFailed);
return;
}
if (text.startsWith(QStringLiteral("calibration failed"))) {
_stopCalibration(StopCalibrationFailed);
return;
}
#if 0
if (text.contains(QLatin1Literal("progress <"))) { if (text.contains(QLatin1Literal("progress <"))) {
QString percent = text.split("<").last().split(">").first(); QString percent = text.split("<").last().split(">").first();
bool ok; bool ok;
...@@ -533,6 +640,7 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId, ...@@ -533,6 +640,7 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId,
_stopCalibration(StopCalibrationFailed); _stopCalibration(StopCalibrationFailed);
return; return;
} }
#endif
} }
void APMSensorsComponentController::_refreshParams(void) void APMSensorsComponentController::_refreshParams(void)
......
src/Vehicle/Vehicle.cc
View file @ b363fbe3
...@@ -903,12 +903,16 @@ void Vehicle::_handleStatusText(mavlink_message_t& message, bool longVersion) ...@@ -903,12 +903,16 @@ void Vehicle::_handleStatusText(mavlink_message_t& message, bool longVersion)
if (longVersion) { if (longVersion) {
b.resize(MAVLINK_MSG_STATUSTEXT_LONG_FIELD_TEXT_LEN+1); b.resize(MAVLINK_MSG_STATUSTEXT_LONG_FIELD_TEXT_LEN+1);
mavlink_msg_statustext_long_get_text(&message, b.data()); mavlink_statustext_long_t statustextLong;
severity = mavlink_msg_statustext_long_get_severity(&message); mavlink_msg_statustext_long_decode(&message, &statustextLong);
strncpy(b.data(), statustextLong.text, MAVLINK_MSG_STATUSTEXT_LONG_FIELD_TEXT_LEN);
severity = statustextLong.severity;
} else { } else {
b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1); b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
mavlink_msg_statustext_get_text(&message, b.data()); mavlink_statustext_t statustext;
severity = mavlink_msg_statustext_get_severity(&message); mavlink_msg_statustext_decode(&message, &statustext);
strncpy(b.data(), statustext.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
severity = statustext.severity;
} }
b[b.length()-1] = '\0'; b[b.length()-1] = '\0';
messageText = QString(b); messageText = QString(b);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment