rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-16 08:29:15 +01:00
Code Issues Releases Activity

OP-1406 - Enable calibration of external mag from GCS

Browse Source
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

236
ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp
View File

@ -29,7 +29,7 @@
#include "extensionsystem/pluginmanager.h" #include "extensionsystem/pluginmanager.h"
#include "calibration/calibrationuiutils.h" #include "calibration/calibrationuiutils.h"
#include "math.h" #include <math.h>
#include <QThread> #include <QThread>
#include "QDebug" #include "QDebug"
@ -45,6 +45,7 @@ namespace OpenPilot {
SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) : SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
QObject(parent), QObject(parent),
calibratingMag(false), calibratingMag(false),
externalMagAvailable(false),
calibratingAccel(false), calibratingAccel(false),
calibrationStepsMag(), calibrationStepsMag(),
calibrationStepsAccelOnly(), calibrationStepsAccelOnly(),
@ -68,6 +69,21 @@ SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW, << CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW,
tr("Place with left side down, nose south and press Save Position...")); 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.clear();
calibrationStepsAccelOnly << CalibrationStep(CALIBRATION_HELPER_IMAGE_NED, calibrationStepsAccelOnly << CalibrationStep(CALIBRATION_HELPER_IMAGE_NED,
tr("Place horizontally and press Save Position...")) tr("Place horizontally and press Save Position..."))
@ -85,14 +101,20 @@ SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
revoCalibration = RevoCalibration::GetInstance(getObjectManager()); revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration); Q_ASSERT(revoCalibration);
auxMagSettings = AuxMagSettings::GetInstance(getObjectManager());
Q_ASSERT(auxMagSettings);
accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager()); accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
Q_ASSERT(accelGyroSettings); Q_ASSERT(accelGyroSettings);
accelState = AccelState::GetInstance(getObjectManager()); accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState); Q_ASSERT(accelState);
magState = MagState::GetInstance(getObjectManager()); magSensor = MagSensor::GetInstance(getObjectManager());
Q_ASSERT(magState); Q_ASSERT(magSensor);
auxMagSensor = AuxMagSensor::GetInstance(getObjectManager());
Q_ASSERT(auxMagSensor);
homeLocation = HomeLocation::GetInstance(getObjectManager()); homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(homeLocation); Q_ASSERT(homeLocation);
@ -166,6 +188,26 @@ void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag)
revoCalibration->setData(revoCalibrationData); 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); QThread::usleep(100000);
mag_accum_x.clear(); mag_accum_x.clear();
@ -186,12 +228,19 @@ void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag)
} }
// Need to get as many mag updates as possible // Need to get as many mag updates as possible
memento.magStateMetadata = magState->getMetadata(); memento.magSensorMetadata = magSensor->getMetadata();
memento.auxMagSensorMetadata = auxMagSensor->getMetadata();
if (calibrateMag) { if (calibrateMag) {
UAVObject::Metadata mdata = magState->getMetadata(); UAVObject::Metadata mdata = magSensor->getMetadata();
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100; 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 // reset dirty state to forget previous unsaved runs
@ -229,6 +278,9 @@ void SixPointCalibrationModel::savePositionData()
mag_accum_x.clear(); mag_accum_x.clear();
mag_accum_y.clear(); mag_accum_y.clear();
mag_accum_z.clear(); mag_accum_z.clear();
aux_mag_accum_x.clear();
aux_mag_accum_y.clear();
aux_mag_accum_z.clear();
collectingData = true; collectingData = true;
@ -236,10 +288,12 @@ void SixPointCalibrationModel::savePositionData()
#ifdef FITTING_USING_CONTINOUS_ACQUISITION #ifdef FITTING_USING_CONTINOUS_ACQUISITION
// Mag samples are acquired during the whole calibration session, to be used for ellipsoid fit. // Mag samples are acquired during the whole calibration session, to be used for ellipsoid fit.
if (!position) { 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 #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) { if (calibratingAccel) {
connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); 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_x.append(accelStateData.x);
accel_accum_y.append(accelStateData.y); accel_accum_y.append(accelStateData.y);
accel_accum_z.append(accelStateData.z); accel_accum_z.append(accelStateData.z);
} else if (obj->getObjID() == MagState::OBJID) { } else if (obj->getObjID() == MagSensor::OBJID) {
MagState::DataFields magData = magState->getData(); MagSensor::DataFields magData = magSensor->getData();
mag_accum_x.append(magData.x); mag_accum_x.append(magData.x);
mag_accum_y.append(magData.y); mag_accum_y.append(magData.y);
mag_accum_z.append(magData.z); mag_accum_z.append(magData.z);
@ -274,6 +328,19 @@ void SixPointCalibrationModel::getSample(UAVObject *obj)
mag_fit_y.append(magData.y); mag_fit_y.append(magData.y);
mag_fit_z.append(magData.z); mag_fit_z.append(magData.z);
#endif // FITTING_USING_CONTINOUS_ACQUISITION #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 { } else {
Q_ASSERT(0); Q_ASSERT(0);
} }
@ -307,10 +374,8 @@ void SixPointCalibrationModel::getSample(UAVObject *obj)
// Store the mean for this position for the mag // Store the mean for this position for the mag
if (calibratingMag) { if (calibratingMag) {
disconnect(magState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); disconnect(magSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
mag_data_x[position] = CalibrationUtils::listMean(mag_accum_x); disconnect(auxMagSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
mag_data_y[position] = CalibrationUtils::listMean(mag_accum_y);
mag_data_z[position] = CalibrationUtils::listMean(mag_accum_z);
} }
position = (position + 1) % 6; position = (position + 1) % 6;
@ -322,17 +387,19 @@ void SixPointCalibrationModel::getSample(UAVObject *obj)
// done... // done...
#ifdef FITTING_USING_CONTINOUS_ACQUISITION #ifdef FITTING_USING_CONTINOUS_ACQUISITION
if (calibratingMag) { 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 #endif // FITTING_USING_CONTINOUS_ACQUISITION
compute(); compute();
// Restore original settings // Restore original settings
accelState->setMetadata(memento.accelStateMetadata); accelState->setMetadata(memento.accelStateMetadata);
magState->setMetadata(memento.magStateMetadata); magSensor->setMetadata(memento.magSensorMetadata);
auxMagSensor->setMetadata(memento.auxMagSensorMetadata);
revoCalibration->setData(memento.revoCalibrationData); revoCalibration->setData(memento.revoCalibrationData);
accelGyroSettings->setData(memento.accelGyroSettingsData); accelGyroSettings->setData(memento.accelGyroSettingsData);
auxMagSettings->setData(memento.auxMagSettings);
// Recall saved board rotation // Recall saved board rotation
recallBoardRotation(); recallBoardRotation();
@ -347,11 +414,19 @@ void SixPointCalibrationModel::continouslyGetMagSamples(UAVObject *obj)
{ {
QMutexLocker lock(&sensorsUpdateLock); QMutexLocker lock(&sensorsUpdateLock);
if (obj->getObjID() == MagState::OBJID) { if (obj->getObjID() == MagSensor::OBJID) {
MagState::DataFields magStateData = magState->getData(); MagSensor::DataFields magSensorData = magSensor->getData();
mag_fit_x.append(magStateData.x); mag_fit_x.append(magSensorData.x);
mag_fit_y.append(magStateData.y); mag_fit_y.append(magSensorData.y);
mag_fit_z.append(magStateData.z); 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; double Be_length;
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
AuxMagSettings::DataFields auxCalibrationData = auxMagSettings->getData();
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
HomeLocation::DataFields homeLocationData = homeLocation->getData(); HomeLocation::DataFields homeLocationData = homeLocation->getData();
@ -384,51 +460,21 @@ void SixPointCalibrationModel::compute()
// Calibration mag // Calibration mag
if (calibratingMag) { if (calibratingMag) {
Be_length = sqrt(pow(homeLocationData.Be[0], 2) + pow(homeLocationData.Be[1], 2) + pow(homeLocationData.Be[2], 2)); 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); qDebug() << "-----------------------------------";
Eigen::VectorXf samples_y(vectSize); qDebug() << "Onboard Mag";
Eigen::VectorXf samples_z(vectSize); CalcCalibration(mag_fit_x, mag_fit_y, mag_fit_z, Be_length, revoCalibrationData.mag_transform, revoCalibrationData.mag_bias);
for (int i = 0; i < vectSize; i++) { if (externalMagAvailable) {
samples_x(i) = mag_fit_x[i]; qDebug() << "Aux Mag";
samples_y(i) = mag_fit_y[i]; CalcCalibration(aux_mag_fit_x, aux_mag_fit_y, aux_mag_fit_z, Be_length, auxCalibrationData.mag_transform, auxCalibrationData.mag_bias);
samples_z(i) = mag_fit_z[i];
} }
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 // Restore the previous setting
revoCalibrationData.MagBiasNullingRate = memento.revoCalibrationData.MagBiasNullingRate;; revoCalibrationData.MagBiasNullingRate = memento.revoCalibrationData.MagBiasNullingRate;;
// Check the mag calibration is good // Check the mag calibration is good
bool good_calibration = true; bool good_calibration = true;
bool good_aux_calibration = true;
if (calibratingMag) { if (calibratingMag) {
good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0]); good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0]);
good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1]); 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_X]);
good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y]); good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y]);
good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z]); 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 // Check the accel calibration is good
if (calibratingAccel) { if (calibratingAccel) {
@ -458,7 +521,8 @@ void SixPointCalibrationModel::compute()
if (good_calibration) { if (good_calibration) {
m_dirty = true; m_dirty = true;
if (calibratingMag) { if (calibratingMag) {
result.revoCalibrationData = revoCalibrationData; result.revoCalibrationData = revoCalibrationData;
result.auxMagSettingsData = auxCalibrationData;
displayInstructions(tr("Magnetometer calibration completed successfully."), WizardModel::Success); displayInstructions(tr("Magnetometer calibration completed successfully."), WizardModel::Success);
} }
if (calibratingAccel) { if (calibratingAccel) {
@ -473,6 +537,42 @@ void SixPointCalibrationModel::compute()
position = -1; 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() void SixPointCalibrationModel::save()
{ {
if (!m_dirty) { if (!m_dirty) {
@ -489,6 +589,18 @@ void SixPointCalibrationModel::save()
} }
revoCalibration->setData(revoCalibrationData); 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) { if (calibratingAccel) {
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();

25
ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h
View File

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

4
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

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