diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponent.cc b/src/AutoPilotPlugins/APM/APMSensorsComponent.cc index da9ed08a5e57a75deecddfa0db0fefa734c0181a..7df8c28697882415856eee67935e01d63f549c6b 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponent.cc +++ b/src/AutoPilotPlugins/APM/APMSensorsComponent.cc @@ -57,13 +57,7 @@ bool APMSensorsComponent::requiresSetup(void) const bool APMSensorsComponent::setupComplete(void) const { - foreach(QString triggerParam, setupCompleteChangedTriggerList()) { - if (_autopilot->getParameterFact(FactSystem::defaultComponentId, triggerParam)->rawValue().toFloat() == 0.0f) { - return false; - } - } - - return true; + return !compassSetupNeeded() && !accelSetupNeeded(); } QString APMSensorsComponent::setupStateDescription(void) const @@ -82,8 +76,17 @@ QStringList APMSensorsComponent::setupCompleteChangedTriggerList(void) const { QStringList triggers; - triggers << "COMPASS_OFS_X" << "COMPASS_OFS_X" << "COMPASS_OFS_X" - << "INS_ACCOFFS_X" << "INS_ACCOFFS_Y" << "INS_ACCOFFS_Z"; + // Compass triggers + triggers << "COMPASS_DEV_ID" << "COMPASS_DEV_ID2" << "COMPASS_DEV_ID3" + << "COMPASS_OFS_X" << "COMPASS_OFS_X" << "COMPASS_OFS_X" + << "COMPASS_OFS2_X" << "COMPASS_OFS2_X" << "COMPASS_OFS2_X" + << "COMPASS_OFS3_X" << "COMPASS_OFS3_X" << "COMPASS_OFS3_X"; + + // Acceleromter triggers + 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"; return triggers; } @@ -114,3 +117,54 @@ QString APMSensorsComponent::prerequisiteSetup(void) const return QString(); } + +bool APMSensorsComponent::compassSetupNeeded(void) const +{ + const size_t cCompass = 3; + const size_t cOffset = 3; + QStringList devicesIds; + QStringList rgOffsets[cCompass]; + + devicesIds << "COMPASS_DEV_ID" << "COMPASS_DEV_ID2" << "COMPASS_DEV_ID3"; + rgOffsets[0] << "COMPASS_OFS_X" << "COMPASS_OFS_X" << "COMPASS_OFS_X"; + rgOffsets[1] << "COMPASS_OFS2_X" << "COMPASS_OFS2_X" << "COMPASS_OFS2_X"; + rgOffsets[2] << "COMPASS_OFS3_X" << "COMPASS_OFS3_X" << "COMPASS_OFS3_X"; + + for (size_t i=0; igetParameterFact(FactSystem::defaultComponentId, devicesIds[i])->rawValue().toInt() != 0) { + for (size_t j=0; jgetParameterFact(FactSystem::defaultComponentId, rgOffsets[i][j])->rawValue().toFloat() == 0.0f) { + return true; + } + } + } + } + + return false; +} + +bool APMSensorsComponent::accelSetupNeeded(void) const +{ + const size_t cAccel = 3; + const size_t cOffset = 3; + QStringList insUse; + QStringList rgOffsets[cAccel]; + + insUse << "INS_USE" << "INS_USE2" << "INS_USE3"; + rgOffsets[0] << "INS_ACCOFFS_X" << "INS_ACCOFFS_Y" << "INS_ACCOFFS_Z"; + rgOffsets[1] << "INS_ACC2OFFS_X" << "INS_ACC2OFFS_Y" << "INS_ACC2OFFS_Z"; + rgOffsets[2] << "INS_ACC3OFFS_X" << "INS_ACC3OFFS_Y" << "INS_ACC3OFFS_Z"; + + 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; + } + } + } + } + + return false; +} + diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponent.h b/src/AutoPilotPlugins/APM/APMSensorsComponent.h index aa53a5bbec815dd0cf68cf274266b55e2c6fa42c..c22e9492f1d4c40ee46b6a9747e80691285c4b11 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponent.h +++ b/src/AutoPilotPlugins/APM/APMSensorsComponent.h @@ -32,7 +32,10 @@ class APMSensorsComponent : public APMComponent public: APMSensorsComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL); - + + bool compassSetupNeeded(void) const; + bool accelSetupNeeded(void) const; + // Virtuals from APMComponent virtual QStringList setupCompleteChangedTriggerList(void) const; diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponent.qml b/src/AutoPilotPlugins/APM/APMSensorsComponent.qml index 4fd3d21a5bd03113d52cf945cf76abcd24dadda9..f5674dee720b7f0a2de94e5a9200299cb2ba926d 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponent.qml +++ b/src/AutoPilotPlugins/APM/APMSensorsComponent.qml @@ -112,24 +112,8 @@ QGCView { property Fact sens_board_rot: controller.getParameterFact(-1, "AHRS_ORIENTATION") - /* - property Fact cal_gyro0_id: controller.getParameterFact(-1, "CAL_GYRO0_ID") - property Fact cal_acc0_id: controller.getParameterFact(-1, "CAL_ACC0_ID") - - property Fact sens_board_x_off: controller.getParameterFact(-1, "SENS_BOARD_X_OFF") - property Fact sens_dpres_off: controller.getParameterFact(-1, "SENS_DPRES_OFF") - */ - - property Fact ins_accoffs_x: controller.getParameterFact(-1, "INS_ACCOFFS_X") - property Fact ins_accoffs_y: controller.getParameterFact(-1, "INS_ACCOFFS_Y") - property Fact ins_accoffs_z: controller.getParameterFact(-1, "INS_ACCOFFS_Z") - - property Fact compass_ofs_x: controller.getParameterFact(-1, "COMPASS_OFS_X") - property Fact compass_ofs_y: controller.getParameterFact(-1, "COMPASS_OFS_Y") - property Fact compass_ofs_z: controller.getParameterFact(-1, "COMPASS_OFS_Z") - - property bool accelCalNeeded: ins_accoffs_x.value == 0 && ins_accoffs_y.value == 0 && ins_accoffs_z.value == 0 - property bool compassCalNeeded: compass_ofs_x.value == 0 && compass_ofs_y.value == 0 && compass_ofs_y.value == 0 + property bool accelCalNeeded: controller.accelSetupNeeded + property bool compassCalNeeded: controller.compassSetupNeeded // Id > = signals compass available, rot < 0 signals internal compass property bool showCompass0Rot: cal_mag0_id.value > 0 && cal_mag0_rot.value >= 0 diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc index 2a267d33c227f5b9ae372914383ef293272b3a62..c160dbf48c2427e326beab1abe160fb794556395 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc @@ -25,6 +25,7 @@ #include "QGCMAVLink.h" #include "UAS.h" #include "QGCApplication.h" +#include "APMAutoPilotPlugin.h" #include #include @@ -68,7 +69,10 @@ APMSensorsComponentController::APMSensorsComponentController(void) : _orientationCalTailDownSideRotate(false), _waitingForCancel(false) { + APMAutoPilotPlugin * apmPlugin = qobject_cast(_vehicle->autopilotPlugin()); + _sensorsComponent = apmPlugin->sensorsComponent(); + connect(apmPlugin, &APMAutoPilotPlugin::setupCompleteChanged, this, &APMSensorsComponentController::setupNeededChanged); } /// Appends the specified text to the status log area in the ui @@ -180,12 +184,6 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll _gyroCalInProgress = false; } -void APMSensorsComponentController::calibrateGyro(void) -{ - _startLogCalibration(); - _uas->startCalibration(UASInterface::StartCalibrationGyro); -} - void APMSensorsComponentController::calibrateCompass(void) { _startLogCalibration(); @@ -198,18 +196,6 @@ void APMSensorsComponentController::calibrateAccel(void) _uas->startCalibration(UASInterface::StartCalibrationAccel); } -void APMSensorsComponentController::calibrateLevel(void) -{ - _startLogCalibration(); - _uas->startCalibration(UASInterface::StartCalibrationLevel); -} - -void APMSensorsComponentController::calibrateAirspeed(void) -{ - _startLogCalibration(); - _uas->startCalibration(UASInterface::StartCalibrationAirspeed); -} - void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text) { Q_UNUSED(compId); @@ -467,3 +453,13 @@ void APMSensorsComponentController::nextClicked(void) _vehicle->sendMessage(msg); } + +bool APMSensorsComponentController::compassSetupNeeded(void) const +{ + return _sensorsComponent->compassSetupNeeded(); +} + +bool APMSensorsComponentController::accelSetupNeeded(void) const +{ + return _sensorsComponent->accelSetupNeeded(); +} diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.h b/src/AutoPilotPlugins/APM/APMSensorsComponentController.h index aba7ea3b7b1a6ae3b9d1f0765cafe2b233e6ab5b..5df68392838ad8b5f406eba25e096dd1f09f5430 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.h +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentController.h @@ -29,6 +29,7 @@ #include "UASInterface.h" #include "FactPanelController.h" #include "QGCLoggingCategory.h" +#include "APMSensorsComponent.h" Q_DECLARE_LOGGING_CATEGORY(APMSensorsComponentControllerLog) @@ -50,7 +51,10 @@ public: Q_PROPERTY(QQuickItem* nextButton MEMBER _nextButton) Q_PROPERTY(QQuickItem* cancelButton MEMBER _cancelButton) Q_PROPERTY(QQuickItem* orientationCalAreaHelpText MEMBER _orientationCalAreaHelpText) - + + Q_PROPERTY(bool compassSetupNeeded READ compassSetupNeeded NOTIFY setupNeededChanged) + Q_PROPERTY(bool accelSetupNeeded READ accelSetupNeeded NOTIFY setupNeededChanged) + Q_PROPERTY(bool showOrientationCalArea MEMBER _showOrientationCalArea NOTIFY showOrientationCalAreaChanged) Q_PROPERTY(bool orientationCalDownSideDone MEMBER _orientationCalDownSideDone NOTIFY orientationCalSidesDoneChanged) @@ -84,14 +88,14 @@ public: Q_PROPERTY(bool waitingForCancel MEMBER _waitingForCancel NOTIFY waitingForCancelChanged) Q_INVOKABLE void calibrateCompass(void); - Q_INVOKABLE void calibrateGyro(void); Q_INVOKABLE void calibrateAccel(void); - Q_INVOKABLE void calibrateLevel(void); - Q_INVOKABLE void calibrateAirspeed(void); Q_INVOKABLE void cancelCalibration(void); Q_INVOKABLE void nextClicked(void); bool fixedWing(void); + + bool compassSetupNeeded(void) const; + bool accelSetupNeeded(void) const; signals: void showGyroCalAreaChanged(void); @@ -103,6 +107,7 @@ signals: void resetStatusTextArea(void); void waitingForCancelChanged(void); void setCompassRotations(void); + void setupNeededChanged(void); private slots: void _handleUASTextMessage(int uasId, int compId, int severity, QString text); @@ -124,6 +129,8 @@ private: void _updateAndEmitShowOrientationCalArea(bool show); + APMSensorsComponent* _sensorsComponent; + QQuickItem* _statusLog; QQuickItem* _progressBar; QQuickItem* _compassButton; diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentSummary.qml b/src/AutoPilotPlugins/APM/APMSensorsComponentSummary.qml index de5e5381874f736efd7a23aaba007bc6d2131f36..75902f55147be8fc21855b1aeb5fed2fd509514d 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentSummary.qml +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentSummary.qml @@ -1,11 +1,12 @@ -import QtQuick 2.2 -import QtQuick.Controls 1.2 -import QtQuick.Controls.Styles 1.2 +import QtQuick 2.5 +import QtQuick.Controls 1.2 +import QtQuick.Controls.Styles 1.2 -import QGroundControl.FactSystem 1.0 -import QGroundControl.FactControls 1.0 -import QGroundControl.Controls 1.0 -import QGroundControl.Palette 1.0 +import QGroundControl.FactSystem 1.0 +import QGroundControl.FactControls 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.Controllers 1.0 /* IMPORTANT NOTE: Any changes made here must also be made to SensorsComponentSummary.qml @@ -17,18 +18,10 @@ FactPanel { color: qgcPal.windowShadeDark QGCPalette { id: qgcPal; colorGroupEnabled: enabled } - FactPanelController { id: controller; factPanel: panel } + APMSensorsComponentController { id: controller; factPanel: panel } - property Fact ins_accoffs_x: controller.getParameterFact(-1, "INS_ACCOFFS_X") - property Fact ins_accoffs_y: controller.getParameterFact(-1, "INS_ACCOFFS_Y") - property Fact ins_accoffs_z: controller.getParameterFact(-1, "INS_ACCOFFS_Z") - - property Fact compass_ofs_x: controller.getParameterFact(-1, "COMPASS_OFS_X") - property Fact compass_ofs_y: controller.getParameterFact(-1, "COMPASS_OFS_Y") - property Fact compass_ofs_z: controller.getParameterFact(-1, "COMPASS_OFS_Z") - - property bool accelCalNeeded: ins_accoffs_x.value == 0 && ins_accoffs_y.value == 0 && ins_accoffs_z.value == 0 - property bool compassCalNeeded: compass_ofs_x.value == 0 && compass_ofs_y.value == 0 && compass_ofs_y.value == 0 + property bool accelCalNeeded: controller.accelSetupNeeded + property bool compassCalNeeded: controller.compassSetupNeeded Column { anchors.fill: parent