From 86bef7b96ba7e5fffba1d97407524daceafe7693 Mon Sep 17 00:00:00 2001 From: edouard Date: Wed, 25 Aug 2010 22:54:32 +0000 Subject: [PATCH] OP-38 Experimental AHRS automated calibration widget, for testing purpose only. Does not save to SD, and wait 10 seconds before hitting "Save" once the button becomes enabled. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1410 ebee16cc-31ac-478f-84a7-5cbb03baadba --- ground/src/plugins/config/ahrs.ui | 60 ++++-- .../src/plugins/config/configahrswidget.cpp | 182 ++++++++++++++++-- ground/src/plugins/config/configahrswidget.h | 10 + .../src/plugins/config/configservowidget.cpp | 2 +- .../src/plugins/config/images/ahrs-calib.svg | 40 ++-- 5 files changed, 237 insertions(+), 57 deletions(-) diff --git a/ground/src/plugins/config/ahrs.ui b/ground/src/plugins/config/ahrs.ui index fa0ef4c2d..53e92885d 100644 --- a/ground/src/plugins/config/ahrs.ui +++ b/ground/src/plugins/config/ahrs.ui @@ -37,6 +37,11 @@ 141 + + These are the sensor variance values computed by the AHRS. + +Tip: lower is better! + Qt::ScrollBarAlwaysOff @@ -49,40 +54,32 @@ 390 90 - 301 + 231 17 - Instructions...................... + Press "Start" above to calibrate. - 400 - 30 + 380 + 40 93 27 + + Press to start a calibration procedure. + +Takes about 10 seconds. + Start - - - - 540 - 30 - 93 - 27 - - - - Cancel - - @@ -269,6 +266,35 @@ Qt::Horizontal + + + + 380 + 10 + 151 + 17 + + + + Sensors calibration + + + + + false + + + + 500 + 40 + 93 + 27 + + + + Save + + diff --git a/ground/src/plugins/config/configahrswidget.cpp b/ground/src/plugins/config/configahrswidget.cpp index c6d3769e3..f0c90beb7 100644 --- a/ground/src/plugins/config/configahrswidget.cpp +++ b/ground/src/plugins/config/configahrswidget.cpp @@ -26,6 +26,7 @@ */ #include "configahrswidget.h" +#include "math.h" #include #include #include @@ -56,8 +57,11 @@ ConfigAHRSWidget::ConfigAHRSWidget(QWidget *parent) : ConfigTaskWidget(parent) // Initialize the 9 bargraph values: QMatrix lineMatrix = renderer->matrixForElement("accel_x"); - qreal startX = lineMatrix.mapRect(renderer->boundsOnElement("accel_x")).x(); - qreal startY = lineMatrix.mapRect(renderer->boundsOnElement("accel_x")).y(); + QRectF rect = lineMatrix.mapRect(renderer->boundsOnElement("accel_x")); + qreal startX = rect.x(); + qreal startY = rect.y()+ rect.height(); + // maxBarHeight will be used for scaling it later. + maxBarHeight = rect.height(); // Then once we have the initial location, we can put it // into a QGraphicsSvgItem which we will display at the same // place: we do this so that the heading scale can be clipped to @@ -66,31 +70,97 @@ ConfigAHRSWidget::ConfigAHRSWidget(QWidget *parent) : ConfigTaskWidget(parent) accel_x->setSharedRenderer(renderer); accel_x->setElementId("accel_x"); m_ahrs->ahrsBargraph->scene()->addItem(accel_x); - QTransform matrix; - matrix.translate(startX,startY); - accel_x->setTransform(matrix,false); + accel_x->setPos(startX, startY); + accel_x->setTransform(QTransform::fromScale(1,0),true); lineMatrix = renderer->matrixForElement("accel_y"); - startX = lineMatrix.mapRect(renderer->boundsOnElement("accel_y")).x(); - startY = lineMatrix.mapRect(renderer->boundsOnElement("accel_y")).y(); + rect = lineMatrix.mapRect(renderer->boundsOnElement("accel_y")); + startX = rect.x(); + startY = rect.y()+ rect.height(); accel_y = new QGraphicsSvgItem(); accel_y->setSharedRenderer(renderer); accel_y->setElementId("accel_y"); m_ahrs->ahrsBargraph->scene()->addItem(accel_y); - matrix.reset(); - matrix.translate(startX,startY); - accel_y->setTransform(matrix,false); + accel_y->setPos(startX,startY); + accel_y->setTransform(QTransform::fromScale(1,0),true); lineMatrix = renderer->matrixForElement("accel_z"); - startX = lineMatrix.mapRect(renderer->boundsOnElement("accel_z")).x(); - startY = lineMatrix.mapRect(renderer->boundsOnElement("accel_z")).y(); + rect = lineMatrix.mapRect(renderer->boundsOnElement("accel_z")); + startX = rect.x(); + startY = rect.y()+ rect.height(); accel_z = new QGraphicsSvgItem(); accel_z->setSharedRenderer(renderer); accel_z->setElementId("accel_z"); m_ahrs->ahrsBargraph->scene()->addItem(accel_z); - matrix.reset(); - matrix.translate(startX,startY); - accel_z->setTransform(matrix,false); + accel_z->setPos(startX,startY); + accel_z->setTransform(QTransform::fromScale(1,0),true); + + lineMatrix = renderer->matrixForElement("gyro_x"); + rect = lineMatrix.mapRect(renderer->boundsOnElement("gyro_x")); + startX = rect.x(); + startY = rect.y()+ rect.height(); + gyro_x = new QGraphicsSvgItem(); + gyro_x->setSharedRenderer(renderer); + gyro_x->setElementId("gyro_x"); + m_ahrs->ahrsBargraph->scene()->addItem(gyro_x); + gyro_x->setPos(startX,startY); + gyro_x->setTransform(QTransform::fromScale(1,0),true); + + lineMatrix = renderer->matrixForElement("gyro_y"); + rect = lineMatrix.mapRect(renderer->boundsOnElement("gyro_y")); + startX = rect.x(); + startY = rect.y()+ rect.height(); + gyro_y = new QGraphicsSvgItem(); + gyro_y->setSharedRenderer(renderer); + gyro_y->setElementId("gyro_y"); + m_ahrs->ahrsBargraph->scene()->addItem(gyro_y); + gyro_y->setPos(startX,startY); + gyro_y->setTransform(QTransform::fromScale(1,0),true); + + + lineMatrix = renderer->matrixForElement("gyro_z"); + rect = lineMatrix.mapRect(renderer->boundsOnElement("gyro_z")); + startX = rect.x(); + startY = rect.y()+ rect.height(); + gyro_z = new QGraphicsSvgItem(); + gyro_z->setSharedRenderer(renderer); + gyro_z->setElementId("gyro_z"); + m_ahrs->ahrsBargraph->scene()->addItem(gyro_z); + gyro_z->setPos(startX,startY); + gyro_z->setTransform(QTransform::fromScale(1,0),true); + + lineMatrix = renderer->matrixForElement("mag_x"); + rect = lineMatrix.mapRect(renderer->boundsOnElement("mag_x")); + startX = rect.x(); + startY = rect.y()+ rect.height(); + mag_x = new QGraphicsSvgItem(); + mag_x->setSharedRenderer(renderer); + mag_x->setElementId("mag_x"); + m_ahrs->ahrsBargraph->scene()->addItem(mag_x); + mag_x->setPos(startX,startY); + mag_x->setTransform(QTransform::fromScale(1,0),true); + + lineMatrix = renderer->matrixForElement("mag_y"); + rect = lineMatrix.mapRect(renderer->boundsOnElement("mag_y")); + startX = rect.x(); + startY = rect.y()+ rect.height(); + mag_y = new QGraphicsSvgItem(); + mag_y->setSharedRenderer(renderer); + mag_y->setElementId("mag_y"); + m_ahrs->ahrsBargraph->scene()->addItem(mag_y); + mag_y->setPos(startX,startY); + mag_y->setTransform(QTransform::fromScale(1,0),true); + + lineMatrix = renderer->matrixForElement("mag_z"); + rect = lineMatrix.mapRect(renderer->boundsOnElement("mag_z")); + startX = rect.x(); + startY = rect.y()+ rect.height(); + mag_z = new QGraphicsSvgItem(); + mag_z->setSharedRenderer(renderer); + mag_z->setElementId("mag_z"); + m_ahrs->ahrsBargraph->scene()->addItem(mag_z); + mag_z->setPos(startX,startY); + mag_z->setTransform(QTransform::fromScale(1,0),true); @@ -105,6 +175,7 @@ ConfigAHRSWidget::ConfigAHRSWidget(QWidget *parent) : ConfigTaskWidget(parent) // Connect the signals connect(m_ahrs->ahrsCalibStart, SIGNAL(clicked()), this, SLOT(launchAHRSCalibration())); + connect(m_ahrs->ahrsCalibSave, SIGNAL(clicked()), this, SLOT(saveAHRSCalibration())); } @@ -126,6 +197,7 @@ void ConfigAHRSWidget::launchAHRSCalibration() { m_ahrs->calibInstructions->setText("Calibration launched..."); m_ahrs->ahrsCalibStart->setEnabled(false); + m_ahrs->ahrsCalibSave->setEnabled(false); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); @@ -138,18 +210,90 @@ void ConfigAHRSWidget::launchAHRSCalibration() waitabit->setSingleShot(true); waitabit->start(15000); connect(waitabit, SIGNAL(timeout()), this, SLOT(calibPhase2())); + phaseCounter = 0; } +/** + Callback after 15 seconds once calibration is done on the board. + + Currently we don't have a way to tell if calibration is finished, so we + have to use a timer. + + */ void ConfigAHRSWidget::calibPhase2() { - m_ahrs->calibInstructions->setText("Confirming settings..."); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + UAVObject *obj = dynamic_cast(objManager->getObject(QString("AHRSCalibration"))); - // Now update size of all the graphs in log scale - // 'log(1/val)' + // This is a bit weird, but it is because we are expecting an update from the + // OP board with the correct calibration values, and those only arrive on the object update + // which comes back from the board, and not the first object update signal which is in fast + // the object update we did ourselves... Clear ? + switch (phaseCounter) { + case 0: + phaseCounter++; + m_ahrs->calibInstructions->setText("Getting results..."); + connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(calibPhase2())); + obj->updated(); + break; + case 1: // this is where we end up with the update just above + phaseCounter++; + break; + case 2: // This is the update with the right values + disconnect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(calibPhase2())); + // Now update size of all the graphs + // I have not found a way to do this elegantly... + UAVObjectField *field = obj->getField(QString("accel_var")); + // The expected range is from 1E-6 to 1E-1 + double steps = 6; // 6 bars on the graph + float accel_x_var = -1/steps*(1+steps+log10(field->getValue(0).toFloat())); + accel_x->setTransform(QTransform::fromScale(1,accel_x_var),false); + float accel_y_var = -1/steps*(1+steps+log10(field->getValue(1).toFloat())); + accel_y->setTransform(QTransform::fromScale(1,accel_y_var),false); + float accel_z_var = -1/steps*(1+steps+log10(field->getValue(2).toFloat())); + accel_z->setTransform(QTransform::fromScale(1,accel_z_var),false); + + field = obj->getField(QString("gyro_var")); + float gyro_x_var = -1/steps*(1+steps+log10(field->getValue(0).toFloat())); + gyro_x->setTransform(QTransform::fromScale(1,gyro_x_var),false); + float gyro_y_var = -1/steps*(1+steps+log10(field->getValue(1).toFloat())); + gyro_y->setTransform(QTransform::fromScale(1,gyro_y_var),false); + float gyro_z_var = -1/steps*(1+steps+log10(field->getValue(2).toFloat())); + gyro_z->setTransform(QTransform::fromScale(1,gyro_z_var),false); + + field = obj->getField(QString("mag_var")); + float mag_x_var = -1/steps*(1+steps+log10(field->getValue(0).toFloat())); + mag_x->setTransform(QTransform::fromScale(1,mag_x_var),false); + float mag_y_var = -1/steps*(1+steps+log10(field->getValue(1).toFloat())); + mag_y->setTransform(QTransform::fromScale(1,mag_y_var),false); + float mag_z_var = -1/steps*(1+steps+log10(field->getValue(2).toFloat())); + mag_z->setTransform(QTransform::fromScale(1,mag_z_var),false); + + m_ahrs->ahrsCalibStart->setEnabled(true); + m_ahrs->ahrsCalibSave->setEnabled(true); + break; + } + +} + +/** + Saves the AHRS sensors calibration + */ +void ConfigAHRSWidget::saveAHRSCalibration() +{ + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + + UAVObject *obj = dynamic_cast(objManager->getObject(QString("AHRSCalibration"))); + UAVObjectField *field = obj->getField(QString("measure_var")); + field->setValue("FALSE"); + obj->updated(); - m_ahrs->ahrsCalibStart->setEnabled(true); } + + diff --git a/ground/src/plugins/config/configahrswidget.h b/ground/src/plugins/config/configahrswidget.h index e991af9e7..79ea67f33 100644 --- a/ground/src/plugins/config/configahrswidget.h +++ b/ground/src/plugins/config/configahrswidget.h @@ -52,9 +52,19 @@ private: QGraphicsSvgItem *accel_x; QGraphicsSvgItem *accel_y; QGraphicsSvgItem *accel_z; + QGraphicsSvgItem *gyro_x; + QGraphicsSvgItem *gyro_y; + QGraphicsSvgItem *gyro_z; + QGraphicsSvgItem *mag_x; + QGraphicsSvgItem *mag_y; + QGraphicsSvgItem *mag_z; + double maxBarHeight; + int phaseCounter; + const static double maxVarValue = 0.1; private slots: void launchAHRSCalibration(); + void saveAHRSCalibration(); void calibPhase2(); protected: diff --git a/ground/src/plugins/config/configservowidget.cpp b/ground/src/plugins/config/configservowidget.cpp index abedd7cd7..dec73b80c 100644 --- a/ground/src/plugins/config/configservowidget.cpp +++ b/ground/src/plugins/config/configservowidget.cpp @@ -235,7 +235,7 @@ void ConfigServoWidget::requestRCOutputUpdate() obj->requestUpdate(); QList fieldList = obj->getFields(); foreach (UAVObjectField* field, fieldList) { - if (!field->getName().contains("Channel")) { + if (field->getUnits().contains("channel")) { assignOutputChannel(obj,field,field->getName()); } } diff --git a/ground/src/plugins/config/images/ahrs-calib.svg b/ground/src/plugins/config/images/ahrs-calib.svg index 965027b5e..02795ca0d 100644 --- a/ground/src/plugins/config/images/ahrs-calib.svg +++ b/ground/src/plugins/config/images/ahrs-calib.svg @@ -288,7 +288,7 @@ inkscape:pageshadow="2" inkscape:zoom="0.98994949" inkscape:cx="334.4393" - inkscape:cy="152.4432" + inkscape:cy="117.47189" inkscape:document-units="px" inkscape:current-layer="background" showgrid="false" @@ -333,7 +333,7 @@ 1E-2 1E-4 Accel + style="font-size:23.50640677999999895px;fill:#ffffff;fill-opacity:1">Accel Gyro + style="font-size:23.50640677999999895px;fill:#ffffff;fill-opacity:1">Gyro Mag + style="font-size:23.50640677999999895px;fill:#ffffff;fill-opacity:1">Mag