mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-19 09:54:15 +01:00
OP-233 Further work on Temperature compensation for gyros (GCS config). Everyting is in place to sample data and compute the regression.
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2343 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
edouard
edouard
parent
ccc8bc089b
commit
489fde5357
@ -356,82 +356,42 @@ Hint: run this with engines at cruising speed.</string>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_5">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_10">
|
||||
<property name="text">
|
||||
<string> Z:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QLabel" name="zMinTemp">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>9</pointsize>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Min</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QSlider" name="zSlider">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="tickPosition">
|
||||
<enum>QSlider::TicksBelow</enum>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QLabel" name="zMaxTemp">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>9</pointsize>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Max</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_2">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_9">
|
||||
<property name="text">
|
||||
<string>XY:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QLabel" name="label_8">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>9</pointsize>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Min</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QSlider" name="xySlider">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="tickPosition">
|
||||
<enum>QSlider::TicksBelow</enum>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QLabel" name="label_7">
|
||||
<property name="text">
|
||||
<string>Temp:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QLabel" name="gyroMinTemp">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>9</pointsize>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Min</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QSlider" name="gyroTempSlider">
|
||||
<property name="toolTip">
|
||||
<string>Currently measured temperature on the system. This is actually the
|
||||
MB temperature, be careful if somehow you know that your INS
|
||||
temperature is very different from your MB temp...</string>
|
||||
</property>
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="tickPosition">
|
||||
<enum>QSlider::TicksBelow</enum>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QLabel" name="gyroMaxTemp">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>9</pointsize>
|
||||
@ -469,22 +429,44 @@ Hint: run this with engines at cruising speed.</string>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_6">
|
||||
<item>
|
||||
<widget class="QPushButton" name="resetDrift">
|
||||
<widget class="QPushButton" name="startDriftCalib">
|
||||
<property name="enabled">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string>Reset Gyro drift value to zero.</string>
|
||||
<string>Start gathering data for temperature drift calibration.
|
||||
Avoid sudden moves once you have started gathering!</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Reset</string>
|
||||
<string>Start</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QPushButton" name="gyroMeasureDrift">
|
||||
<property name="enabled">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string>Launch drift measurement based on gathered data.
|
||||
|
||||
TODO: is this necessary? Measurement could be auto updated every second or so, or done when we stop measuring...</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Measure</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QPushButton" name="updateDrift">
|
||||
<property name="enabled">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string>Updates the XYZ drift values into the AHRS (saves to SD)</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Update</string>
|
||||
<string>Save</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
|
||||
@ -59,6 +59,8 @@ ConfigAHRSWidget::ConfigAHRSWidget(QWidget *parent) : ConfigTaskWidget(parent)
|
||||
m_ahrs = new Ui_AHRSWidget();
|
||||
m_ahrs->setupUi(this);
|
||||
|
||||
collectingData = false;
|
||||
|
||||
// Initialization of the Paper plane widget
|
||||
m_ahrs->sixPointsHelp->setScene(new QGraphicsScene(this));
|
||||
|
||||
@ -214,6 +216,7 @@ ConfigAHRSWidget::ConfigAHRSWidget(QWidget *parent) : ConfigTaskWidget(parent)
|
||||
connect(m_ahrs->ahrsSettingsSaveSD, SIGNAL(clicked()), this, SLOT(ahrsSettingsSaveSD()));
|
||||
connect(m_ahrs->sixPointsStart, SIGNAL(clicked()), this, SLOT(sixPointCalibrationMode()));
|
||||
connect(m_ahrs->sixPointsSave, SIGNAL(clicked()), this, SLOT(savePositionData()));
|
||||
connect(m_ahrs->startDriftCalib, SIGNAL(clicked()),this, SLOT(launchGyroDriftCalibration()));
|
||||
connect(parent, SIGNAL(autopilotConnected()),this, SLOT(ahrsSettingsRequest()));
|
||||
|
||||
|
||||
@ -299,7 +302,7 @@ void ConfigAHRSWidget::accelBiasattitudeRawUpdated(UAVObject *obj)
|
||||
|
||||
if(accel_accum_x.size() >= 100 && collectingData == true) {
|
||||
collectingData = false;
|
||||
disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(attitudeRawUpdated(UAVObject*)));
|
||||
disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(accelBiasattitudeRawUpdated(UAVObject*)));
|
||||
m_ahrs->accelBiasStart->setEnabled(true);
|
||||
|
||||
UAVDataObject* ahrsCalib = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration")));
|
||||
@ -333,6 +336,120 @@ void ConfigAHRSWidget::accelBiasattitudeRawUpdated(UAVObject *obj)
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
Starts an Gyro temperature drift calibration.
|
||||
*/
|
||||
void ConfigAHRSWidget::launchGyroDriftCalibration()
|
||||
{
|
||||
if (!collectingData) {
|
||||
// m_ahrs->startDriftCalib->setEnabled(false);
|
||||
m_ahrs->startDriftCalib->setText("Stop");
|
||||
m_ahrs->accelBiasStart->setEnabled(false);
|
||||
m_ahrs->ahrsCalibStart->setEnabled(false);
|
||||
m_ahrs->sixPointsStart->setEnabled(false);
|
||||
|
||||
// Setup the AHRS to give us the right data at the right rate:
|
||||
UAVDataObject* obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings")));
|
||||
UAVObjectField* field = obj->getField(QString("BiasCorrectedRaw"));
|
||||
field->setValue("FALSE");
|
||||
obj->updated();
|
||||
|
||||
/* Need to get as many AttitudeRaw updates as possible */
|
||||
obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeRaw")));
|
||||
initialMdata = obj->getMetadata();
|
||||
UAVObject::Metadata mdata = initialMdata;
|
||||
mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
|
||||
mdata.flightTelemetryUpdatePeriod = 100;
|
||||
obj->setMetadata(mdata);
|
||||
|
||||
// Now connect to the attituderaw updates until we stop
|
||||
collectingData = true;
|
||||
obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("BaroAltitude")));
|
||||
field = obj->getField(QString("Temperature"));
|
||||
double temp = field->getValue().toDouble();
|
||||
m_ahrs->gyroTempSlider->setRange(temp*10,temp*10);
|
||||
m_ahrs->gyroMaxTemp->setText(QString::number(temp,'g',3));
|
||||
m_ahrs->gyroMinTemp->setText(QString::number(temp,'g',3));
|
||||
|
||||
connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(driftCalibrationAttitudeRawUpdated(UAVObject*)));
|
||||
} else {
|
||||
// Stop all the gathering:
|
||||
collectingData = false;
|
||||
m_ahrs->startDriftCalib->setText("Start");
|
||||
m_ahrs->accelBiasStart->setEnabled(true);
|
||||
m_ahrs->ahrsCalibStart->setEnabled(true);
|
||||
m_ahrs->sixPointsStart->setEnabled(true);
|
||||
|
||||
UAVDataObject* obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeRaw")));
|
||||
disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(driftCalibrationAttitudeRawUpdated(UAVObject*)));
|
||||
|
||||
getObjectManager()->getObject(QString("AttitudeRaw"))->setMetadata(initialMdata);
|
||||
|
||||
obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings")));
|
||||
UAVObjectField* field = obj->getField(QString("BiasCorrectedRaw"));
|
||||
field->setValue("TRUE");
|
||||
obj->updated();
|
||||
|
||||
// TODO: Now compute the drift here
|
||||
computeGyroDrift();
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
Updates the gyro drift calibration values in real time
|
||||
*/
|
||||
void ConfigAHRSWidget::driftCalibrationAttitudeRawUpdated(UAVObject* obj) {
|
||||
|
||||
// This is necessary to prevent a race condition on disconnect signal and another update
|
||||
if (collectingData == true) {
|
||||
/**
|
||||
First of all, update the temperature user feedback
|
||||
This is not what we will use for our calculations, but it it easier for the
|
||||
user to have the real temperature rather than an obscure unit...
|
||||
*/
|
||||
UAVDataObject* obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("BaroAltitude")));
|
||||
UAVObjectField *tempField = obj->getField(QString("Temperature"));
|
||||
Q_ASSERT(tempField != 0);
|
||||
double mbTemp = tempField->getValue().toDouble();
|
||||
if (mbTemp*10 < m_ahrs->gyroTempSlider->minimum()) {
|
||||
m_ahrs->gyroTempSlider->setMinimum(mbTemp*10);
|
||||
m_ahrs->gyroMinTemp->setText(QString::number(mbTemp,'g',3));
|
||||
} else if (mbTemp*10 > m_ahrs->gyroTempSlider->maximum()) {
|
||||
m_ahrs->gyroTempSlider->setMaximum(mbTemp*10);
|
||||
m_ahrs->gyroMaxTemp->setText(QString::number(mbTemp,'g',3));
|
||||
}
|
||||
m_ahrs->gyroTempSlider->setValue(mbTemp*10);
|
||||
// TODO:
|
||||
// - Add an indicator to show that we have a significant
|
||||
// temperature difference in our gathered data (red/yellow/green)
|
||||
|
||||
/**
|
||||
Now, append gyro values + gyro temp data into our buffers
|
||||
*/
|
||||
// TODO:
|
||||
// - choose a storage type for this data
|
||||
// - Check it's not getting too big
|
||||
// - do the actual appending
|
||||
// - That's it, really...
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
Computes gyro drift based on sampled data
|
||||
*/
|
||||
void ConfigAHRSWidget::computeGyroDrift() {
|
||||
// TODO
|
||||
|
||||
// TODO: if this is not too computing-intensive, we could consider
|
||||
// calling this with a timer when data sampling is enabled, to get
|
||||
// a real-time view of the computed drift convergence and let the
|
||||
// user stop sampling when it becomes stable enough...
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
@ -804,6 +921,8 @@ void ConfigAHRSWidget::ahrsSettingsRequest()
|
||||
m_ahrs->ahrsCalibStart->setEnabled(true);
|
||||
m_ahrs->sixPointsStart->setEnabled(true);
|
||||
m_ahrs->accelBiasStart->setEnabled(true);
|
||||
m_ahrs->startDriftCalib->setEnabled(true);
|
||||
|
||||
m_ahrs->calibInstructions->setText(QString("Press \"Start\" above to calibrate."));
|
||||
|
||||
}
|
||||
|
||||
@ -91,9 +91,11 @@ private:
|
||||
double mag_data_z[6];
|
||||
int position;
|
||||
|
||||
|
||||
UAVObject::Metadata initialMdata;
|
||||
|
||||
double listMean(QList<double> list);
|
||||
void computeGyroDrift();
|
||||
|
||||
private slots:
|
||||
void enableHomeLocSave(UAVObject *obj);
|
||||
@ -110,6 +112,8 @@ private slots:
|
||||
void sixPointCalibrationMode();
|
||||
void attitudeRawUpdated(UAVObject * obj);
|
||||
void accelBiasattitudeRawUpdated(UAVObject*);
|
||||
void driftCalibrationAttitudeRawUpdated(UAVObject*);
|
||||
void launchGyroDriftCalibration();
|
||||
|
||||
protected:
|
||||
void showEvent(QShowEvent *event);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user