1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00

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
This commit is contained in:
edouard 2010-08-25 22:54:32 +00:00 committed by edouard
parent d71ade8227
commit 86bef7b96b
5 changed files with 237 additions and 57 deletions

View File

@ -37,6 +37,11 @@
<height>141</height>
</rect>
</property>
<property name="toolTip">
<string>These are the sensor variance values computed by the AHRS.
Tip: lower is better!</string>
</property>
<property name="verticalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
@ -49,40 +54,32 @@
<rect>
<x>390</x>
<y>90</y>
<width>301</width>
<width>231</width>
<height>17</height>
</rect>
</property>
<property name="text">
<string>Instructions......................</string>
<string>Press &quot;Start&quot; above to calibrate.</string>
</property>
</widget>
<widget class="QPushButton" name="ahrsCalibStart">
<property name="geometry">
<rect>
<x>400</x>
<y>30</y>
<x>380</x>
<y>40</y>
<width>93</width>
<height>27</height>
</rect>
</property>
<property name="toolTip">
<string>Press to start a calibration procedure.
Takes about 10 seconds.</string>
</property>
<property name="text">
<string>Start</string>
</property>
</widget>
<widget class="QPushButton" name="pushButton_2">
<property name="geometry">
<rect>
<x>540</x>
<y>30</y>
<width>93</width>
<height>27</height>
</rect>
</property>
<property name="text">
<string>Cancel</string>
</property>
</widget>
<widget class="Line" name="line">
<property name="geometry">
<rect>
@ -269,6 +266,35 @@
<enum>Qt::Horizontal</enum>
</property>
</widget>
<widget class="QLabel" name="label">
<property name="geometry">
<rect>
<x>380</x>
<y>10</y>
<width>151</width>
<height>17</height>
</rect>
</property>
<property name="text">
<string>Sensors calibration</string>
</property>
</widget>
<widget class="QPushButton" name="ahrsCalibSave">
<property name="enabled">
<bool>false</bool>
</property>
<property name="geometry">
<rect>
<x>500</x>
<y>40</y>
<width>93</width>
<height>27</height>
</rect>
</property>
<property name="text">
<string>Save</string>
</property>
</widget>
</widget>
</widget>
<resources/>

View File

@ -26,6 +26,7 @@
*/
#include "configahrswidget.h"
#include "math.h"
#include <QDebug>
#include <QTimer>
#include <QStringList>
@ -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<UAVObjectManager>();
@ -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<UAVObjectManager>();
UAVObject *obj = dynamic_cast<UAVDataObject*>(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<UAVObjectManager>();
UAVObject *obj = dynamic_cast<UAVDataObject*>(objManager->getObject(QString("AHRSCalibration")));
UAVObjectField *field = obj->getField(QString("measure_var"));
field->setValue("FALSE");
obj->updated();
m_ahrs->ahrsCalibStart->setEnabled(true);
}

View File

@ -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:

View File

@ -235,7 +235,7 @@ void ConfigServoWidget::requestRCOutputUpdate()
obj->requestUpdate();
QList<UAVObjectField*> fieldList = obj->getFields();
foreach (UAVObjectField* field, fieldList) {
if (!field->getName().contains("Channel")) {
if (field->getUnits().contains("channel")) {
assignOutputChannel(obj,field,field->getName());
}
}

View File

@ -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 @@
<rect
style="fill:#ffffff;fill-opacity:1;stroke:#ffffff;stroke-width:9.4025631;stroke-miterlimit:10;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
id="rect6737"
width="687.74054"
width="683.69995"
height="354.43222"
x="187.97757"
y="-563.36774"
@ -353,28 +353,28 @@
inkscape:export-ydpi="88.809998"
rx="3.1741481" />
<path
style="fill:none;stroke:#000000;stroke-width:1.17532039px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-start:url(#Arrow1Lstart);marker-end:url(#Arrow1Lend)"
style="fill:none;stroke:#000000;stroke-width:1.17532039000000021px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-start:none;marker-end:none"
d="m 537.38036,217.29216 -294.4803,0 0,614.17274"
id="path5991" />
<path
style="fill:none;stroke:#000000;stroke-width:1.17532039;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:1.17532035, 3.52596104;stroke-dashoffset:0"
d="m 281.83723,219.31246 0,602.05092"
d="m 285.2717,219.31246 0,602.05092"
id="path6633" />
<path
id="path6635"
d="m 324.63724,219.31246 0,602.05092"
d="m 327.3848,219.31246 0,602.05092"
style="fill:none;stroke:#000000;stroke-width:1.17532039;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:1.17532035, 3.52596104;stroke-dashoffset:0" />
<path
style="fill:none;stroke:#000000;stroke-width:1.17532039;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:1.17532035, 3.52596104;stroke-dashoffset:0"
d="m 367.43724,219.31246 0,602.05092"
d="m 369.49791,219.31246 0,602.05092"
id="path6637" />
<path
id="path6639"
d="m 410.23724,219.31246 0,602.05092"
d="m 411.61102,219.31246 0,602.05092"
style="fill:none;stroke:#000000;stroke-width:1.17532039;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:1.17532035, 3.52596104;stroke-dashoffset:0" />
<path
style="fill:none;stroke:#000000;stroke-width:1.17532039;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:1.17532035, 3.52596104;stroke-dashoffset:0"
d="m 453.03722,219.31246 0,602.05092"
d="m 453.72413,219.31246 0,602.05092"
id="path6641" />
<path
id="path6643"
@ -382,7 +382,7 @@
style="fill:none;stroke:#000000;stroke-width:1.17532039;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:1.17532035, 3.52596104;stroke-dashoffset:0" />
<text
xml:space="preserve"
style="font-size:21.98640251px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans"
style="font-size:21.98640251px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans"
x="808.37286"
y="-500.03018"
id="text6659"
@ -396,7 +396,7 @@
id="text6667"
y="-458.03018"
x="808.37286"
style="font-size:21.98640251px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans"
style="font-size:21.98640251px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans"
xml:space="preserve"><tspan
id="tspan6669"
y="-458.03018"
@ -404,7 +404,7 @@
sodipodi:role="line">1E-2</tspan></text>
<text
xml:space="preserve"
style="font-size:21.98640251px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans"
style="font-size:21.98640251px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans"
x="808.37286"
y="-418.03018"
id="text6671"
@ -418,7 +418,7 @@
id="text6675"
y="-374.03018"
x="808.37286"
style="font-size:21.98640251px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans"
style="font-size:21.98640251px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans"
xml:space="preserve"><tspan
id="tspan6677"
y="-374.03018"
@ -426,7 +426,7 @@
sodipodi:role="line">1E-4</tspan></text>
<text
xml:space="preserve"
style="font-size:21.98640251px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans"
style="font-size:21.98640251px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans"
x="808.37286"
y="-330.03018"
id="text6679"
@ -440,7 +440,7 @@
id="text6683"
y="-288.03018"
x="808.37286"
style="font-size:21.98640251px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans"
style="font-size:21.98640251px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans"
xml:space="preserve"><tspan
id="tspan6685"
y="-288.03018"
@ -460,7 +460,7 @@
id="path6707" />
<text
xml:space="preserve"
style="font-size:47.01281357px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;display:inline;font-family:Bitstream Vera Sans"
style="font-size:47.01281356999999161px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Bitstream Vera Sans"
x="355.29535"
y="-449.45621"
id="text6755"
@ -469,10 +469,10 @@
id="tspan6757"
x="355.29535"
y="-449.45621"
style="font-size:23.50640678px">Accel</tspan></text>
style="font-size:23.50640677999999895px;fill:#ffffff;fill-opacity:1">Accel</tspan></text>
<text
xml:space="preserve"
style="font-size:47.01281357px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;display:inline;font-family:Bitstream Vera Sans"
style="font-size:47.01281356999999161px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Bitstream Vera Sans"
x="573.94739"
y="-451.94113"
id="text6759"
@ -481,10 +481,10 @@
id="tspan6761"
x="573.94739"
y="-451.94113"
style="font-size:23.50640678px">Gyro</tspan></text>
style="font-size:23.50640677999999895px;fill:#ffffff;fill-opacity:1">Gyro</tspan></text>
<text
xml:space="preserve"
style="font-size:47.01281357px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;display:inline;font-family:Bitstream Vera Sans"
style="font-size:47.01281356999999161px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Bitstream Vera Sans"
x="797.87463"
y="-452.0961"
id="text6763"
@ -493,7 +493,7 @@
id="tspan6765"
x="797.87463"
y="-452.0961"
style="font-size:23.50640678px">Mag</tspan></text>
style="font-size:23.50640677999999895px;fill:#ffffff;fill-opacity:1">Mag</tspan></text>
</g>
</g>
<g

Before

Width:  |  Height:  |  Size: 23 KiB

After

Width:  |  Height:  |  Size: 23 KiB