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 {
calValid: controller.orientationCalDownSideDone
calInProgress: controller.orientationCalDownSideInProgress
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 {
width: parent.indicatorWidth
height: parent.indicatorHeight
visible: controller.orientationCalUpsideDownSideVisible
calValid: controller.orientationCalUpsideDownSideDone
calInProgress: controller.orientationCalUpsideDownSideInProgress
calInProgressText: controller.orientationCalUpsideDownSideRotate ? qsTr("Rotate") : qsTr("Hold Still")
imageSource: controller.orientationCalUpsideDownSideRotate ? "qrc:///qmlimages/VehicleUpsideDownRotate.png" : "qrc:///qmlimages/VehicleUpsideDown.png"
visible: controller.orientationCalLeftSideVisible
calValid: controller.orientationCalLeftSideDone
calInProgress: controller.orientationCalLeftSideInProgress
calInProgressText: controller.orientationCalLeftSideRotate ? qsTr("Rotate") : qsTr("Hold Still")
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 {
width: parent.indicatorWidth
......@@ -712,7 +721,7 @@ SetupPage {
calValid: controller.orientationCalNoseDownSideDone
calInProgress: controller.orientationCalNoseDownSideInProgress
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 {
width: parent.indicatorWidth
......@@ -721,25 +730,16 @@ SetupPage {
calValid: controller.orientationCalTailDownSideDone
calInProgress: controller.orientationCalTailDownSideInProgress
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 {
width: parent.indicatorWidth
height: parent.indicatorHeight
visible: controller.orientationCalLeftSideVisible
calValid: controller.orientationCalLeftSideDone
calInProgress: controller.orientationCalLeftSideInProgress
calInProgressText: controller.orientationCalLeftSideRotate ? qsTr("Rotate") : qsTr("Hold Still")
imageSource: controller.orientationCalLeftSideRotate ? "qrc:///qmlimages/VehicleLeftRotate.png" : "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: controller.orientationCalRightSideRotate ? "qrc:///qmlimages/VehicleRightRotate.png" : "qrc:///qmlimages/VehicleRight.png"
visible: controller.orientationCalUpsideDownSideVisible
calValid: controller.orientationCalUpsideDownSideDone
calInProgress: controller.orientationCalUpsideDownSideInProgress
calInProgressText: controller.orientationCalUpsideDownSideRotate ? qsTr("Rotate") : qsTr("Hold Still")
imageSource: "qrc:///qmlimages/VehicleUpsideDown.png"
}
}
}
......
src/AutoPilotPlugins/APM/APMSensorsComponentController.cc
View file @ b363fbe3
......@@ -227,7 +227,7 @@ void APMSensorsComponentController::_mavCommandResult(int vehicleId, int compone
_startLogCalibration();
uint8_t compassBits = 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;
qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 1";
} else {
......@@ -236,7 +236,7 @@ void APMSensorsComponentController::_mavCommandResult(int vehicleId, int compone
_rgCompassCalFitness[0] = 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;
qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 2";
} else {
......@@ -245,7 +245,7 @@ void APMSensorsComponentController::_mavCommandResult(int vehicleId, int compone
_rgCompassCalFitness[1] = 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;
qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 3";
} else {
......@@ -335,11 +335,118 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId,
return;
}
if (text.startsWith(QLatin1Literal("PreArm:")) || text.startsWith(QLatin1Literal("EKF"))
|| text.startsWith(QLatin1Literal("Arm")) || text.startsWith(QLatin1Literal("Initialising"))) {
QString originalMessageText = text;
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;
}
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 <"))) {
QString percent = text.split("<").last().split(">").first();
bool ok;
......@@ -533,6 +640,7 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId,
_stopCalibration(StopCalibrationFailed);
return;
}
#endif
}
void APMSensorsComponentController::_refreshParams(void)
......
src/Vehicle/Vehicle.cc
View file @ b363fbe3
......@@ -903,12 +903,16 @@ void Vehicle::_handleStatusText(mavlink_message_t& message, bool longVersion)
if (longVersion) {
b.resize(MAVLINK_MSG_STATUSTEXT_LONG_FIELD_TEXT_LEN+1);
mavlink_msg_statustext_long_get_text(&message, b.data());
severity = mavlink_msg_statustext_long_get_severity(&message);
mavlink_statustext_long_t statustextLong;
mavlink_msg_statustext_long_decode(&message, &statustextLong);
strncpy(b.data(), statustextLong.text, MAVLINK_MSG_STATUSTEXT_LONG_FIELD_TEXT_LEN);
severity = statustextLong.severity;
} else {
b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
mavlink_msg_statustext_get_text(&message, b.data());
severity = mavlink_msg_statustext_get_severity(&message);
mavlink_statustext_t statustext;
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';
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