diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponent.cc b/src/AutoPilotPlugins/APM/APMSensorsComponent.cc index 9e25a183147cb49450b95d4b7beff59b60e2dc34..8372b423764b042f6d0caf26873be588f527f773 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponent.cc +++ b/src/AutoPilotPlugins/APM/APMSensorsComponent.cc @@ -72,16 +72,7 @@ QStringList APMSensorsComponent::setupCompleteChangedTriggerList(void) const << "COMPASS_OFS3_X" << "COMPASS_OFS3_X" << "COMPASS_OFS3_X"; // Accelerometer triggers - if (_autopilot->parameterExists(FactSystem::defaultComponentId, "INS_USE")) { - triggers << "INS_USE" << "INS_USE2" << "INS_USE3" - << "INS_ACCOFFS_X" << "INS_ACCOFFS_Y" << "INS_ACCOFFS_Z" - << "INS_ACC2OFFS_X" << "INS_ACC2OFFS_Y" << "INS_ACC2OFFS_Z" - << "INS_ACC3OFFS_X" << "INS_ACC3OFFS_Y" << "INS_ACC3OFFS_Z"; - } else { - // For older firmwares which don't support the INS_USE parameter we can't determine which secondary accels are in use. - // So we just base things off the the first accel. - triggers << "INS_ACCOFFS_X" << "INS_ACCOFFS_Y" << "INS_ACCOFFS_Z"; - } + triggers << "INS_ACCOFFS_X" << "INS_ACCOFFS_Y" << "INS_ACCOFFS_Z"; return triggers; } @@ -116,9 +107,9 @@ bool APMSensorsComponent::compassSetupNeeded(void) const QStringList rgOffsets[cCompass]; devicesIds << QStringLiteral("COMPASS_DEV_ID") << QStringLiteral("COMPASS_DEV_ID2") << QStringLiteral("COMPASS_DEV_ID3"); - rgOffsets[0] << QStringLiteral("COMPASS_OFS_X") << QStringLiteral("COMPASS_OFS_X") << QStringLiteral("COMPASS_OFS_X"); - rgOffsets[1] << QStringLiteral("COMPASS_OFS2_X") << QStringLiteral("COMPASS_OFS2_X") << QStringLiteral("COMPASS_OFS2_X"); - rgOffsets[2] << QStringLiteral("COMPASS_OFS3_X") << QStringLiteral("COMPASS_OFS3_X") << QStringLiteral("COMPASS_OFS3_X"); + rgOffsets[0] << QStringLiteral("COMPASS_OFS_X") << QStringLiteral("COMPASS_OFS_Y") << QStringLiteral("COMPASS_OFS_Z"); + rgOffsets[1] << QStringLiteral("COMPASS_OFS2_X") << QStringLiteral("COMPASS_OFS2_Y") << QStringLiteral("COMPASS_OFS2_Z"); + rgOffsets[2] << QStringLiteral("COMPASS_OFS3_X") << QStringLiteral("COMPASS_OFS3_Y") << QStringLiteral("COMPASS_OFS3_Z"); for (size_t i=0; igetParameterFact(FactSystem::defaultComponentId, devicesIds[i])->rawValue().toInt() != 0) { @@ -135,29 +126,13 @@ bool APMSensorsComponent::compassSetupNeeded(void) const bool APMSensorsComponent::accelSetupNeeded(void) const { - const size_t cAccel = 3; - const size_t cOffset = 3; - QStringList insUse; - QStringList rgOffsets[cAccel]; - - if (_autopilot->parameterExists(FactSystem::defaultComponentId, "INS_USE")) { - insUse << QStringLiteral("INS_USE") << QStringLiteral("INS_USE2") << QStringLiteral("INS_USE3"); - rgOffsets[0] << QStringLiteral("INS_ACCOFFS_X") << QStringLiteral("INS_ACCOFFS_Y") << QStringLiteral("INS_ACCOFFS_Z"); - rgOffsets[1] << QStringLiteral("INS_ACC2OFFS_X") << QStringLiteral("INS_ACC2OFFS_Y") << QStringLiteral("INS_ACC2OFFS_Z"); - rgOffsets[2] << QStringLiteral("INS_ACC3OFFS_X") << QStringLiteral("INS_ACC3OFFS_Y") << QStringLiteral("INS_ACC3OFFS_Z"); - } else { - // For older firmwares which don't support the INS_USE parameter we can't determine which secondary accels are in use. - // So we just base things off the the first accel. - rgOffsets[0] << QStringLiteral("INS_ACCOFFS_X") << QStringLiteral("INS_ACCOFFS_Y") << QStringLiteral("INS_ACCOFFS_Z"); - } + QStringList offsets; - for (size_t i=0; igetParameterFact(FactSystem::defaultComponentId, insUse[i])->rawValue().toInt() != 0) { - for (size_t j=0; jgetParameterFact(FactSystem::defaultComponentId, rgOffsets[i][j])->rawValue().toFloat() == 0.0f) { - return true; - } - } + offsets << QStringLiteral("INS_ACCOFFS_X") << QStringLiteral("INS_ACCOFFS_Y") << QStringLiteral("INS_ACCOFFS_Z"); + + foreach(const QString& offset, offsets) { + if (_autopilot->getParameterFact(FactSystem::defaultComponentId, offset)->rawValue().toFloat() == 0.0f) { + return true; } } diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponent.qml b/src/AutoPilotPlugins/APM/APMSensorsComponent.qml index 708b809febc0ea1a2f6e0585044ad5cf7c5cf3ee..2723d135d529aa39ced6ec0688a82b1dbba36839 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponent.qml +++ b/src/AutoPilotPlugins/APM/APMSensorsComponent.qml @@ -369,6 +369,19 @@ QGCView { QGCLabel { text: "Calibrate:"; anchors.baseline: compassButton.baseline } + IndicatorButton { + id: accelButton + width: parent.buttonWidth + text: "Accelerometer" + indicatorGreen: !accelCalNeeded + + onClicked: { + preCalibrationDialogType = "accel" + preCalibrationDialogHelp = accelHelp + showDialog(preCalibrationDialogComponent, "Calibrate Accelerometer", qgcView.showDialogDefaultWidth, StandardButton.Cancel | StandardButton.Ok) + } + } + IndicatorButton { id: compassButton width: parent.buttonWidth @@ -388,18 +401,6 @@ QGCView { } } - IndicatorButton { - id: accelButton - width: parent.buttonWidth - text: "Accelerometer" - indicatorGreen: !accelCalNeeded - - onClicked: { - preCalibrationDialogType = "accel" - preCalibrationDialogHelp = accelHelp - showDialog(preCalibrationDialogComponent, "Calibrate Accelerometer", qgcView.showDialogDefaultWidth, StandardButton.Cancel | StandardButton.Ok) - } - } QGCButton { id: nextButton showBorder: true diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc index f2639e531c6d86a118b329ce61fe4efb467459b0..d71b536fb1038c38f0c0ba94fc898205a625eca1 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc @@ -77,11 +77,6 @@ APMSensorsComponentController::APMSensorsComponentController(void) : connect(_sensorsComponent, &VehicleComponent::setupCompleteChanged, this, &APMSensorsComponentController::setupNeededChanged); } -APMSensorsComponentController::~APMSensorsComponentController() -{ - _vehicle->setConnectionLostEnabled(true); -} - /// Appends the specified text to the status log area in the ui void APMSensorsComponentController::_appendStatusLog(const QString& text) { diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.h b/src/AutoPilotPlugins/APM/APMSensorsComponentController.h index dabdba5239ab58fa3a8e985f7fea4e128ae06a52..7af291585e39842ac584a9b72346d797a88caca2 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.h +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentController.h @@ -41,7 +41,6 @@ class APMSensorsComponentController : public FactPanelController public: APMSensorsComponentController(void); - ~APMSensorsComponentController(); Q_PROPERTY(bool fixedWing READ fixedWing CONSTANT) diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentSummary.qml b/src/AutoPilotPlugins/APM/APMSensorsComponentSummary.qml index 75902f55147be8fc21855b1aeb5fed2fd509514d..cb9014b684b53f7f462c7326c0dd683f95cdf174 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentSummary.qml +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentSummary.qml @@ -20,7 +20,28 @@ FactPanel { QGCPalette { id: qgcPal; colorGroupEnabled: enabled } APMSensorsComponentController { id: controller; factPanel: panel } - property bool accelCalNeeded: controller.accelSetupNeeded + property Fact compass1IdFact: controller.getParameterFact(-1, "COMPASS_DEV_ID") + property Fact compass2IdFact: controller.getParameterFact(-1, "COMPASS_DEV_ID2") + property Fact compass3IdFact: controller.getParameterFact(-1, "COMPASS_DEV_ID3") + + property Fact compass1OfsXFact: controller.getParameterFact(-1, "COMPASS_OFS_X") + property Fact compass1OfsYFact: controller.getParameterFact(-1, "COMPASS_OFS_Y") + property Fact compass1OfsZFact: controller.getParameterFact(-1, "COMPASS_OFS_Z") + property Fact compass2OfsXFact: controller.getParameterFact(-1, "COMPASS_OFS2_X") + property Fact compass2OfsYFact: controller.getParameterFact(-1, "COMPASS_OFS2_Y") + property Fact compass2OfsZFact: controller.getParameterFact(-1, "COMPASS_OFS2_Z") + property Fact compass3OfsXFact: controller.getParameterFact(-1, "COMPASS_OFS3_X") + property Fact compass3OfsYFact: controller.getParameterFact(-1, "COMPASS_OFS3_Y") + property Fact compass3OfsZFact: controller.getParameterFact(-1, "COMPASS_OFS3_Z") + + property bool compass1Available: compass1IdFact.value !== 0 + property bool compass2Available: compass2IdFact.value !== 0 + property bool compass3Available: compass3IdFact.value !== 0 + + property bool compass1Calibrated: compass1Available ? compass1OfsXFact.value != 0.0 && compass1OfsYFact.value != 0.0 &&compass1OfsZFact.value != 0.0 : false + property bool compass2Calibrated: compass2Available ? compass2OfsXFact.value != 0.0 && compass2OfsYFact.value != 0.0 &&compass2OfsZFact.value != 0.0 : false + property bool compass3Calibrated: compass3Available ? compass3OfsXFact.value != 0.0 && compass3OfsYFact.value != 0.0 &&compass3OfsZFact.value != 0.0 : false + property bool compassCalNeeded: controller.compassSetupNeeded Column { @@ -28,13 +49,26 @@ FactPanel { anchors.margins: 8 VehicleSummaryRow { - labelText: "Compass:" - valueText: compassCalNeeded ? "Setup required" : "Ready" + labelText: "Compass 1:" + visible: compass1Available + valueText: compass1Calibrated ? "Ready" : "Setup required" + } + + VehicleSummaryRow { + labelText: "Compass 2:" + visible: compass2Available + valueText: compass2Calibrated ? "Ready" : "Setup required" + } + + VehicleSummaryRow { + labelText: "Compass 3:" + visible: compass3Available + valueText: compass3Calibrated ? "Ready" : "Setup required" } VehicleSummaryRow { labelText: "Accelerometer:" - valueText: accelCalNeeded ? "Setup required" : "Ready" + valueText: controller.accelSetupNeeded ? "Setup required" : "Ready" } } } diff --git a/src/AutoPilotPlugins/PX4/SensorsComponent.qml b/src/AutoPilotPlugins/PX4/SensorsComponent.qml index c6f74b77009d9e13c557b4cab6f64797b7649bc6..a6d38f25aeb73fb64e07ecd7962c199b90969038 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponent.qml +++ b/src/AutoPilotPlugins/PX4/SensorsComponent.qml @@ -130,7 +130,7 @@ QGCView { onSetCompassRotations: { if (showCompass0Rot || showCompass1Rot || showCompass2Rot) { - showDialog(compassRotationDialogComponent, "Set Compass Rotation(s)", qgcView.showDialogDefaultWidth, StandardButton.Ok) + showDialog(compassRotationDialogComponent, "Set Compass Rotation(s)", qgcView.showDialogDefaultWidth, StandardButton.Ok) } } diff --git a/src/AutoPilotPlugins/PX4/SensorsComponentSummary.qml b/src/AutoPilotPlugins/PX4/SensorsComponentSummary.qml index 2fd15c11c41e9816f878373b91a5568018379da8..564f0c8fac8493b88c10da17483f1830ab6efa05 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponentSummary.qml +++ b/src/AutoPilotPlugins/PX4/SensorsComponentSummary.qml @@ -20,6 +20,8 @@ FactPanel { FactPanelController { id: controller; factPanel: panel } property Fact mag0IdFact: controller.getParameterFact(-1, "CAL_MAG0_ID") + property Fact mag1IdFact: controller.getParameterFact(-1, "CAL_MAG1_ID") + property Fact mag2IdFact: controller.getParameterFact(-1, "CAL_MAG2_ID") property Fact gyro0IdFact: controller.getParameterFact(-1, "CAL_GYRO0_ID") property Fact accel0IdFact: controller.getParameterFact(-1, "CAL_ACC0_ID") @@ -28,10 +30,22 @@ FactPanel { anchors.margins: 8 VehicleSummaryRow { - labelText: "Compass:" + labelText: "Compass 0:" valueText: mag0IdFact ? (mag0IdFact.value === 0 ? "Setup required" : "Ready") : "" } + VehicleSummaryRow { + labelText: "Compass 1:" + visible: mag1IdFact.value !== 0 + valueText: "Ready" + } + + VehicleSummaryRow { + labelText: "Compass 2:" + visible: mag2IdFact.value !== 0 + valueText: "Ready" + } + VehicleSummaryRow { labelText: "Gyro:" valueText: gyro0IdFact ? (gyro0IdFact.value === 0 ? "Setup required" : "Ready") : ""