1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00

OP-1406 - Enable calibration of external mag from GCS

This commit is contained in:
Alessio Morale 2014-07-17 20:53:14 +02:00
parent e511190ad3
commit 869515b349
3 changed files with 196 additions and 69 deletions

View File

@ -29,7 +29,7 @@
#include "extensionsystem/pluginmanager.h"
#include "calibration/calibrationuiutils.h"
#include "math.h"
#include <math.h>
#include <QThread>
#include "QDebug"
@ -45,6 +45,7 @@ namespace OpenPilot {
SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
QObject(parent),
calibratingMag(false),
externalMagAvailable(false),
calibratingAccel(false),
calibrationStepsMag(),
calibrationStepsAccelOnly(),
@ -68,6 +69,21 @@ SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW,
tr("Place with left side down, nose south and press Save Position..."));
// All mag calibration matrices have the same structure, this is also used when calculating bias/transforms
// this is enforced using assert.
Q_STATIC_ASSERT((int)RevoCalibration::MAG_TRANSFORM_R0C0 == (int)AuxMagSettings::MAG_TRANSFORM_R0C0 &&
(int)RevoCalibration::MAG_TRANSFORM_R1C0 == (int)AuxMagSettings::MAG_TRANSFORM_R1C0 &&
(int)RevoCalibration::MAG_TRANSFORM_R2C0 == (int)AuxMagSettings::MAG_TRANSFORM_R2C0 &&
(int)RevoCalibration::MAG_TRANSFORM_R0C1 == (int)AuxMagSettings::MAG_TRANSFORM_R0C1 &&
(int)RevoCalibration::MAG_TRANSFORM_R1C1 == (int)AuxMagSettings::MAG_TRANSFORM_R1C1 &&
(int)RevoCalibration::MAG_TRANSFORM_R2C1 == (int)AuxMagSettings::MAG_TRANSFORM_R2C1 &&
(int)RevoCalibration::MAG_TRANSFORM_R0C2 == (int)AuxMagSettings::MAG_TRANSFORM_R0C2 &&
(int)RevoCalibration::MAG_TRANSFORM_R1C2 == (int)AuxMagSettings::MAG_TRANSFORM_R1C2 &&
(int)RevoCalibration::MAG_TRANSFORM_R2C2 == (int)AuxMagSettings::MAG_TRANSFORM_R2C2 &&
(int)RevoCalibration::MAG_BIAS_X == (int)AuxMagSettings::MAG_BIAS_X &&
(int)RevoCalibration::MAG_BIAS_Y == (int)AuxMagSettings::MAG_BIAS_Y &&
(int)RevoCalibration::MAG_BIAS_Z == (int)AuxMagSettings::MAG_BIAS_Z);
calibrationStepsAccelOnly.clear();
calibrationStepsAccelOnly << CalibrationStep(CALIBRATION_HELPER_IMAGE_NED,
tr("Place horizontally and press Save Position..."))
@ -85,14 +101,20 @@ SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
auxMagSettings = AuxMagSettings::GetInstance(getObjectManager());
Q_ASSERT(auxMagSettings);
accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
Q_ASSERT(accelGyroSettings);
accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
magState = MagState::GetInstance(getObjectManager());
Q_ASSERT(magState);
magSensor = MagSensor::GetInstance(getObjectManager());
Q_ASSERT(magSensor);
auxMagSensor = AuxMagSensor::GetInstance(getObjectManager());
Q_ASSERT(auxMagSensor);
homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(homeLocation);
@ -166,6 +188,26 @@ void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag)
revoCalibration->setData(revoCalibrationData);
// Calibration AuxMag
AuxMagSettings::DataFields auxMagSettingsData = auxMagSettings->getData();
memento.auxMagSettings = auxMagSettingsData;
// Reset the transformation matrix to identity
for (int i = 0; i < AuxMagSettings::MAG_TRANSFORM_R2C2; i++) {
auxMagSettingsData.mag_transform[i] = 0;
}
auxMagSettingsData.mag_transform[AuxMagSettings::MAG_TRANSFORM_R0C0] = 1;
auxMagSettingsData.mag_transform[AuxMagSettings::MAG_TRANSFORM_R1C1] = 1;
auxMagSettingsData.mag_transform[AuxMagSettings::MAG_TRANSFORM_R2C2] = 1;
auxMagSettingsData.mag_bias[AuxMagSettings::MAG_BIAS_X] = 0;
auxMagSettingsData.mag_bias[AuxMagSettings::MAG_BIAS_Y] = 0;
auxMagSettingsData.mag_bias[AuxMagSettings::MAG_BIAS_Z] = 0;
// Disable adaptive mag nulling
auxMagSettingsData.MagBiasNullingRate = 0;
auxMagSettings->setData(auxMagSettingsData);
QThread::usleep(100000);
mag_accum_x.clear();
@ -186,12 +228,19 @@ void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag)
}
// Need to get as many mag updates as possible
memento.magStateMetadata = magState->getMetadata();
memento.magSensorMetadata = magSensor->getMetadata();
memento.auxMagSensorMetadata = auxMagSensor->getMetadata();
if (calibrateMag) {
UAVObject::Metadata mdata = magState->getMetadata();
UAVObject::Metadata mdata = magSensor->getMetadata();
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
magState->setMetadata(mdata);
magSensor->setMetadata(mdata);
mdata = auxMagSensor->getMetadata();
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
auxMagSensor->setMetadata(mdata);
}
// reset dirty state to forget previous unsaved runs
@ -229,6 +278,9 @@ void SixPointCalibrationModel::savePositionData()
mag_accum_x.clear();
mag_accum_y.clear();
mag_accum_z.clear();
aux_mag_accum_x.clear();
aux_mag_accum_y.clear();
aux_mag_accum_z.clear();
collectingData = true;
@ -236,10 +288,12 @@ void SixPointCalibrationModel::savePositionData()
#ifdef FITTING_USING_CONTINOUS_ACQUISITION
// Mag samples are acquired during the whole calibration session, to be used for ellipsoid fit.
if (!position) {
connect(magState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
connect(magSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
connect(auxMagSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
}
#endif // FITTING_USING_CONTINOUS_ACQUISITION
connect(magState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
connect(magSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
connect(auxMagSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
}
if (calibratingAccel) {
connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
@ -264,8 +318,8 @@ void SixPointCalibrationModel::getSample(UAVObject *obj)
accel_accum_x.append(accelStateData.x);
accel_accum_y.append(accelStateData.y);
accel_accum_z.append(accelStateData.z);
} else if (obj->getObjID() == MagState::OBJID) {
MagState::DataFields magData = magState->getData();
} else if (obj->getObjID() == MagSensor::OBJID) {
MagSensor::DataFields magData = magSensor->getData();
mag_accum_x.append(magData.x);
mag_accum_y.append(magData.y);
mag_accum_z.append(magData.z);
@ -274,6 +328,19 @@ void SixPointCalibrationModel::getSample(UAVObject *obj)
mag_fit_y.append(magData.y);
mag_fit_z.append(magData.z);
#endif // FITTING_USING_CONTINOUS_ACQUISITION
} else if (obj->getObjID() == AuxMagSensor::OBJID) {
AuxMagSensor::DataFields auxMagData = auxMagSensor->getData();
if (auxMagData.Status == AuxMagSensor::STATUS_OK) {
aux_mag_accum_x.append(auxMagData.x);
aux_mag_accum_y.append(auxMagData.y);
aux_mag_accum_z.append(auxMagData.z);
externalMagAvailable = true;
#ifndef FITTING_USING_CONTINOUS_ACQUISITION
aux_mag_fit_x.append(auxMagData.x);
aux_mag_fit_y.append(auxMagData.y);
aux_mag_fit_z.append(auxMagData.z);
#endif // FITTING_USING_CONTINOUS_ACQUISITION
}
} else {
Q_ASSERT(0);
}
@ -307,10 +374,8 @@ void SixPointCalibrationModel::getSample(UAVObject *obj)
// Store the mean for this position for the mag
if (calibratingMag) {
disconnect(magState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
mag_data_x[position] = CalibrationUtils::listMean(mag_accum_x);
mag_data_y[position] = CalibrationUtils::listMean(mag_accum_y);
mag_data_z[position] = CalibrationUtils::listMean(mag_accum_z);
disconnect(magSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
disconnect(auxMagSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
}
position = (position + 1) % 6;
@ -322,17 +387,19 @@ void SixPointCalibrationModel::getSample(UAVObject *obj)
// done...
#ifdef FITTING_USING_CONTINOUS_ACQUISITION
if (calibratingMag) {
disconnect(magState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
disconnect(magSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
disconnect(auxMagSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
}
#endif // FITTING_USING_CONTINOUS_ACQUISITION
compute();
// Restore original settings
accelState->setMetadata(memento.accelStateMetadata);
magState->setMetadata(memento.magStateMetadata);
magSensor->setMetadata(memento.magSensorMetadata);
auxMagSensor->setMetadata(memento.auxMagSensorMetadata);
revoCalibration->setData(memento.revoCalibrationData);
accelGyroSettings->setData(memento.accelGyroSettingsData);
auxMagSettings->setData(memento.auxMagSettings);
// Recall saved board rotation
recallBoardRotation();
@ -347,11 +414,19 @@ void SixPointCalibrationModel::continouslyGetMagSamples(UAVObject *obj)
{
QMutexLocker lock(&sensorsUpdateLock);
if (obj->getObjID() == MagState::OBJID) {
MagState::DataFields magStateData = magState->getData();
mag_fit_x.append(magStateData.x);
mag_fit_y.append(magStateData.y);
mag_fit_z.append(magStateData.z);
if (obj->getObjID() == MagSensor::OBJID) {
MagSensor::DataFields magSensorData = magSensor->getData();
mag_fit_x.append(magSensorData.x);
mag_fit_y.append(magSensorData.y);
mag_fit_z.append(magSensorData.z);
} else if (obj->getObjID() == AuxMagSensor::OBJID) {
AuxMagSensor::DataFields auxMagData = auxMagSensor->getData();
if (auxMagData.Status == AuxMagSensor::STATUS_OK) {
aux_mag_fit_x.append(auxMagData.x);
aux_mag_fit_y.append(auxMagData.y);
aux_mag_fit_z.append(auxMagData.z);
externalMagAvailable = true;
}
}
}
@ -365,6 +440,7 @@ void SixPointCalibrationModel::compute()
double Be_length;
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
AuxMagSettings::DataFields auxCalibrationData = auxMagSettings->getData();
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
HomeLocation::DataFields homeLocationData = homeLocation->getData();
@ -384,51 +460,21 @@ void SixPointCalibrationModel::compute()
// Calibration mag
if (calibratingMag) {
Be_length = sqrt(pow(homeLocationData.Be[0], 2) + pow(homeLocationData.Be[1], 2) + pow(homeLocationData.Be[2], 2));
int vectSize = mag_fit_x.count();
Eigen::VectorXf samples_x(vectSize);
Eigen::VectorXf samples_y(vectSize);
Eigen::VectorXf samples_z(vectSize);
for (int i = 0; i < vectSize; i++) {
samples_x(i) = mag_fit_x[i];
samples_y(i) = mag_fit_y[i];
samples_z(i) = mag_fit_z[i];
qDebug() << "-----------------------------------";
qDebug() << "Onboard Mag";
CalcCalibration(mag_fit_x, mag_fit_y, mag_fit_z, Be_length, revoCalibrationData.mag_transform, revoCalibrationData.mag_bias);
if (externalMagAvailable) {
qDebug() << "Aux Mag";
CalcCalibration(aux_mag_fit_x, aux_mag_fit_y, aux_mag_fit_z, Be_length, auxCalibrationData.mag_transform, auxCalibrationData.mag_bias);
}
OpenPilot::CalibrationUtils::EllipsoidCalibrationResult result;
OpenPilot::CalibrationUtils::EllipsoidCalibration(&samples_x, &samples_y, &samples_z, Be_length, &result, true);
qDebug() << "-----------------------------------";
qDebug() << "Mag Calibration results: Fit";
qDebug() << "scale(" << result.Scale.coeff(0) << ", " << result.Scale.coeff(1) << ", " << result.Scale.coeff(2) << ")";
qDebug() << "bias(" << result.Bias.coeff(0) << ", " << result.Bias.coeff(1) << ", " << result.Bias.coeff(2) << ")";
OpenPilot::CalibrationUtils::SixPointInConstFieldCal(Be_length, mag_data_x, mag_data_y, mag_data_z, S, b);
qDebug() << "-----------------------------------";
qDebug() << "Mag Calibration results: Six Point";
qDebug() << "scale(" << S[0] << ", " << S[1] << ", " << S[2] << ")";
qDebug() << "bias(" << -sign(S[0]) * b[0] << ", " << -sign(S[1]) * b[1] << ", " << -sign(S[2]) * b[2] << ")";
qDebug() << "-----------------------------------";
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] = result.CalibrationMatrix.coeff(0, 0);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1] = result.CalibrationMatrix.coeff(0, 1);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C2] = result.CalibrationMatrix.coeff(0, 2);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C0] = result.CalibrationMatrix.coeff(1, 0);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] = result.CalibrationMatrix.coeff(1, 1);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C2] = result.CalibrationMatrix.coeff(1, 2);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C0] = result.CalibrationMatrix.coeff(2, 0);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C1] = result.CalibrationMatrix.coeff(2, 1);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] = result.CalibrationMatrix.coeff(2, 2);
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] = result.Bias.coeff(0);
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = result.Bias.coeff(1);
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = result.Bias.coeff(2);
}
// Restore the previous setting
revoCalibrationData.MagBiasNullingRate = memento.revoCalibrationData.MagBiasNullingRate;;
// Check the mag calibration is good
bool good_calibration = true;
bool good_calibration = true;
bool good_aux_calibration = true;
if (calibratingMag) {
good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0]);
good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1]);
@ -445,6 +491,23 @@ void SixPointCalibrationModel::compute()
good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X]);
good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y]);
good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z]);
if (externalMagAvailable) {
good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0]);
good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1]);
good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C2]);
good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C0]);
good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1]);
good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C2]);
good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C0]);
good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C1]);
good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2]);
good_calibration &= !IS_NAN(auxCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X]);
good_calibration &= !IS_NAN(auxCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y]);
good_calibration &= !IS_NAN(auxCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z]);
}
}
// Check the accel calibration is good
if (calibratingAccel) {
@ -458,7 +521,8 @@ void SixPointCalibrationModel::compute()
if (good_calibration) {
m_dirty = true;
if (calibratingMag) {
result.revoCalibrationData = revoCalibrationData;
result.revoCalibrationData = revoCalibrationData;
result.auxMagSettingsData = auxCalibrationData;
displayInstructions(tr("Magnetometer calibration completed successfully."), WizardModel::Success);
}
if (calibratingAccel) {
@ -473,6 +537,42 @@ void SixPointCalibrationModel::compute()
position = -1;
}
void SixPointCalibrationModel::CalcCalibration(QList<float> x, QList<float> y, QList<float> z, double Be_length, float calibrationMatrix[], float bias[])
{
int vectSize = x.count();
Eigen::VectorXf samples_x(vectSize);
Eigen::VectorXf samples_y(vectSize);
Eigen::VectorXf samples_z(vectSize);
for (int i = 0; i < vectSize; i++) {
samples_x(i) = x[i];
samples_y(i) = y[i];
samples_z(i) = z[i];
}
OpenPilot::CalibrationUtils::EllipsoidCalibrationResult result;
OpenPilot::CalibrationUtils::EllipsoidCalibration(&samples_x, &samples_y, &samples_z, Be_length, &result, true);
qDebug() << "Mag fitting results: ";
qDebug() << "scale(" << result.Scale.coeff(0) << ", " << result.Scale.coeff(1) << ", " << result.Scale.coeff(2) << ")";
qDebug() << "bias(" << result.Bias.coeff(0) << ", " << result.Bias.coeff(1) << ", " << result.Bias.coeff(2) << ")";
qDebug() << "-----------------------------------";
calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R0C0] = result.CalibrationMatrix.coeff(0, 0);
calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R0C1] = result.CalibrationMatrix.coeff(0, 1);
calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R0C2] = result.CalibrationMatrix.coeff(0, 2);
calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R1C0] = result.CalibrationMatrix.coeff(1, 0);
calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R1C1] = result.CalibrationMatrix.coeff(1, 1);
calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R1C2] = result.CalibrationMatrix.coeff(1, 2);
calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R2C0] = result.CalibrationMatrix.coeff(2, 0);
calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R2C1] = result.CalibrationMatrix.coeff(2, 1);
calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R2C2] = result.CalibrationMatrix.coeff(2, 2);
bias[RevoCalibration::MAG_BIAS_X] = result.Bias.coeff(0);
bias[RevoCalibration::MAG_BIAS_Y] = result.Bias.coeff(1);
bias[RevoCalibration::MAG_BIAS_Z] = result.Bias.coeff(2);
}
void SixPointCalibrationModel::save()
{
if (!m_dirty) {
@ -489,6 +589,18 @@ void SixPointCalibrationModel::save()
}
revoCalibration->setData(revoCalibrationData);
if (externalMagAvailable) {
AuxMagSettings::DataFields auxCalibrationData = auxMagSettings->getData();
// Note that Revo/AuxMag MAG_TRANSFORM_RxCx are interchangeable, an assertion at initialization enforces the structs are equal
for (int i = 0; i < RevoCalibration::MAG_TRANSFORM_R2C2; i++) {
auxCalibrationData.mag_transform[i] = result.auxMagSettingsData.mag_transform[i];
}
for (int i = 0; i < 3; i++) {
auxCalibrationData.mag_bias[i] = result.auxMagSettingsData.mag_bias[i];
}
auxMagSettings->setData(auxCalibrationData);
}
}
if (calibratingAccel) {
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();

View File

@ -31,10 +31,13 @@
#include "wizardmodel.h"
#include "calibration/calibrationutils.h"
#include <revocalibration.h>
#include <auxmagsettings.h>
#include <accelgyrosettings.h>
#include <homelocation.h>
#include <accelstate.h>
#include <magstate.h>
#include <magsensor.h>
#include <auxmagsensor.h>
#include <QMutex>
#include <QObject>
@ -87,17 +90,21 @@ public:
typedef struct {
UAVObject::Metadata accelStateMetadata;
UAVObject::Metadata magStateMetadata;
RevoCalibration::DataFields revoCalibrationData;
UAVObject::Metadata magSensorMetadata;
UAVObject::Metadata auxMagSensorMetadata;
AuxMagSettings::DataFields auxMagSettings;
RevoCalibration::DataFields revoCalibrationData;
AccelGyroSettings::DataFields accelGyroSettingsData;
} Memento;
typedef struct {
RevoCalibration::DataFields revoCalibrationData;
AuxMagSettings::DataFields auxMagSettingsData;
AccelGyroSettings::DataFields accelGyroSettingsData;
} Result;
bool calibratingMag;
bool externalMagAvailable;
bool calibratingAccel;
QList<CalibrationStep> calibrationStepsMag;
@ -114,7 +121,6 @@ public:
QMutex sensorsUpdateLock;
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];
QList<double> accel_accum_x;
QList<double> accel_accum_y;
@ -122,15 +128,23 @@ public:
QList<double> mag_accum_x;
QList<double> mag_accum_y;
QList<double> mag_accum_z;
QList<double> aux_mag_accum_x;
QList<double> aux_mag_accum_y;
QList<double> aux_mag_accum_z;
QList<float> mag_fit_x;
QList<float> mag_fit_y;
QList<float> mag_fit_z;
QList<float> aux_mag_fit_x;
QList<float> aux_mag_fit_y;
QList<float> aux_mag_fit_z;
// convenience pointers
RevoCalibration *revoCalibration;
AccelGyroSettings *accelGyroSettings;
AuxMagSettings *auxMagSettings;
AccelState *accelState;
MagState *magState;
MagSensor *magSensor;
AuxMagSensor *auxMagSensor;
HomeLocation *homeLocation;
void start(bool calibrateAccel, bool calibrateMag);
@ -138,6 +152,7 @@ public:
void compute();
void showHelp(QString image);
UAVObjectManager *getObjectManager();
void CalcCalibration(QList<float> x, QList<float> y, QList<float> z, double Be_length, float calibrationMatrix[], float bias[]);
};
}

View File

@ -128,7 +128,7 @@ HEADERS += \
$$UAVOBJECT_SYNTHETICS/mpu6000settings.h \
$$UAVOBJECT_SYNTHETICS/takeofflocation.h \
$$UAVOBJECT_SYNTHETICS/auxmagsensor.h \
$$UAVOBJECT_SYNTHETICS/auxmagcalibration.h \
$$UAVOBJECT_SYNTHETICS/auxmagsettings.h \
$$UAVOBJECT_SYNTHETICS/perfcounter.h
SOURCES += \
@ -234,6 +234,6 @@ SOURCES += \
$$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp \
$$UAVOBJECT_SYNTHETICS/takeofflocation.cpp \
$$UAVOBJECT_SYNTHETICS/auxmagsensor.cpp \
$$UAVOBJECT_SYNTHETICS/auxmagcalibration.cpp \
$$UAVOBJECT_SYNTHETICS/auxmagsettings.cpp \
$$UAVOBJECT_SYNTHETICS/perfcounter.cpp