rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-05 21:52:10 +01:00
Code Issues Releases Activity

Get the 6pt calibration working again for revo

Browse Source
This commit is contained in:
James Cotton 2011-12-24 15:56:16 -06:00
parent d78d345953
commit 710f95feeb
5 changed files with 323 additions and 810 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp
View File

@ -36,6 +36,7 @@
#include "configcamerastabilizationwidget.h" #include "configcamerastabilizationwidget.h"
#include "config_pro_hw_widget.h" #include "config_pro_hw_widget.h"
#include "config_cc_hw_widget.h" #include "config_cc_hw_widget.h"
#include "configrevowidget.h"
#include "defaultattitudewidget.h" #include "defaultattitudewidget.h"
#include "defaulthwsettingswidget.h" #include "defaulthwsettingswidget.h"
#include "uavobjectutilmanager.h" #include "uavobjectutilmanager.h"

1072
ground/openpilotgcs/src/plugins/config/configrevowidget.cpp
View File

@ -43,8 +43,8 @@
#include <accels.h> #include <accels.h>
#include <gyros.h> #include <gyros.h>
#include <magnetometer.h> #include <magnetometer.h>
#include <homelocation.h>
#define GRAVITY 9.81f
#include "assertions.h" #include "assertions.h"
#include "calibration.h" #include "calibration.h"
@ -65,13 +65,14 @@ public:
// ***************** // *****************
ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : ConfigTaskWidget(parent) ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
ConfigTaskWidget(parent),
collectingData(false),
position(-1),
m_ui(new Ui_RevoSensorsWidget())
{ {
m_ui = new Ui_RevoWidget();
m_ui->setupUi(this); m_ui->setupUi(this);
collectingData = false;
// Initialization of the Paper plane widget // Initialization of the Paper plane widget
m_ui->sixPointsHelp->setScene(new QGraphicsScene(this)); m_ui->sixPointsHelp->setScene(new QGraphicsScene(this));
@ -82,8 +83,7 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : ConfigTaskWidget(parent)
m_ui->sixPointsHelp->scene()->addItem(paperplane); m_ui->sixPointsHelp->scene()->addItem(paperplane);
m_ui->sixPointsHelp->setSceneRect(paperplane->boundingRect()); m_ui->sixPointsHelp->setSceneRect(paperplane->boundingRect());
// Initialization of the AHRS bargraph graph // Initialization of the Revo sensor noise bargraph graph
m_ui->ahrsBargraph->setScene(new QGraphicsScene(this)); m_ui->ahrsBargraph->setScene(new QGraphicsScene(this));
QSvgRenderer *renderer = new QSvgRenderer(); QSvgRenderer *renderer = new QSvgRenderer();
@ -203,36 +203,18 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : ConfigTaskWidget(parent)
mag_z->setPos(startX,startY); mag_z->setPos(startX,startY);
mag_z->setTransform(QTransform::fromScale(1,0),true); mag_z->setTransform(QTransform::fromScale(1,0),true);
position = -1;
/* // Fill the dropdown menus:
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
Q_ASSERT(insSettings);
UAVObjectField *field = insSettings->getField(QString("Algorithm"));
Q_ASSERT(field);
m_ui->algorithm->addItems(field->getOptions()); */
// Register for Home Location state changes
HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(homeLocation);
connect(homeLocation, SIGNAL(objectUpdated(UAVObject*)), this , SLOT(enableHomeLocSave(UAVObject*)));
// Don't enable multi-point calibration until HomeLocation is set.
HomeLocation::DataFields homeLocationData = homeLocation->getData();
m_ui->sixPointsStart->setEnabled(homeLocationData.Set == HomeLocation::SET_TRUE);
// Connect the signals // Connect the signals
connect(m_ui->ahrsCalibStart, SIGNAL(clicked()), this, SLOT(measureNoise())); connect(m_ui->ahrsCalibStart, SIGNAL(clicked()), this, SLOT(measureNoise()));
connect(m_ui->accelBiasStart, SIGNAL(clicked()), this, SLOT(launchAccelBiasCalibration())); connect(m_ui->accelBiasStart, SIGNAL(clicked()), this, SLOT(launchAccelBiasCalibration()));
connect(homeLocation, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshValues())); RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
connect(insSettings, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshValues())); Q_ASSERT(revoCalibration);
connect(revoCalibration, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshValues()));
connect(m_ui->ahrsSettingsSaveRAM, SIGNAL(clicked()), this, SLOT(ahrsSettingsSaveRAM())); connect(m_ui->ahrsSettingsSaveRAM, SIGNAL(clicked()), this, SLOT(SettingsToRam()));
connect(m_ui->ahrsSettingsSaveSD, SIGNAL(clicked()), this, SLOT(ahrsSettingsSaveSD())); connect(m_ui->ahrsSettingsSaveSD, SIGNAL(clicked()), this, SLOT(SettingsToFlash()));
connect(m_ui->sixPointsStart, SIGNAL(clicked()), this, SLOT(multiPointCalibrationMode())); connect(m_ui->sixPointsStart, SIGNAL(clicked()), this, SLOT(sixPointCalibrationMode()));
connect(m_ui->sixPointsSave, SIGNAL(clicked()), this, SLOT(savePositionData())); connect(m_ui->sixPointsSave, SIGNAL(clicked()), this, SLOT(savePositionData()));
connect(m_ui->startDriftCalib, SIGNAL(clicked()),this, SLOT(launchGyroDriftCalibration()));
// Leave this timer permanently connected. The timer itself is started and stopped. // Leave this timer permanently connected. The timer itself is started and stopped.
connect(&progressBarTimer, SIGNAL(timeout()), this, SLOT(incrementProgress())); connect(&progressBarTimer, SIGNAL(timeout()), this, SLOT(incrementProgress()));
@ -287,29 +269,29 @@ void ConfigRevoWidget::launchAccelBiasCalibration()
m_ui->accelBiasStart->setEnabled(false); m_ui->accelBiasStart->setEnabled(false);
m_ui->accelBiasProgress->setValue(0); m_ui->accelBiasProgress->setValue(0);
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager()); RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(insSettings); Q_ASSERT(revoCalibration);
InsSettings::DataFields insSettingsData = insSettings->getData(); RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
insSettingsData.BiasCorrectedRaw = InsSettings::BIASCORRECTEDRAW_FALSE; revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE;
insSettings->setData(insSettingsData); revoCalibration->setData(revoCalibrationData);
insSettings->updated(); revoCalibration->updated();
accel_accum_x.clear(); accel_accum_x.clear();
accel_accum_y.clear(); accel_accum_y.clear();
accel_accum_z.clear(); accel_accum_z.clear();
/* Need to get as many AttitudeRaw updates as possible */ /* Need to get as many AttitudeRaw updates as possible */
AttitudeRaw * attitudeRaw = AttitudeRaw::GetInstance(getObjectManager()); Accels * accels = Accels::GetInstance(getObjectManager());
Q_ASSERT(attitudeRaw); Q_ASSERT(accels);
initialMdata = attitudeRaw->getMetadata(); initialMdata = accels->getMetadata();
UAVObject::Metadata mdata = initialMdata; UAVObject::Metadata mdata = initialMdata;
mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
mdata.flightTelemetryUpdatePeriod = 100; mdata.flightTelemetryUpdatePeriod = 100;
attitudeRaw->setMetadata(mdata); accels->setMetadata(mdata);
// Now connect to the attituderaw updates, gather for 100 samples // Now connect to the accels and mag updates, gather for 100 samples
collectingData = true; collectingData = true;
connect(attitudeRaw, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(accelBiasattitudeRawUpdated(UAVObject*))); connect(accels, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(accelBiasattitudeRawUpdated(UAVObject*)));
} }
/** /**
@ -319,211 +301,40 @@ void ConfigRevoWidget::accelBiasattitudeRawUpdated(UAVObject *obj)
{ {
Q_UNUSED(obj); Q_UNUSED(obj);
AttitudeRaw * attitudeRaw = AttitudeRaw::GetInstance(getObjectManager()); Accels * accels = Accels::GetInstance(getObjectManager());
Q_ASSERT(attitudeRaw); Q_ASSERT(accels);
AttitudeRaw::DataFields attitudeRawData = attitudeRaw->getData(); Accels::DataFields accelsData = accels->getData();
// This is necessary to prevent a race condition on disconnect signal and another update // This is necessary to prevent a race condition on disconnect signal and another update
if (collectingData == true) { if (collectingData == true) {
accel_accum_x.append(attitudeRawData.accels[InsSettings::ACCEL_BIAS_X]); accel_accum_x.append(accelsData.x);
accel_accum_y.append(attitudeRawData.accels[InsSettings::ACCEL_BIAS_Y]); accel_accum_y.append(accelsData.y);
accel_accum_z.append(attitudeRawData.accels[InsSettings::ACCEL_BIAS_Z]); accel_accum_z.append(accelsData.z);
} }
m_ui->accelBiasProgress->setValue(m_ui->accelBiasProgress->value()+1); m_ui->accelBiasProgress->setValue(m_ui->accelBiasProgress->value()+1);
if(accel_accum_x.size() >= 100 && collectingData == true) { if(accel_accum_x.size() >= 100 && collectingData == true) {
collectingData = false; collectingData = false;
disconnect(attitudeRaw,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(accelBiasattitudeRawUpdated(UAVObject*))); disconnect(accels,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(accelBiasattitudeRawUpdated(UAVObject*)));
m_ui->accelBiasStart->setEnabled(true); m_ui->accelBiasStart->setEnabled(true);
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager()); RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(insSettings); Q_ASSERT(revoCalibration);
InsSettings::DataFields insSettingsData = insSettings->getData(); RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
insSettingsData.BiasCorrectedRaw = InsSettings::BIASCORRECTEDRAW_TRUE; revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE;
insSettingsData.accel_bias[InsSettings::ACCEL_BIAS_X] -= listMean(accel_accum_x); revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] -= listMean(accel_accum_x);
insSettingsData.accel_bias[InsSettings::ACCEL_BIAS_Y] -= listMean(accel_accum_y); revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] -= listMean(accel_accum_y);
insSettingsData.accel_bias[InsSettings::ACCEL_BIAS_Z] -= 9.81 + listMean(accel_accum_z); revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] -= GRAVITY + listMean(accel_accum_z);
insSettings->setData(insSettingsData); revoCalibration->setData(revoCalibrationData);
insSettings->updated(); revoCalibration->updated();
attitudeRaw->setMetadata(initialMdata); accels->setMetadata(initialMdata);
saveAHRSCalibration();
} }
} }
/**
Starts a Gyro temperature drift calibration.
*/
void ConfigRevoWidget::launchGyroDriftCalibration()
{
if (!collectingData) {
// m_ui->startDriftCalib->setEnabled(false);
m_ui->startDriftCalib->setText("Stop");
m_ui->accelBiasStart->setEnabled(false);
m_ui->ahrsCalibStart->setEnabled(false);
m_ui->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_ui->gyroTempSlider->setRange(temp*10,temp*10);
m_ui->gyroMaxTemp->setText(QString::number(temp,'g',3));
m_ui->gyroMinTemp->setText(QString::number(temp,'g',3));
connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(driftCalibrationAttitudeRawUpdated(UAVObject*)));
} else {
// Stop all the gathering:
collectingData = false;
m_ui->startDriftCalib->setText("Start");
m_ui->accelBiasStart->setEnabled(true);
m_ui->ahrsCalibStart->setEnabled(true);
m_ui->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 ConfigRevoWidget::driftCalibrationAttitudeRawUpdated(UAVObject* obj) {
Q_UNUSED(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_ui->gyroTempSlider->minimum()) {
m_ui->gyroTempSlider->setMinimum(mbTemp*10);
m_ui->gyroMinTemp->setText(QString::number(mbTemp,'g',3));
} else if (mbTemp*10 > m_ui->gyroTempSlider->maximum()) {
m_ui->gyroTempSlider->setMaximum(mbTemp*10);
m_ui->gyroMaxTemp->setText(QString::number(mbTemp,'g',3));
}
m_ui->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 ConfigRevoWidget::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...
//
// Hint for whoever wants to implement that:
// The formula I use for computing the temperature compensation factor from
// two nicely filtered (downsampled) sample points is as follows:
//
// gyro_tempcompfactor == -(raw_gyro1 - raw_gyro2)/(gyro_temp1 - gyro_temp2)
//
// where raw_gyro1 and raw_gyro2 are gyroscope raw measurement values and
// gyro_temp1 and gyro_temp2 are the measurements from the gyroscope internal
// temperature sensors, each at two measure points T1 and T2
// note that the X and Y gyroscopes share one temperature sensor while
// Z has its own.
//
// the formula that calculates the AttitudeRav.gyros[X,Y,Z] values is
// currently as follows:
//
// gyro = 180/Pi * ( ( ( raw_gyro + raw_gyro * gyro_tempcompfactor ) * gyro_scale) + gyro_bias )
//
// so to get gyro_raw do the following:
// 1. set AHRSSettings.BiasCorrectedRaw to FALSE before measuring! (already done right now)
// 2. set AHRSCalibration.gyro_tempcompfactor to 0 before measuring!
// 3. gyro_raw = ( ( gyro * Pi / 180 ) - gyro_bias ) / gyro_scale
//
// a nice trick is to set gyro_bias to 0 and gyro_scale to (Pi / 180) in which case gyro = raw_gyro
// note that Pi/180 is very close to the "real" scale of the AHRS gyros anyway (though with switched signs)
}
/**
Launches the INS sensors noise measurements
*/
void ConfigRevoWidget::measureNoise()
{
if(collectingData) {
QErrorMessage err(this);
err.showMessage("Cannot start noise measurement as data already being gathered");
err.exec();
return;
}
m_ui->calibInstructions->setText("Estimating sensor variance...");
m_ui->ahrsCalibStart->setEnabled(false);
m_ui->calibProgress->setValue(0);
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
InsSettings::DataFields insSettingsData = insSettings->getData();
algorithm = insSettingsData.Algorithm;
insSettingsData.Algorithm = InsSettings::ALGORITHM_CALIBRATION;
insSettings->setData(insSettingsData);
insSettings->updated();
collectingData = true;
connect(insSettings,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(noiseMeasured()));
m_ui->calibProgress->setRange(0,calibrationDelay*10);
progressBarTimer.start(100);
}
/** /**
Increment progress bar for noise measurements (not really based on feedback) Increment progress bar for noise measurements (not really based on feedback)
*/ */
@ -533,9 +344,9 @@ void ConfigRevoWidget::incrementProgress()
if (m_ui->calibProgress->value() >= m_ui->calibProgress->maximum()) { if (m_ui->calibProgress->value() >= m_ui->calibProgress->maximum()) {
progressBarTimer.stop(); progressBarTimer.stop();
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager()); RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(insSettings); Q_ASSERT(revoCalibration);
disconnect(insSettings, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(noiseMeasured())); disconnect(revoCalibration, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(noiseMeasured()));
collectingData = false; collectingData = false;
QErrorMessage err(this); QErrorMessage err(this);
@ -544,137 +355,83 @@ void ConfigRevoWidget::incrementProgress()
} }
} }
/** void ConfigRevoWidget::sensorsUpdated(UAVObject * obj)
*@brief Callback once calibration is done on the board. Restores the original algorithm.
*/
void ConfigRevoWidget::noiseMeasured()
{
Q_ASSERT(collectingData); // Let's catch any race conditions
// Do all the clean stopping stuff
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
Q_ASSERT(insSettings);
disconnect(insSettings, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(noiseMeasured()));
collectingData = false;
progressBarTimer.stop();
m_ui->calibProgress->setValue(m_ui->calibProgress->maximum());
InsSettings::DataFields insSettingsData = insSettings->getData();
insSettingsData.Algorithm = algorithm;
insSettings->setData(insSettingsData);
insSettings->updated();
m_ui->calibInstructions->setText(QString("Calibration complete."));
m_ui->ahrsCalibStart->setEnabled(true);
}
/**
Saves the AHRS sensors calibration (to RAM and SD)
*/
void ConfigRevoWidget::saveAHRSCalibration()
{
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
saveObjectToSD(insSettings);
}
FORCE_ALIGN_FUNC
void ConfigRevoWidget::attitudeRawUpdated(UAVObject * obj)
{ {
QMutexLocker lock(&attitudeRawUpdateLock); QMutexLocker lock(&attitudeRawUpdateLock);
UAVObjectField *accel_field = obj->getField(QString("accels")); qDebug() << "Data";
UAVObjectField *gyro_field = obj->getField(QString("gyros"));
UAVObjectField *mag_field = obj->getField(QString("magnetometers"));
Q_ASSERT(gyro_field != 0 && accel_field != 0 && mag_field != 0);
// This is necessary to prevent a race condition on disconnect signal and another update // This is necessary to prevent a race condition on disconnect signal and another update
if (collectingData == true) { if (collectingData == true) {
accel_accum_x.append(accel_field->getValue(0).toDouble()); qDebug() << "Collecting";
accel_accum_y.append(accel_field->getValue(1).toDouble()); if( obj->getObjID() == Accels::OBJID ) {
accel_accum_z.append(accel_field->getValue(2).toDouble()); qDebug() << "Accels";
// Note gyros actually (-y,-x,-z) but since we consistent here no prob Accels * accels = Accels::GetInstance(getObjectManager());
mag_accum_x.append(mag_field->getValue(0).toDouble()); Q_ASSERT(accels);
mag_accum_y.append(mag_field->getValue(1).toDouble()); Accels::DataFields accelsData = accels->getData();
mag_accum_z.append(mag_field->getValue(2).toDouble()); accel_accum_x.append(accelsData.x);
accel_accum_y.append(accelsData.y);
gyro_accum_x.append(gyro_field->getValue(0).toDouble()); accel_accum_z.append(accelsData.z);
gyro_accum_y.append(gyro_field->getValue(1).toDouble()); } else if( obj->getObjID() == Magnetometer::OBJID ) {
gyro_accum_z.append(gyro_field->getValue(2).toDouble()); qDebug() << "Mag";
Magnetometer * mag = Magnetometer::GetInstance(getObjectManager());
Q_ASSERT(mag);
Magnetometer::DataFields magData = mag->getData();
mag_accum_x.append(magData.x);
mag_accum_y.append(magData.y);
mag_accum_z.append(magData.z);
} else {
Q_ASSERT(0);
}
} }
if(accel_accum_x.size() >= 8 && collectingData == true) { if(accel_accum_x.size() >= 20 && mag_accum_x.size() >= 20 && collectingData == true) {
collectingData = false; collectingData = false;
disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(attitudeRawUpdated(UAVObject*)));
Accels * accels = Accels::GetInstance(getObjectManager());
Q_ASSERT(accels);
Magnetometer * mag = Magnetometer::GetInstance(getObjectManager());
Q_ASSERT(mag);
disconnect(accels,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*)));
disconnect(mag,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*)));
m_ui->sixPointsSave->setEnabled(true); m_ui->sixPointsSave->setEnabled(true);
accel_data[position] << listMean(accel_accum_x), accel_data_x[position] = listMean(accel_accum_x);
listMean(accel_accum_y), accel_data_y[position] = listMean(accel_accum_y);
listMean(accel_accum_z); accel_data_z[position] = listMean(accel_accum_z);
mag_data_x[position] = listMean(mag_accum_x);
mag_data_y[position] = listMean(mag_accum_y);
mag_data_z[position] = listMean(mag_accum_z);
mag_data[position] << listMean(mag_accum_x), position = (position + 1) % 6;
listMean(mag_accum_y), if(position == 1) {
listMean(mag_accum_z); m_ui->sixPointCalibInstructions->append("Place with left side down and click save position...");
displayPlane("plane-left");
gyro_data[position] << listMean(gyro_accum_x),
listMean(gyro_accum_y),
listMean(gyro_accum_z);
std::cout << "observed accel: " << accel_data[position].transpose()
<< "\nobserved mag: " << mag_data[position].transpose()
<< "\nobserved gyro: " << gyro_data[position].transpose()
<< std::endl;
struct {
const char* instructions;
const char* display;
} instructions[] = {
{ "Pitch up 45 deg and click save position...", "plane-horizontal" },
{ "Pitch down 45 deg and click save position...", "plane-horizontal" },
{ "Roll left 45 deg and click save position...", "plane-left" },
{ "Roll right 45 deg and click save position...", "plane-left" },
{ "Turn left 90 deg to 09:00 position and click save position...", "plane-horizontal" },
{ "Pitch up 45 deg and click save position...", "plane-horizontal" },
{ "Pitch down 45 deg and click save position...", "plane-horizontal" },
{ "Roll left 45 deg and click save position...", "plane-left" },
{ "Roll right 45 deg and click save position...", "plane-left" },
{ "Turn left 90 deg to 06:00 position and click save position...", "plane-horizontal" },
{ "Pitch up 45 deg and click save position...", "plane-horizontal" },
{ "Pitch down 45 deg and click save position...", "plane-horizontal" },
{ "Roll left 45 deg and click save position...", "plane-left" },
{ "Roll right 45 deg and click save position...", "plane-left" },
{ "Turn left 90 deg to 03:00 position and click save position...", "plane-horizontal" },
{ "Pitch up 45 deg and click save position...", "plane-horizontal" },
{ "Pitch down 45 deg and click save position...", "plane-horizontal" },
{ "Roll left 45 deg and click save position...", "plane-left" },
{ "Roll right 45 deg and click save position...", "plane-left" },
{ "Place with nose vertically up and click save position...", "plane-up" },
{ "Place with nose straight down and click save position...", "plane-down" },
{ "Place upside down and click save position...", "plane-flip" },
};
n_positions = sizeof(instructions) / sizeof(instructions[0]);
position = (position + 1) % n_positions;
if (position != 0 && position < n_positions) {
m_ui->sixPointCalibInstructions->append(instructions[position-1].instructions);
displayPlane(instructions[position-1].display);
} }
else if(position == 0) { if(position == 2) {
position = n_positions; m_ui->sixPointCalibInstructions->append("Place upside down and click save position...");
displayPlane("plane-flip");
}
if(position == 3) {
m_ui->sixPointCalibInstructions->append("Place with right side down and click save position...");
displayPlane("plane-right");
}
if(position == 4) {
m_ui->sixPointCalibInstructions->append("Place with nose up and click save position...");
displayPlane("plane-up");
}
if(position == 5) {
m_ui->sixPointCalibInstructions->append("Place with nose down and click save position...");
displayPlane("plane-down");
}
if(position == 0) {
computeScaleBias(); computeScaleBias();
m_ui->sixPointsStart->setEnabled(true); m_ui->sixPointsStart->setEnabled(true);
m_ui->sixPointsSave->setEnabled(false); m_ui->sixPointsSave->setEnabled(false);
saveAHRSCalibration(); // Saves the result to SD.
/* Cleanup original settings */ /* Cleanup original settings */
getObjectManager()->getObject(QString("AttitudeRaw"))->setMetadata(initialMdata); accels->setMetadata(initialMdata);
mag->setMetadata(initialMdata);
} }
} }
} }
@ -698,395 +455,221 @@ void ConfigRevoWidget::savePositionData()
gyro_accum_z.clear(); gyro_accum_z.clear();
collectingData = true; collectingData = true;
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeRaw")));
connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(attitudeRawUpdated(UAVObject*))); Accels * accels = Accels::GetInstance(getObjectManager());
Q_ASSERT(accels);
Magnetometer * mag = Magnetometer::GetInstance(getObjectManager());
Q_ASSERT(mag);
connect(accels, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(sensorsUpdated(UAVObject*)));
connect(mag, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(sensorsUpdated(UAVObject*)));
m_ui->sixPointCalibInstructions->append("Hold..."); m_ui->sixPointCalibInstructions->append("Hold...");
} }
//***************************************************************** int LinearEquationsSolving(int nDim, double* pfMatr, double* pfVect, double* pfSolution)
namespace {
/*
* Calibrated scale factors should be real values with scale factor less than 10% from nominal
*/
bool checkScaleFactors(const Vector3f& scalars)
{ {
return isReal(scalars) && double fMaxElem;
scalars.cwise().abs().maxCoeff() < 1.10f; double fAcc;
int i , j, k, m;
for(k=0; k<(nDim-1); k++) // base row of matrix
{
// search of line with max element
fMaxElem = fabs( pfMatr[k*nDim + k] );
m = k;
for(i=k+1; i<nDim; i++)
{
if(fMaxElem < fabs(pfMatr[i*nDim + k]) )
{
fMaxElem = pfMatr[i*nDim + k];
m = i;
}
}
// permutation of base line (index k) and max element line(index m)
if(m != k)
{
for(i=k; i<nDim; i++)
{
fAcc = pfMatr[k*nDim + i];
pfMatr[k*nDim + i] = pfMatr[m*nDim + i];
pfMatr[m*nDim + i] = fAcc;
}
fAcc = pfVect[k];
pfVect[k] = pfVect[m];
pfVect[m] = fAcc;
}
if( pfMatr[k*nDim + k] == 0.) return 0; // needs improvement !!!
// triangulation of matrix with coefficients
for(j=(k+1); j<nDim; j++) // current row of matrix
{
fAcc = - pfMatr[j*nDim + k] / pfMatr[k*nDim + k];
for(i=k; i<nDim; i++)
{
pfMatr[j*nDim + i] = pfMatr[j*nDim + i] + fAcc*pfMatr[k*nDim + i];
}
pfVect[j] = pfVect[j] + fAcc*pfVect[k]; // free member recalculation
}
}
for(k=(nDim-1); k>=0; k--)
{
pfSolution[k] = pfVect[k];
for(i=(k+1); i<nDim; i++)
{
pfSolution[k] -= (pfMatr[k*nDim + i]*pfSolution[i]);
}
pfSolution[k] = pfSolution[k] / pfMatr[k*nDim + k];
}
return 1;
} }
/*
* Calibrated offsets should be real values. TODO: Add range checks int SixPointInConstFieldCal( double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3] )
*/
bool checkOffsets(const Vector3f& offsets)
{ {
return isReal(offsets); int i;
double A[5][5];
double f[5], c[5];
double xp, yp, zp, Sx;
// Fill in matrix A -
// write six difference-in-magnitude equations of the form
// Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0
// or in other words
// 2*Sx*bx*(x2-x1)/Sx^2 + Sy^2(y2^2-y1^2)/Sx^2 + 2*Sy*by*(y2-y1)/Sx^2 + Sz^2(z2^2-z1^2)/Sx^2 + 2*Sz*bz*(z2-z1)/Sx^2 = (x1^2-x2^2)
for (i=0;i<5;i++){
A[i][0] = 2.0 * (x[i+1] - x[i]);
A[i][1] = y[i+1]*y[i+1] - y[i]*y[i];
A[i][2] = 2.0 * (y[i+1] - y[i]);
A[i][3] = z[i+1]*z[i+1] - z[i]*z[i];
A[i][4] = 2.0 * (z[i+1] - z[i]);
f[i] = x[i]*x[i] - x[i+1]*x[i+1];
}
// solve for c0=bx/Sx, c1=Sy^2/Sx^2; c2=Sy*by/Sx^2, c3=Sz^2/Sx^2, c4=Sz*bz/Sx^2
if ( !LinearEquationsSolving( 5, (double *)A, f, c) ) return 0;
// use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer
xp = x[0]; yp = y[0]; zp = z[0];
Sx = sqrt(ConstMag*ConstMag / (xp*xp + 2*c[0]*xp + c[0]*c[0] + c[1]*yp*yp + 2*c[2]*yp + c[2]*c[2]/c[1] + c[3]*zp*zp + 2*c[4]*zp + c[4]*c[4]/c[3]));
S[0] = Sx;
b[0] = Sx*c[0];
S[1] = sqrt(c[1]*Sx*Sx);
b[1] = c[2]*Sx*Sx/S[1];
S[2] = sqrt(c[3]*Sx*Sx);
b[2] = c[4]*Sx*Sx/S[2];
return 1;
} }
/**
* Given a UAVObjectField that is a 3-tuple, produce an Eigen::Vector3f
* from it.
*/
Vector3f
tupleToVector(UAVObjectField *tuple)
{
return (Vector3f() << tuple->getDouble(0),
tuple->getDouble(1),
tuple->getDouble(2)).finished();
}
/**
* Convert a 3-vector to a 3-tuple
* @param v A 3-vector
* @param tuple[in] Assign the elements of this three-tuple to the elements of v
*/
void
vectorToTuple(UAVObjectField *tuple, const Vector3f& v)
{
for (int i = 0; i < 3; ++i) {
tuple->setDouble(v(i), i);
}
}
/**
* Updates the offsets for a calibrated gyro field.
* @param scale[in] Non-null pointer to a 3-element scale factor field.
* @param bias[out] Non-null pointer to a 3-element bias field.
* @param updateBias the source bias matrix.
*/
void
updateBias(UAVObjectField *scale,
UAVObjectField *bias ,
const Vector3f& updateBias)
{
Vector3f scale_factor = (Vector3f() << scale->getDouble(0),
scale->getDouble(1),
scale->getDouble(2)).finished();
Vector3f old_bias = (Vector3f() << bias->getDouble(0),
bias->getDouble(1),
bias->getDouble(2)).finished();
// Convert to radians/second
Vector3f final_bias = -(M_PI)/180.0f * updateBias + old_bias;
bias->setDouble(final_bias(0), 0);
bias->setDouble(final_bias(1), 1);
bias->setDouble(final_bias(2), 2);
}
void
updateRotation(UAVObjectField *rotation, const Vector3f& updateRotation)
{
for (int i = 0; i < 3; ++i) {
rotation->setDouble(updateRotation[i], i);
}
}
} // !namespace (anon)
/**
* Updates the scale factors and offsets for a calibrated vector field.
* @param scale[out] Non-null pointer to a 3-element scale factor field.
* @param bias[out] Non-null pointer to a 3-element bias field.
* @param ortho[out] Optional pointer to a 3-element orthogonal correction field
* @param updateScale the source scale factor matrix.
* @param updateBias the source bias matrix.
* @param oldScale The original sensor scale factor
* @param oldBias The original bias value
* @param oldOrtho Optional. The original orthogonality scale factor value.
* @return true if successful, false otherwise.
*/
bool
ConfigRevoWidget::updateScaleFactors(UAVObjectField *scale,
UAVObjectField *bias ,
UAVObjectField *ortho,
const Matrix3f& updateScale,
const Vector3f& updateBias,
const Vector3f& oldScale,
const Vector3f& oldBias,
const Vector3f& oldOrtho)
{
// Compose a 4x4 affine transformation matrix composed of the scale factor,
// orthogonality correction, and bias.
Matrix4f calibration;
calibration << tupleToVector(scale).asDiagonal(),
tupleToVector(bias),
Vector4f::UnitW().transpose();
if (ortho) {
Vector3f orthof = tupleToVector(ortho);
calibration(1, 0) = calibration(0, 1) = orthof(0);
calibration(2, 0) = calibration(0, 2) = orthof(1);
calibration(1, 2) = calibration(2, 1) = orthof(2);
}
std::cout << "old calibration matrix: \n" << calibration << "\n";
Matrix4f update;
update << updateScale, updateBias, Vector4f::UnitW().transpose();
std::cout << "new calibration matrix update: \n" << update << "\n";
calibration = update * calibration;
if (checkOffsets(updateBias) && checkScaleFactors(updateScale.diagonal())) {
// Apply the new calibration
vectorToTuple(scale, calibration.diagonal().start<3>());
vectorToTuple(bias, calibration.col(3).start<3>());
if (ortho) {
ortho->setDouble(calibration(0, 1), 0);
ortho->setDouble(calibration(0, 2), 1);
ortho->setDouble(calibration(1, 2), 2);
}
return true;
}
else {
// Give the user the calibration data and restore their settings.
std::ostringstream msg;
msg << "Scale factors and/or offsets are out of range.\n";
msg << "Please see the troubleshooting section of the manual and retry.\n\n"
"The following values were computed:\n";
msg << qPrintable(scale->getName()) << ": "
<< calibration.diagonal().start<3>().transpose() << "\n";
vectorToTuple(scale, oldScale);
if (ortho) {
msg << qPrintable(ortho->getName()) << ": "
<< calibration(0,1) << ", " << calibration(0,2) << ", " << calibration(1,2) << "\n";
vectorToTuple(ortho, oldOrtho);
}
msg << qPrintable(bias->getName()) << ": "
<< calibration.col(3).start<3>().transpose() << "\n";
vectorToTuple(bias, oldBias);
m_ui->sixPointCalibInstructions->append(msg.str().c_str());
return false;
}
}
FORCE_ALIGN_FUNC
void ConfigRevoWidget::computeScaleBias() void ConfigRevoWidget::computeScaleBias()
{ {
// Extract the local magnetic and gravitational field vectors from HomeLocation. double S[3], b[3];
UAVObject *home = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("HomeLocation"))); RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Vector3f localMagField; Q_ASSERT(revoCalibration);
localMagField << home->getField("Be")->getValue(0).toDouble(), RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
home->getField("Be")->getValue(1).toDouble(),
home->getField("Be")->getValue(2).toDouble();
float localGravity = home->getField("g_e")->getDouble(); // Calibration accel
SixPointInConstFieldCal( GRAVITY, accel_data_x, accel_data_y, accel_data_z, S, b);
revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X] = fabs(S[0]);
revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y] = fabs(S[1]);
revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z] = fabs(S[2]);
Vector3f referenceField = Vector3f::UnitZ()*localGravity; revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] = sign(S[0]) * b[0];
double noise = 0.04; revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] = sign(S[1]) * b[1];
Vector3f accelBias; revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] = sign(S[2]) * b[2];
Matrix3f accelScale;
std::cout << "number of samples: " << n_positions << "\n";
twostep_bias_scale(accelBias, accelScale, accel_data, n_positions, referenceField, noise*noise);
// Twostep computes an offset from the identity scalar, and a negative bias offset
accelScale += Matrix3f::Identity();
accelBias = -accelBias;
std::cout << "computed accel bias: " << accelBias.transpose()
<< "\ncomputed accel scale:\n" << accelScale<< std::endl;
// Apply the computed scale factor and bias to each sample // Calibration mag
for (int i = 0; i < n_positions; ++i) { SixPointInConstFieldCal( 1000, mag_data_x, mag_data_y, mag_data_z, S, b);
accel_data[i] = accelScale * accel_data[i] + accelBias; revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] = fabs(S[0]);
} revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] = fabs(S[1]);
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] = fabs(S[2]);
// Magnetometer has excellent orthogonality, so only calibrate the scale factors. revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] = sign(S[0]) * b[0];
Vector3f magBias; revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = sign(S[1]) * b[1];
Vector3f magScale; revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = sign(S[2]) * b[2];
noise = 4.0;
twostep_bias_scale(magBias, magScale, mag_data, n_positions, localMagField, noise*noise);
magScale += Vector3f::Ones();
magBias = -magBias;
std::cout << "computed mag bias: " << magBias.transpose()
<< "\ncomputed mag scale:\n" << magScale << std::endl;
// Apply the computed scale factor and bias to each sample revoCalibration->setData(revoCalibrationData);
for (int i = 0; i < n_positions; ++i) {
mag_data[i] = magScale.asDiagonal() * mag_data[i] + magBias;
}
// Calibrate gyro bias and acceleration sensitivity position = -1; //set to run again
Matrix3f accelSensitivity; m_ui->sixPointCalibInstructions->append("Computed accel and mag scale and bias...");
Vector3f gyroBias;
gyroscope_calibration(gyroBias, accelSensitivity, gyro_data, accel_data, n_positions);
std::cout << "gyro bias: " << gyroBias.transpose()
<< "\ngyro's acceleration sensitivity:\n" << accelSensitivity << std::endl;
// Calibrate alignment between the accelerometer and magnetometer, taking the mag as the
// reference.
Vector3f accelRotation;
calibration_misalignment(accelRotation, accel_data, -Vector3f::UnitZ()*localGravity,
mag_data, localMagField, n_positions);
std::cout << "magnetometer rotation vector: " << accelRotation.transpose() << std::endl;
// Update the calibration scalars with a clear message box
m_ui->sixPointCalibInstructions->clear();
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration")));
bool success = updateScaleFactors(obj->getField(QString("accel_scale")),
obj->getField(QString("accel_bias")),
obj->getField(QString("accel_ortho")),
accelScale,
accelBias,
saved_accel_scale,
saved_accel_bias,
saved_accel_ortho);
success &= updateScaleFactors(obj->getField(QString("mag_scale")),
obj->getField(QString("mag_bias")),
NULL,
magScale.asDiagonal(),
magBias,
saved_mag_scale,
saved_mag_bias);
updateBias(obj->getField(QString("gyro_scale")),
obj->getField(QString("gyro_bias")),
gyroBias);
#if 0
// TODO: Enable after v1.0 feature freeze is lifted.
updateRotation(obj->getField(QString("accel_rotation")), accelRotation);
#endif
obj->updated();
position = -1; //set to run again
if (success)
m_ui->sixPointCalibInstructions->append("Computed new accel and mag scale and bias.");
} }
/** /**
Multi-point calibration mode Six point calibration mode
*/ */
FORCE_ALIGN_FUNC void ConfigRevoWidget::sixPointCalibrationMode()
void ConfigRevoWidget::multiPointCalibrationMode()
{ {
cacheCurrentCalibration(); double S[3], b[3];
resetCalibrationDefaults(); RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
UAVObject* obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings"))); // Calibration accel
UAVObjectField *field = obj->getField(QString("BiasCorrectedRaw")); revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X] = 1;
field->setValue("FALSE"); revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y] = 1;
obj->updated(); revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z] = 1;
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] = 0;
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] = 0;
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] = 0;
// Calibration mag
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] = 1;
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] = 1;
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] = 1;
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] = 0;
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = 0;
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = 0;
Thread::usleep(100000); revoCalibration->setData(revoCalibrationData);
gyro_accum_x.clear(); usleep(100000);
gyro_accum_y.clear();
gyro_accum_z.clear();
/* Need to get as many AttitudeRaw updates as possible */ gyro_accum_x.clear();
obj = getObjectManager()->getObject(QString("AttitudeRaw")); gyro_accum_y.clear();
initialMdata = obj->getMetadata(); gyro_accum_z.clear();
UAVObject::Metadata mdata = initialMdata; accel_accum_x.clear();
mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; accel_accum_y.clear();
mdata.flightTelemetryUpdatePeriod = 100; accel_accum_z.clear();
obj->setMetadata(mdata); mag_accum_x.clear();
mag_accum_y.clear();
mag_accum_z.clear();
/* Show instructions and enable controls */ /* Need to get as many accel and mag updates as possible */
m_ui->sixPointCalibInstructions->clear(); Accels * accels = Accels::GetInstance(getObjectManager());
m_ui->sixPointCalibInstructions->append("Stand facing Earth's magnetic N or S. Place the vehicle horizontally facing forward and click save position..."); Q_ASSERT(accels);
displayPlane("plane-horizontal"); Magnetometer * mag = Magnetometer::GetInstance(getObjectManager());
m_ui->sixPointsStart->setEnabled(false); Q_ASSERT(mag);
m_ui->sixPointsSave->setEnabled(true);
position = 0;
initialMdata = accels->getMetadata();
UAVObject::Metadata mdata = initialMdata;
mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
mdata.flightTelemetryUpdatePeriod = 100;
accels->setMetadata(mdata);
mdata = mag->getMetadata();
mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
mdata.flightTelemetryUpdatePeriod = 100;
mag->setMetadata(mdata);
/* Show instructions and enable controls */
m_ui->sixPointCalibInstructions->clear();
m_ui->sixPointCalibInstructions->append("Place horizontally and click save position...");
displayPlane("plane-horizontal");
m_ui->sixPointsStart->setEnabled(false);
m_ui->sixPointsSave->setEnabled(true);
position = 0;
qDebug() << "Starting";
} }
/**
* Read the current calibration scalars and offsets from the target board, and
* save them for later use. In the event of a calibration failure, if the
* calibration method began by resetting calibration values, they may be
* restored later with this information.
*/
void ConfigRevoWidget::cacheCurrentCalibration()
{
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration")));
struct field_t {
const char* field_name;
Vector3f& cache;
} fields[] = {
{ "accel_scale", this->saved_accel_scale },
{ "accel_bias", this->saved_accel_bias },
{ "accel_ortho", this->saved_accel_ortho },
// TODO: Enable after V1.0 feature freeze is lifted.
// { "accel_rotation", this->saved_accel_rotation },
{ "gyro_bias", this->saved_gyro_bias },
{ "mag_scale", this->saved_mag_scale },
{ "mag_bias", this->saved_mag_bias },
{ NULL, this->saved_mag_bias }, // sentinnel
};
for (field_t* i = fields; i->field_name != NULL; ++i) {
UAVObjectField* field = obj->getField(QString(i->field_name));
if (field) {
i->cache = tupleToVector(field);
}
else {
qDebug() << "WARNING: AHRSCalibration field not found: " << i->field_name << "\n";
}
}
}
/**
* Reset all calibration scalars to their default values.
*/
void ConfigRevoWidget::resetCalibrationDefaults()
{
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration")));
// set accels to unity gain
UAVObjectField *field = obj->getField(QString("accel_scale"));
// TODO: Figure out how to load these directly from the saved metadata
// about default values
field->setDouble(0.0359, 0);
field->setDouble(0.0359, 1);
field->setDouble(0.0359, 2);
field = obj->getField(QString("accel_bias"));
field->setDouble(-73.5, 0);
field->setDouble(-73.5, 1);
field->setDouble(73.5, 2);
field = obj->getField(QString("accel_ortho"));
for (int i = 0; i < 3; ++i) {
field->setDouble(0, i);
}
field = obj->getField(QString("gyro_bias"));
UAVObjectField *field2 = obj->getField(QString("gyro_scale"));
field->setDouble(28/-0.017*field2->getDouble(0),0);
field->setDouble(-28/0.017*field2->getDouble(1),1);
field->setDouble(28/-0.017*field2->getDouble(2),2);
field = obj->getField(QString("mag_scale"));
for (int i = 0; i < 3; ++i) {
field->setDouble(-1, i);
}
field = obj->getField(QString("mag_bias"));
for (int i = 0; i < 3; ++i) {
field->setDouble(0, i);
}
#if 0
// TODO: Enable after v1.0 feature freeze is lifted.
field = obj->getField(QString("accel_rotation"));
for (int i = 0; i < 3; ++i) {
field->setDouble(0, i);
}
#endif
obj->updated();
}
/** /**
Rotate the paper plane Rotate the paper plane
@ -1105,32 +688,32 @@ void ConfigRevoWidget::displayPlane(QString elementID)
*/ */
void ConfigRevoWidget::drawVariancesGraph() void ConfigRevoWidget::drawVariancesGraph()
{ {
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager()); RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(insSettings); Q_ASSERT(revoCalibration);
InsSettings::DataFields insSettingsData = insSettings->getData(); RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
// The expected range is from 1E-6 to 1E-1 // The expected range is from 1E-6 to 1E-1
double steps = 6; // 6 bars on the graph double steps = 6; // 6 bars on the graph
float accel_x_var = -1/steps*(1+steps+log10(insSettingsData.accel_var[InsSettings::ACCEL_VAR_X])); float accel_x_var = -1/steps*(1+steps+log10(revoCalibrationData.accel_var[RevoCalibration::ACCEL_VAR_X]));
accel_x->setTransform(QTransform::fromScale(1,accel_x_var),false); accel_x->setTransform(QTransform::fromScale(1,accel_x_var),false);
float accel_y_var = -1/steps*(1+steps+log10(insSettingsData.accel_var[InsSettings::ACCEL_VAR_Y])); float accel_y_var = -1/steps*(1+steps+log10(revoCalibrationData.accel_var[RevoCalibration::ACCEL_VAR_Y]));
accel_y->setTransform(QTransform::fromScale(1,accel_y_var),false); accel_y->setTransform(QTransform::fromScale(1,accel_y_var),false);
float accel_z_var = -1/steps*(1+steps+log10(insSettingsData.accel_var[InsSettings::ACCEL_VAR_Z])); float accel_z_var = -1/steps*(1+steps+log10(revoCalibrationData.accel_var[RevoCalibration::ACCEL_VAR_Z]));
accel_z->setTransform(QTransform::fromScale(1,accel_z_var),false); accel_z->setTransform(QTransform::fromScale(1,accel_z_var),false);
float gyro_x_var = -1/steps*(1+steps+log10(insSettingsData.gyro_var[InsSettings::GYRO_VAR_X])); float gyro_x_var = -1/steps*(1+steps+log10(revoCalibrationData.gyro_var[RevoCalibration::GYRO_VAR_X]));
gyro_x->setTransform(QTransform::fromScale(1,gyro_x_var),false); gyro_x->setTransform(QTransform::fromScale(1,gyro_x_var),false);
float gyro_y_var = -1/steps*(1+steps+log10(insSettingsData.gyro_var[InsSettings::GYRO_VAR_Y])); float gyro_y_var = -1/steps*(1+steps+log10(revoCalibrationData.gyro_var[RevoCalibration::GYRO_VAR_Y]));
gyro_y->setTransform(QTransform::fromScale(1,gyro_y_var),false); gyro_y->setTransform(QTransform::fromScale(1,gyro_y_var),false);
float gyro_z_var = -1/steps*(1+steps+log10(insSettingsData.gyro_var[InsSettings::GYRO_VAR_Z])); float gyro_z_var = -1/steps*(1+steps+log10(revoCalibrationData.gyro_var[RevoCalibration::GYRO_VAR_Z]));
gyro_z->setTransform(QTransform::fromScale(1,gyro_z_var),false); gyro_z->setTransform(QTransform::fromScale(1,gyro_z_var),false);
// Scale by 1e-3 because mag vars are much higher. // Scale by 1e-3 because mag vars are much higher.
float mag_x_var = -1/steps*(1+steps+log10(1e-3*insSettingsData.mag_var[InsSettings::MAG_VAR_X])); float mag_x_var = -1/steps*(1+steps+log10(1e-3*revoCalibrationData.mag_var[RevoCalibration::MAG_VAR_X]));
mag_x->setTransform(QTransform::fromScale(1,mag_x_var),false); mag_x->setTransform(QTransform::fromScale(1,mag_x_var),false);
float mag_y_var = -1/steps*(1+steps+log10(1e-3*insSettingsData.mag_var[InsSettings::MAG_VAR_Y])); float mag_y_var = -1/steps*(1+steps+log10(1e-3*revoCalibrationData.mag_var[RevoCalibration::MAG_VAR_Y]));
mag_y->setTransform(QTransform::fromScale(1,mag_y_var),false); mag_y->setTransform(QTransform::fromScale(1,mag_y_var),false);
float mag_z_var = -1/steps*(1+steps+log10(1e-3*insSettingsData.mag_var[InsSettings::MAG_VAR_Z])); float mag_z_var = -1/steps*(1+steps+log10(1e-3*revoCalibrationData.mag_var[RevoCalibration::MAG_VAR_Z]));
mag_z->setTransform(QTransform::fromScale(1,mag_z_var),false); mag_z->setTransform(QTransform::fromScale(1,mag_z_var),false);
} }
@ -1139,17 +722,8 @@ void ConfigRevoWidget::drawVariancesGraph()
*/ */
void ConfigRevoWidget::refreshValues() void ConfigRevoWidget::refreshValues()
{ {
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
Q_ASSERT(insSettings);
InsSettings::DataFields insSettingsData = insSettings->getData();
m_ui->algorithm->setCurrentIndex(insSettingsData.Algorithm);
drawVariancesGraph(); drawVariancesGraph();
HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(homeLocation);
HomeLocation::DataFields homeLocationData = homeLocation->getData();
m_ui->homeLocationSet->setEnabled(homeLocationData.Set == HomeLocation::SET_TRUE);
m_ui->ahrsCalibStart->setEnabled(true); m_ui->ahrsCalibStart->setEnabled(true);
m_ui->sixPointsStart->setEnabled(true); m_ui->sixPointsStart->setEnabled(true);
m_ui->accelBiasStart->setEnabled(true); m_ui->accelBiasStart->setEnabled(true);
@ -1158,60 +732,32 @@ void ConfigRevoWidget::refreshValues()
m_ui->calibInstructions->setText(QString("Press \"Start\" above to calibrate.")); m_ui->calibInstructions->setText(QString("Press \"Start\" above to calibrate."));
} }
/**
Enables/disables the Home Location saving button depending on whether the
home location is set-able
*/
void ConfigRevoWidget::enableHomeLocSave(UAVObject * obj)
{
UAVObjectField *field = obj->getField(QString("Set"));
if (field) {
m_ui->homeLocationSet->setEnabled(field->getValue().toBool());
m_ui->sixPointsStart->setEnabled(obj->getField("Set")->getValue().toBool());
}
}
/** /**
Save current settings to RAM Save current settings to RAM
*/ */
void ConfigRevoWidget::ahrsSettingsSaveRAM() void ConfigRevoWidget::SettingsToRAM()
{ {
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager()); RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(insSettings); Q_ASSERT(revoCalibration);
InsSettings::DataFields insSettingsData = insSettings->getData(); revoCalibration->updated();
insSettingsData.Algorithm = m_ui->algorithm->currentIndex();
insSettings->setData(insSettingsData);
insSettings->updated();
HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(homeLocation);
HomeLocation::DataFields homeLocationData = homeLocation->getData();
homeLocationData.Set = m_ui->homeLocationSet->isChecked() ?
HomeLocation::SET_TRUE : HomeLocation::SET_FALSE;
homeLocation->setData(homeLocationData);
homeLocation->updated();
} }
/** /**
Save AHRS Settings and home location to SD Save Revo calibration settings to flash
*/ */
void ConfigRevoWidget::ahrsSettingsSaveSD() void ConfigRevoWidget::SettingsToFlash()
{ {
ahrsSettingsSaveRAM(); SettingsToRAM();
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager()); RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(insSettings); Q_ASSERT(revoCalibration);
saveObjectToSD(insSettings); saveObjectToSD(revoCalibration);
HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(homeLocation);
saveObjectToSD(homeLocation);
} }
void ConfigRevoWidget::openHelp() void ConfigRevoWidget::openHelp()
{ {
QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/INS+Configuration", QUrl::StrictMode) ); QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/Revo+Configuration", QUrl::StrictMode) );
} }
/** /**

23
ground/openpilotgcs/src/plugins/config/configrevowidget.h
View File

@ -40,6 +40,8 @@
#include <QMutex> #include <QMutex>
#include <inssettings.h> #include <inssettings.h>
class Ui_Widget;
class ConfigRevoWidget: public ConfigTaskWidget class ConfigRevoWidget: public ConfigTaskWidget
{ {
Q_OBJECT Q_OBJECT
@ -84,32 +86,27 @@ private:
QList<double> mag_accum_x; QList<double> mag_accum_x;
QList<double> mag_accum_y; QList<double> mag_accum_y;
QList<double> mag_accum_z; QList<double> mag_accum_z;
quint8 algorithm;
double accel_data_x[6], accel_data_y[6], accel_data_z[6];
double mag_data_x[6], mag_data_y[6], mag_data_z[6];
UAVObject::Metadata initialMdata; UAVObject::Metadata initialMdata;
int position;
private slots: private slots:
void enableHomeLocSave(UAVObject *obj);
void measureNoise();
void noiseMeasured();
void saveRevoCalibration();
void openHelp(); void openHelp();
void launchAccelBiasCalibration(); void launchAccelBiasCalibration();
void incrementProgress(); void incrementProgress();
virtual void refreshValues(); virtual void refreshValues();
//void ahrsSettingsRequest(); //void ahrsSettingsRequest();
void SettingsSaveRAM(); void SettingsToRAM();
void SettingsSaveSD(); void SettingsToFlash();
void savePositionData(); void savePositionData();
void computeScaleBias(); void computeScaleBias();
void sixPointCalibrationMode(); // this function no longer exists void sixPointCalibrationMode();
void attitudeRawUpdated(UAVObject * obj); void sensorsUpdated(UAVObject * obj);
void accelBiasattitudeRawUpdated(UAVObject*); void accelBiasattitudeRawUpdated(UAVObject*);
void driftCalibrationAttitudeRawUpdated(UAVObject*);
void launchGyroDriftCalibration();
void resetCalibrationDefaults();
void cacheCurrentCalibration();
protected: protected:
void showEvent(QShowEvent *event); void showEvent(QShowEvent *event);

4
ground/openpilotgcs/src/plugins/config/revosensors.ui
View File

@ -1,7 +1,7 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0"> <ui version="4.0">
<class>RevoSensorWidget</class> <class>RevoSensorsWidget</class>
<widget class="QWidget" name="RevoSensorWidget"> <widget class="QWidget" name="RevoSensorsWidget">
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>

33
ground/openpilotgcs/src/plugins/coreplugin/OpenPilotGCS.xml
View File

@ -989,29 +989,6 @@
</default> </default>
</ImportExportGadget> </ImportExportGadget>
<LineardialGadget> <LineardialGadget>
<AHRS__PCT__20CPU>
<configInfo>
<locked>false</locked>
<version>0.0.0</version>
</configInfo>
<data>
<dFile>%%DATAPATH%%dials/deluxe/lineardial-vertical.svg</dFile>
<decimalPlaces>0</decimalPlaces>
<factor>1</factor>
<font>Andale Mono,12,-1,5,75,0,0,0,0,0</font>
<greenMax>50</greenMax>
<greenMin>0</greenMin>
<maxValue>100</maxValue>
<minValue>0</minValue>
<redMax>100</redMax>
<redMin>80</redMin>
<sourceDataObject>AhrsStatus</sourceDataObject>
<sourceObjectField>CPULoad</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>80</yellowMax>
<yellowMin>50</yellowMin>
</data>
</AHRS__PCT__20CPU>
<Accel__PCT__20Horizontal__PCT__20X> <Accel__PCT__20Horizontal__PCT__20X>
<configInfo> <configInfo>
<locked>false</locked> <locked>false</locked>
@ -2260,21 +2237,13 @@
<configurationStreamVersion>1000</configurationStreamVersion> <configurationStreamVersion>1000</configurationStreamVersion>
<dataSize>240</dataSize> <dataSize>240</dataSize>
<plotCurve0> <plotCurve0>
<color>4289374847</color>
<uavField>RunningTime</uavField>
<uavObject>InsStatus</uavObject>
<yMaximum>0</yMaximum>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve0>
<plotCurve1>
<color>4294945407</color> <color>4294945407</color>
<uavField>FlightTime</uavField> <uavField>FlightTime</uavField>
<uavObject>SystemStats</uavObject> <uavObject>SystemStats</uavObject>
<yMaximum>0</yMaximum> <yMaximum>0</yMaximum>
<yMinimum>0</yMinimum> <yMinimum>0</yMinimum>
<yScalePower>0</yScalePower> <yScalePower>0</yScalePower>
</plotCurve1> </plotCurve0>
<plotCurveCount>2</plotCurveCount> <plotCurveCount>2</plotCurveCount>
<plotType>1</plotType> <plotType>1</plotType>
<refreshInterval>800</refreshInterval> <refreshInterval>800</refreshInterval>