diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponent.qml b/src/AutoPilotPlugins/APM/APMSensorsComponent.qml index dad8815f0e4d9559165d2f623b4062506bb6996e..dee0be5f1776d0c44d0fb6bbff6e451aaa497f76 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponent.qml +++ b/src/AutoPilotPlugins/APM/APMSensorsComponent.qml @@ -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" } } } diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc index adc0019f3ddc93cf75cfcdccbe6521d6043f6d4f..1d45eb05abbbb949b9489ab5e723147db7f5eb36 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc @@ -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) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index c366dd45913fd8bd409f2960fe9047453d79e200..791650d862670e3418c1d8aa1414160c8318585b 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -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);