From 64672e34d3485daacc407968cd38ef230b5bab27 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 10 Feb 2015 14:04:30 -0800 Subject: [PATCH] Request all sensor parameters before/after cal Include support for both param sets --- .../QGCPX4SensorCalibration.cc | 87 ++++++++----------- .../QGCPX4SensorCalibration.h | 2 + 2 files changed, 36 insertions(+), 53 deletions(-) diff --git a/src/ui/px4_configuration/QGCPX4SensorCalibration.cc b/src/ui/px4_configuration/QGCPX4SensorCalibration.cc index 515ec2b16..dc7c0bdda 100644 --- a/src/ui/px4_configuration/QGCPX4SensorCalibration.cc +++ b/src/ui/px4_configuration/QGCPX4SensorCalibration.cc @@ -351,28 +351,9 @@ void QGCPX4SensorCalibration::setActiveUAS(UASInterface* uas) updateSystemSpecs(uas->getUASID()); - // If the parameters are ready, we aren't going to get paramterChanged signals. So fake them in order to make the UI work. + // If the parameters are ready, we aren't going to get paramterChanged signals. So re-request them in order to make the UI work. if (uas->getParamManager()->parametersReady()) { - QVariant value; - static const char* rgParamsV1[] = { "SENS_BOARD_ROT", "SENS_EXT_MAG_ROT", "SENS_MAG_XOFF", "SENS_GYRO_XOFF", "SENS_ACC_XOFF", "SENS_DPRES_OFF" }; - static const char* rgParamsV2[] = { "SENS_BOARD_ROT", "SENS_EXT_MAG_ROT", "CAL_MAG0_XOFF", "CAL_GYRO0_XOFF", "CAL_ACC0_XOFF", "SENS_DPRES_OFF" }; - - QGCUASParamManagerInterface* paramMgr = uas->getParamManager(); - - // Temp hack for parameter mapping - bool paramsV1 = paramMgr->getComponentForParam("SENS_MAG_XOFF").count(); - static const char** prgParamList = paramsV1 ? rgParamsV1 : rgParamsV2; - - for (size_t i=0; i compIds = paramMgr->getComponentForParam(prgParamList[i]); - Q_ASSERT(compIds.count() == 1); - bool found = paramMgr->getParameterValue(compIds[0], prgParamList[i], value); - Q_ASSERT(found); - Q_UNUSED(found); - parameterChanged(uas->getUASID(), compIds[0], prgParamList[i], value); - } + _requestAllSensorParameters(); } } @@ -436,38 +417,7 @@ void QGCPX4SensorCalibration::handleTextMessage(int uasid, int compId, int sever } if (activeUAS) { - if (text.startsWith("accel ")) { - activeUAS->requestParameter(0, "SENS_ACC_XOFF"); - activeUAS->requestParameter(0, "SENS_ACC_YOFF"); - activeUAS->requestParameter(0, "SENS_ACC_ZOFF"); - activeUAS->requestParameter(0, "SENS_ACC_XSCALE"); - activeUAS->requestParameter(0, "SENS_ACC_YSCALE"); - activeUAS->requestParameter(0, "SENS_ACC_ZSCALE"); - activeUAS->requestParameter(0, "SENS_BOARD_ROT"); - } - if (text.startsWith("gyro ")) { - activeUAS->requestParameter(0, "SENS_GYRO_XOFF"); - activeUAS->requestParameter(0, "SENS_GYRO_YOFF"); - activeUAS->requestParameter(0, "SENS_GYRO_ZOFF"); - activeUAS->requestParameter(0, "SENS_GYRO_XSCALE"); - activeUAS->requestParameter(0, "SENS_GYRO_YSCALE"); - activeUAS->requestParameter(0, "SENS_GYRO_ZSCALE"); - activeUAS->requestParameter(0, "SENS_BOARD_ROT"); - } - if (text.startsWith("mag ")) { - activeUAS->requestParameter(0, "SENS_MAG_XOFF"); - activeUAS->requestParameter(0, "SENS_MAG_YOFF"); - activeUAS->requestParameter(0, "SENS_MAG_ZOFF"); - activeUAS->requestParameter(0, "SENS_MAG_XSCALE"); - activeUAS->requestParameter(0, "SENS_MAG_YSCALE"); - activeUAS->requestParameter(0, "SENS_MAG_ZSCALE"); - activeUAS->requestParameter(0, "SENS_EXT_MAG_ROT"); - } - - if (text.startsWith("dpress ")) { - activeUAS->requestParameter(0, "SENS_DPRES_OFF"); - activeUAS->requestParameter(0, "SENS_DPRES_ANA"); - } + _requestAllSensorParameters(); } } @@ -524,3 +474,34 @@ void QGCPX4SensorCalibration::contextMenuEvent(QContextMenuEvent* event) menu.addAction(clearAction); menu.exec(event->globalPos()); } + +void QGCPX4SensorCalibration::_requestAllSensorParameters(void) +{ + static const char* rgSensorsCalParamsV1[] = { + "SENS_ACC_XOFF", "SENS_ACC_YOFF", "SENS_ACC_ZOFF", "SENS_ACC_XSCALE", "SENS_ACC_YSCALE", "SENS_ACC_ZSCALE", + "SENS_GYRO_XOFF", "SENS_GYRO_YOFF", "SENS_GYRO_ZOFF", "SENS_GYRO_XSCALE", "SENS_GYRO_YSCALE", "SENS_GYRO_ZSCALE", + "SENS_MAG_XOFF", "SENS_MAG_YOFF", "SENS_MAG_ZOFF", "SENS_MAG_XSCALE", "SENS_MAG_YSCALE", "SENS_MAG_ZSCALE", "SENS_EXT_MAG_ROT", + "SENS_DPRES_OFF", "SENS_DPRES_ANA", + "SENS_BOARD_ROT", + NULL }; + + static const char* rgSensorsCalParamsV2[] = { + "CAL_ACC0_ID", "CAL_ACC0_XOFF", "CAL_ACC0_YOFF", "CAL_ACC0_ZOFF", "CAL_ACC0_XSCALE", "CAL_ACC0_YSCALE", "CAL_ACC0_ZSCALE", + "CAL_GYRO0_ID", "CAL_GYRO0_XOFF", "CAL_GYRO0_YOFF", "CAL_GYRO0_ZOFF", "CAL_GYRO0_XSCALE", "CAL_GYRO0_YSCALE", "CAL_GYRO0_ZSCALE", + "CAL_MAG0_ID", "CAL_MAG0_XOFF", "CAL_MAG0_YOFF", "CAL_MAG0_ZOFF", "CAL_MAG0_XSCALE", "CAL_MAG0_YSCALE", "CAL_MAG0_ZSCALE", "SENS_EXT_MAG_ROT", + "SENS_DPRES_OFF", "SENS_DPRES_ANA", + "SENS_BOARD_ROT", + NULL }; + + Q_ASSERT(activeUAS); + QGCUASParamManagerInterface* paramMgr = activeUAS->getParamManager(); + + // Temp hack for parameter mapping + bool paramsV1 = paramMgr->getComponentForParam("SENS_MAG_XOFF").count(); + static const char** prgParamList = paramsV1 ? rgSensorsCalParamsV1 : rgSensorsCalParamsV2; + + for (size_t i=0; prgParamList[i] != NULL; i++) { + qDebug() << "Requesting" << prgParamList[i]; + activeUAS->requestParameter(0, prgParamList[i]); + } +} \ No newline at end of file diff --git a/src/ui/px4_configuration/QGCPX4SensorCalibration.h b/src/ui/px4_configuration/QGCPX4SensorCalibration.h index 630a1c895..8c84e62e0 100644 --- a/src/ui/px4_configuration/QGCPX4SensorCalibration.h +++ b/src/ui/px4_configuration/QGCPX4SensorCalibration.h @@ -83,6 +83,8 @@ protected: void updateIcons(); private: + void _requestAllSensorParameters(void); + Ui::QGCPX4SensorCalibration *ui; }; -- 2.22.0