mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-29 14:52:12 +01:00
Get the 6pt calibration working again for revo
This commit is contained in:
James Cotton
parent
d78d345953
commit
710f95feeb
@ -36,6 +36,7 @@
|
||||
#include "configcamerastabilizationwidget.h"
|
||||
#include "config_pro_hw_widget.h"
|
||||
#include "config_cc_hw_widget.h"
|
||||
#include "configrevowidget.h"
|
||||
#include "defaultattitudewidget.h"
|
||||
#include "defaulthwsettingswidget.h"
|
||||
#include "uavobjectutilmanager.h"
|
||||
|
||||
@ -43,8 +43,8 @@
|
||||
#include <accels.h>
|
||||
#include <gyros.h>
|
||||
#include <magnetometer.h>
|
||||
#include <homelocation.h>
|
||||
|
||||
#define GRAVITY 9.81f
|
||||
#include "assertions.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);
|
||||
|
||||
collectingData = false;
|
||||
|
||||
// Initialization of the Paper plane widget
|
||||
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->setSceneRect(paperplane->boundingRect());
|
||||
|
||||
// Initialization of the AHRS bargraph graph
|
||||
|
||||
// Initialization of the Revo sensor noise bargraph graph
|
||||
m_ui->ahrsBargraph->setScene(new QGraphicsScene(this));
|
||||
|
||||
QSvgRenderer *renderer = new QSvgRenderer();
|
||||
@ -203,36 +203,18 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : ConfigTaskWidget(parent)
|
||||
mag_z->setPos(startX,startY);
|
||||
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(m_ui->ahrsCalibStart, SIGNAL(clicked()), this, SLOT(measureNoise()));
|
||||
connect(m_ui->accelBiasStart, SIGNAL(clicked()), this, SLOT(launchAccelBiasCalibration()));
|
||||
|
||||
connect(homeLocation, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshValues()));
|
||||
connect(insSettings, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshValues()));
|
||||
RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
|
||||
Q_ASSERT(revoCalibration);
|
||||
connect(revoCalibration, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshValues()));
|
||||
|
||||
connect(m_ui->ahrsSettingsSaveRAM, SIGNAL(clicked()), this, SLOT(ahrsSettingsSaveRAM()));
|
||||
connect(m_ui->ahrsSettingsSaveSD, SIGNAL(clicked()), this, SLOT(ahrsSettingsSaveSD()));
|
||||
connect(m_ui->sixPointsStart, SIGNAL(clicked()), this, SLOT(multiPointCalibrationMode()));
|
||||
connect(m_ui->ahrsSettingsSaveRAM, SIGNAL(clicked()), this, SLOT(SettingsToRam()));
|
||||
connect(m_ui->ahrsSettingsSaveSD, SIGNAL(clicked()), this, SLOT(SettingsToFlash()));
|
||||
connect(m_ui->sixPointsStart, SIGNAL(clicked()), this, SLOT(sixPointCalibrationMode()));
|
||||
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.
|
||||
connect(&progressBarTimer, SIGNAL(timeout()), this, SLOT(incrementProgress()));
|
||||
@ -287,29 +269,29 @@ void ConfigRevoWidget::launchAccelBiasCalibration()
|
||||
m_ui->accelBiasStart->setEnabled(false);
|
||||
m_ui->accelBiasProgress->setValue(0);
|
||||
|
||||
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
|
||||
Q_ASSERT(insSettings);
|
||||
InsSettings::DataFields insSettingsData = insSettings->getData();
|
||||
insSettingsData.BiasCorrectedRaw = InsSettings::BIASCORRECTEDRAW_FALSE;
|
||||
insSettings->setData(insSettingsData);
|
||||
insSettings->updated();
|
||||
RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
|
||||
Q_ASSERT(revoCalibration);
|
||||
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
|
||||
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE;
|
||||
revoCalibration->setData(revoCalibrationData);
|
||||
revoCalibration->updated();
|
||||
|
||||
accel_accum_x.clear();
|
||||
accel_accum_y.clear();
|
||||
accel_accum_z.clear();
|
||||
|
||||
/* Need to get as many AttitudeRaw updates as possible */
|
||||
AttitudeRaw * attitudeRaw = AttitudeRaw::GetInstance(getObjectManager());
|
||||
Q_ASSERT(attitudeRaw);
|
||||
initialMdata = attitudeRaw->getMetadata();
|
||||
Accels * accels = Accels::GetInstance(getObjectManager());
|
||||
Q_ASSERT(accels);
|
||||
initialMdata = accels->getMetadata();
|
||||
UAVObject::Metadata mdata = initialMdata;
|
||||
mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
|
||||
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;
|
||||
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);
|
||||
|
||||
AttitudeRaw * attitudeRaw = AttitudeRaw::GetInstance(getObjectManager());
|
||||
Q_ASSERT(attitudeRaw);
|
||||
AttitudeRaw::DataFields attitudeRawData = attitudeRaw->getData();
|
||||
Accels * accels = Accels::GetInstance(getObjectManager());
|
||||
Q_ASSERT(accels);
|
||||
Accels::DataFields accelsData = accels->getData();
|
||||
|
||||
// This is necessary to prevent a race condition on disconnect signal and another update
|
||||
if (collectingData == true) {
|
||||
accel_accum_x.append(attitudeRawData.accels[InsSettings::ACCEL_BIAS_X]);
|
||||
accel_accum_y.append(attitudeRawData.accels[InsSettings::ACCEL_BIAS_Y]);
|
||||
accel_accum_z.append(attitudeRawData.accels[InsSettings::ACCEL_BIAS_Z]);
|
||||
accel_accum_x.append(accelsData.x);
|
||||
accel_accum_y.append(accelsData.y);
|
||||
accel_accum_z.append(accelsData.z);
|
||||
}
|
||||
|
||||
m_ui->accelBiasProgress->setValue(m_ui->accelBiasProgress->value()+1);
|
||||
|
||||
if(accel_accum_x.size() >= 100 && collectingData == true) {
|
||||
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);
|
||||
|
||||
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
|
||||
Q_ASSERT(insSettings);
|
||||
InsSettings::DataFields insSettingsData = insSettings->getData();
|
||||
insSettingsData.BiasCorrectedRaw = InsSettings::BIASCORRECTEDRAW_TRUE;
|
||||
RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
|
||||
Q_ASSERT(revoCalibration);
|
||||
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
|
||||
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE;
|
||||
|
||||
insSettingsData.accel_bias[InsSettings::ACCEL_BIAS_X] -= listMean(accel_accum_x);
|
||||
insSettingsData.accel_bias[InsSettings::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_X] -= listMean(accel_accum_x);
|
||||
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] -= listMean(accel_accum_y);
|
||||
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] -= GRAVITY + listMean(accel_accum_z);
|
||||
|
||||
insSettings->setData(insSettingsData);
|
||||
insSettings->updated();
|
||||
revoCalibration->setData(revoCalibrationData);
|
||||
revoCalibration->updated();
|
||||
|
||||
attitudeRaw->setMetadata(initialMdata);
|
||||
|
||||
saveAHRSCalibration();
|
||||
accels->setMetadata(initialMdata);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
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)
|
||||
*/
|
||||
@ -533,9 +344,9 @@ void ConfigRevoWidget::incrementProgress()
|
||||
if (m_ui->calibProgress->value() >= m_ui->calibProgress->maximum()) {
|
||||
progressBarTimer.stop();
|
||||
|
||||
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
|
||||
Q_ASSERT(insSettings);
|
||||
disconnect(insSettings, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(noiseMeasured()));
|
||||
RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
|
||||
Q_ASSERT(revoCalibration);
|
||||
disconnect(revoCalibration, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(noiseMeasured()));
|
||||
collectingData = false;
|
||||
|
||||
QErrorMessage err(this);
|
||||
@ -544,137 +355,83 @@ void ConfigRevoWidget::incrementProgress()
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
*@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)
|
||||
void ConfigRevoWidget::sensorsUpdated(UAVObject * obj)
|
||||
{
|
||||
QMutexLocker lock(&attitudeRawUpdateLock);
|
||||
|
||||
UAVObjectField *accel_field = obj->getField(QString("accels"));
|
||||
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);
|
||||
|
||||
qDebug() << "Data";
|
||||
// This is necessary to prevent a race condition on disconnect signal and another update
|
||||
if (collectingData == true) {
|
||||
accel_accum_x.append(accel_field->getValue(0).toDouble());
|
||||
accel_accum_y.append(accel_field->getValue(1).toDouble());
|
||||
accel_accum_z.append(accel_field->getValue(2).toDouble());
|
||||
// Note gyros actually (-y,-x,-z) but since we consistent here no prob
|
||||
mag_accum_x.append(mag_field->getValue(0).toDouble());
|
||||
mag_accum_y.append(mag_field->getValue(1).toDouble());
|
||||
mag_accum_z.append(mag_field->getValue(2).toDouble());
|
||||
|
||||
gyro_accum_x.append(gyro_field->getValue(0).toDouble());
|
||||
gyro_accum_y.append(gyro_field->getValue(1).toDouble());
|
||||
gyro_accum_z.append(gyro_field->getValue(2).toDouble());
|
||||
qDebug() << "Collecting";
|
||||
if( obj->getObjID() == Accels::OBJID ) {
|
||||
qDebug() << "Accels";
|
||||
Accels * accels = Accels::GetInstance(getObjectManager());
|
||||
Q_ASSERT(accels);
|
||||
Accels::DataFields accelsData = accels->getData();
|
||||
accel_accum_x.append(accelsData.x);
|
||||
accel_accum_y.append(accelsData.y);
|
||||
accel_accum_z.append(accelsData.z);
|
||||
} else if( obj->getObjID() == Magnetometer::OBJID ) {
|
||||
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;
|
||||
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);
|
||||
|
||||
accel_data[position] << listMean(accel_accum_x),
|
||||
listMean(accel_accum_y),
|
||||
listMean(accel_accum_z);
|
||||
accel_data_x[position] = listMean(accel_accum_x);
|
||||
accel_data_y[position] = listMean(accel_accum_y);
|
||||
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),
|
||||
listMean(mag_accum_y),
|
||||
listMean(mag_accum_z);
|
||||
|
||||
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);
|
||||
position = (position + 1) % 6;
|
||||
if(position == 1) {
|
||||
m_ui->sixPointCalibInstructions->append("Place with left side down and click save position...");
|
||||
displayPlane("plane-left");
|
||||
}
|
||||
else if(position == 0) {
|
||||
position = n_positions;
|
||||
if(position == 2) {
|
||||
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();
|
||||
m_ui->sixPointsStart->setEnabled(true);
|
||||
m_ui->sixPointsSave->setEnabled(false);
|
||||
saveAHRSCalibration(); // Saves the result to SD.
|
||||
|
||||
/* 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();
|
||||
|
||||
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...");
|
||||
}
|
||||
|
||||
//*****************************************************************
|
||||
namespace {
|
||||
|
||||
/*
|
||||
* Calibrated scale factors should be real values with scale factor less than 10% from nominal
|
||||
*/
|
||||
bool checkScaleFactors(const Vector3f& scalars)
|
||||
int LinearEquationsSolving(int nDim, double* pfMatr, double* pfVect, double* pfSolution)
|
||||
{
|
||||
return isReal(scalars) &&
|
||||
scalars.cwise().abs().maxCoeff() < 1.10f;
|
||||
double fMaxElem;
|
||||
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
|
||||
*/
|
||||
bool checkOffsets(const Vector3f& offsets)
|
||||
|
||||
int SixPointInConstFieldCal( double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3] )
|
||||
{
|
||||
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()
|
||||
{
|
||||
// Extract the local magnetic and gravitational field vectors from HomeLocation.
|
||||
UAVObject *home = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("HomeLocation")));
|
||||
Vector3f localMagField;
|
||||
localMagField << home->getField("Be")->getValue(0).toDouble(),
|
||||
home->getField("Be")->getValue(1).toDouble(),
|
||||
home->getField("Be")->getValue(2).toDouble();
|
||||
double S[3], b[3];
|
||||
RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
|
||||
Q_ASSERT(revoCalibration);
|
||||
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
|
||||
|
||||
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;
|
||||
double noise = 0.04;
|
||||
Vector3f accelBias;
|
||||
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;
|
||||
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] = sign(S[0]) * b[0];
|
||||
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] = sign(S[1]) * b[1];
|
||||
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] = sign(S[2]) * b[2];
|
||||
|
||||
// Apply the computed scale factor and bias to each sample
|
||||
for (int i = 0; i < n_positions; ++i) {
|
||||
accel_data[i] = accelScale * accel_data[i] + accelBias;
|
||||
}
|
||||
// Calibration mag
|
||||
SixPointInConstFieldCal( 1000, mag_data_x, mag_data_y, mag_data_z, S, b);
|
||||
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.
|
||||
Vector3f magBias;
|
||||
Vector3f magScale;
|
||||
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;
|
||||
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] = sign(S[0]) * b[0];
|
||||
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = sign(S[1]) * b[1];
|
||||
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = sign(S[2]) * b[2];
|
||||
|
||||
// Apply the computed scale factor and bias to each sample
|
||||
for (int i = 0; i < n_positions; ++i) {
|
||||
mag_data[i] = magScale.asDiagonal() * mag_data[i] + magBias;
|
||||
}
|
||||
revoCalibration->setData(revoCalibrationData);
|
||||
|
||||
// Calibrate gyro bias and acceleration sensitivity
|
||||
Matrix3f accelSensitivity;
|
||||
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.");
|
||||
position = -1; //set to run again
|
||||
m_ui->sixPointCalibInstructions->append("Computed accel and mag scale and bias...");
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
Multi-point calibration mode
|
||||
Six point calibration mode
|
||||
*/
|
||||
FORCE_ALIGN_FUNC
|
||||
void ConfigRevoWidget::multiPointCalibrationMode()
|
||||
void ConfigRevoWidget::sixPointCalibrationMode()
|
||||
{
|
||||
cacheCurrentCalibration();
|
||||
resetCalibrationDefaults();
|
||||
double S[3], b[3];
|
||||
RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
|
||||
Q_ASSERT(revoCalibration);
|
||||
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
|
||||
|
||||
UAVObject* obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings")));
|
||||
UAVObjectField *field = obj->getField(QString("BiasCorrectedRaw"));
|
||||
field->setValue("FALSE");
|
||||
obj->updated();
|
||||
// Calibration accel
|
||||
revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X] = 1;
|
||||
revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y] = 1;
|
||||
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();
|
||||
gyro_accum_y.clear();
|
||||
gyro_accum_z.clear();
|
||||
usleep(100000);
|
||||
|
||||
/* Need to get as many AttitudeRaw updates as possible */
|
||||
obj = getObjectManager()->getObject(QString("AttitudeRaw"));
|
||||
initialMdata = obj->getMetadata();
|
||||
UAVObject::Metadata mdata = initialMdata;
|
||||
mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
|
||||
mdata.flightTelemetryUpdatePeriod = 100;
|
||||
obj->setMetadata(mdata);
|
||||
gyro_accum_x.clear();
|
||||
gyro_accum_y.clear();
|
||||
gyro_accum_z.clear();
|
||||
accel_accum_x.clear();
|
||||
accel_accum_y.clear();
|
||||
accel_accum_z.clear();
|
||||
mag_accum_x.clear();
|
||||
mag_accum_y.clear();
|
||||
mag_accum_z.clear();
|
||||
|
||||
/* Show instructions and enable controls */
|
||||
m_ui->sixPointCalibInstructions->clear();
|
||||
m_ui->sixPointCalibInstructions->append("Stand facing Earth's magnetic N or S. Place the vehicle horizontally facing forward and click save position...");
|
||||
displayPlane("plane-horizontal");
|
||||
m_ui->sixPointsStart->setEnabled(false);
|
||||
m_ui->sixPointsSave->setEnabled(true);
|
||||
position = 0;
|
||||
/* Need to get as many accel and mag updates as possible */
|
||||
Accels * accels = Accels::GetInstance(getObjectManager());
|
||||
Q_ASSERT(accels);
|
||||
Magnetometer * mag = Magnetometer::GetInstance(getObjectManager());
|
||||
Q_ASSERT(mag);
|
||||
|
||||
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
|
||||
@ -1105,32 +688,32 @@ void ConfigRevoWidget::displayPlane(QString elementID)
|
||||
*/
|
||||
void ConfigRevoWidget::drawVariancesGraph()
|
||||
{
|
||||
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
|
||||
Q_ASSERT(insSettings);
|
||||
InsSettings::DataFields insSettingsData = insSettings->getData();
|
||||
RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
|
||||
Q_ASSERT(revoCalibration);
|
||||
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
|
||||
|
||||
// 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(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);
|
||||
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);
|
||||
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);
|
||||
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
|
||||
// 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);
|
||||
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);
|
||||
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);
|
||||
}
|
||||
|
||||
@ -1139,17 +722,8 @@ void ConfigRevoWidget::drawVariancesGraph()
|
||||
*/
|
||||
void ConfigRevoWidget::refreshValues()
|
||||
{
|
||||
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
|
||||
Q_ASSERT(insSettings);
|
||||
InsSettings::DataFields insSettingsData = insSettings->getData();
|
||||
m_ui->algorithm->setCurrentIndex(insSettingsData.Algorithm);
|
||||
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->sixPointsStart->setEnabled(true);
|
||||
m_ui->accelBiasStart->setEnabled(true);
|
||||
@ -1158,60 +732,32 @@ void ConfigRevoWidget::refreshValues()
|
||||
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
|
||||
*/
|
||||
void ConfigRevoWidget::ahrsSettingsSaveRAM()
|
||||
void ConfigRevoWidget::SettingsToRAM()
|
||||
{
|
||||
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
|
||||
Q_ASSERT(insSettings);
|
||||
InsSettings::DataFields insSettingsData = insSettings->getData();
|
||||
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();
|
||||
RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
|
||||
Q_ASSERT(revoCalibration);
|
||||
revoCalibration->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());
|
||||
Q_ASSERT(insSettings);
|
||||
saveObjectToSD(insSettings);
|
||||
|
||||
HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager());
|
||||
Q_ASSERT(homeLocation);
|
||||
saveObjectToSD(homeLocation);
|
||||
RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
|
||||
Q_ASSERT(revoCalibration);
|
||||
saveObjectToSD(revoCalibration);
|
||||
}
|
||||
|
||||
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) );
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@ -40,6 +40,8 @@
|
||||
#include <QMutex>
|
||||
#include <inssettings.h>
|
||||
|
||||
class Ui_Widget;
|
||||
|
||||
class ConfigRevoWidget: public ConfigTaskWidget
|
||||
{
|
||||
Q_OBJECT
|
||||
@ -84,32 +86,27 @@ private:
|
||||
QList<double> mag_accum_x;
|
||||
QList<double> mag_accum_y;
|
||||
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;
|
||||
int position;
|
||||
|
||||
private slots:
|
||||
void enableHomeLocSave(UAVObject *obj);
|
||||
void measureNoise();
|
||||
void noiseMeasured();
|
||||
void saveRevoCalibration();
|
||||
void openHelp();
|
||||
void launchAccelBiasCalibration();
|
||||
void incrementProgress();
|
||||
|
||||
virtual void refreshValues();
|
||||
//void ahrsSettingsRequest();
|
||||
void SettingsSaveRAM();
|
||||
void SettingsSaveSD();
|
||||
void SettingsToRAM();
|
||||
void SettingsToFlash();
|
||||
void savePositionData();
|
||||
void computeScaleBias();
|
||||
void sixPointCalibrationMode(); // this function no longer exists
|
||||
void attitudeRawUpdated(UAVObject * obj);
|
||||
void sixPointCalibrationMode();
|
||||
void sensorsUpdated(UAVObject * obj);
|
||||
void accelBiasattitudeRawUpdated(UAVObject*);
|
||||
void driftCalibrationAttitudeRawUpdated(UAVObject*);
|
||||
void launchGyroDriftCalibration();
|
||||
void resetCalibrationDefaults();
|
||||
void cacheCurrentCalibration();
|
||||
|
||||
protected:
|
||||
void showEvent(QShowEvent *event);
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<ui version="4.0">
|
||||
<class>RevoSensorWidget</class>
|
||||
<widget class="QWidget" name="RevoSensorWidget">
|
||||
<class>RevoSensorsWidget</class>
|
||||
<widget class="QWidget" name="RevoSensorsWidget">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
|
||||
@ -989,29 +989,6 @@
|
||||
</default>
|
||||
</ImportExportGadget>
|
||||
<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>
|
||||
<configInfo>
|
||||
<locked>false</locked>
|
||||
@ -2260,21 +2237,13 @@
|
||||
<configurationStreamVersion>1000</configurationStreamVersion>
|
||||
<dataSize>240</dataSize>
|
||||
<plotCurve0>
|
||||
<color>4289374847</color>
|
||||
<uavField>RunningTime</uavField>
|
||||
<uavObject>InsStatus</uavObject>
|
||||
<yMaximum>0</yMaximum>
|
||||
<yMinimum>0</yMinimum>
|
||||
<yScalePower>0</yScalePower>
|
||||
</plotCurve0>
|
||||
<plotCurve1>
|
||||
<color>4294945407</color>
|
||||
<uavField>FlightTime</uavField>
|
||||
<uavObject>SystemStats</uavObject>
|
||||
<yMaximum>0</yMaximum>
|
||||
<yMinimum>0</yMinimum>
|
||||
<yScalePower>0</yScalePower>
|
||||
</plotCurve1>
|
||||
</plotCurve0>
|
||||
<plotCurveCount>2</plotCurveCount>
|
||||
<plotType>1</plotType>
|
||||
<refreshInterval>800</refreshInterval>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user