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

Merge pull request #7418 from DonLakeFlyer/VisualCal 

ArduPilot: Visual accel cal
QGC4.4
Don Gagne 6 years ago committed by GitHub
parent
ada66ceb28 083ae31002
commit
b363fbe3d0
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 143 additions and 31 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. 44
      src/AutoPilotPlugins/APM/APMSensorsComponent.qml
  2. 118
      src/AutoPilotPlugins/APM/APMSensorsComponentController.cc
  3. 12
      src/Vehicle/Vehicle.cc

44
src/AutoPilotPlugins/APM/APMSensorsComponent.qml
Unescape View File

@ -694,16 +694,25 @@ SetupPage { @@ -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 { @@ -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 { @@ -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"
}
}
}

118
src/AutoPilotPlugins/APM/APMSensorsComponentController.cc
Unescape View File

@ -227,7 +227,7 @@ void APMSensorsComponentController::_mavCommandResult(int vehicleId, int compone @@ -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 @@ -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 @@ -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, @@ -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, @@ -533,6 +640,7 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId,
_stopCalibration(StopCalibrationFailed);
return;
}
#endif
}
void APMSensorsComponentController::_refreshParams(void)

12
src/Vehicle/Vehicle.cc
Unescape View File

@ -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) {
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);

Write Preview
Loading…
Cancel
Save