mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-29 07:24:13 +01:00
OP-975 Move six points calibration to a reusable class
This commit is contained in:
Alessio Morale
parent
73169e0f20
commit
7708cdd15d
@ -423,6 +423,33 @@ int CalibrationUtils::LinearEquationsSolve(int nDim, double *pfMatr, double *pfV
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
double CalibrationUtils::listMean(QList<double> list)
|
||||
{
|
||||
double accum = 0;
|
||||
|
||||
for (int i = 0; i < list.size(); i++) {
|
||||
accum += list[i];
|
||||
}
|
||||
return accum / list.size();
|
||||
}
|
||||
|
||||
double CalibrationUtils::listVar(QList<double> list)
|
||||
{
|
||||
double mean_accum = 0;
|
||||
double var_accum = 0;
|
||||
double mean;
|
||||
|
||||
for (int i = 0; i < list.size(); i++) {
|
||||
mean_accum += list[i];
|
||||
}
|
||||
mean = mean_accum / list.size();
|
||||
|
||||
for (int i = 0; i < list.size(); i++) {
|
||||
var_accum += (list[i] - mean) * (list[i] - mean);
|
||||
}
|
||||
|
||||
// Use unbiased estimator
|
||||
return var_accum / (list.size() - 1);
|
||||
}
|
||||
}
|
||||
|
||||
@ -32,7 +32,7 @@
|
||||
#include <Eigen/Eigenvalues>
|
||||
#include <Eigen/Dense>
|
||||
#include <Eigen/LU>
|
||||
|
||||
#include <QList>
|
||||
namespace OpenPilot {
|
||||
class CalibrationUtils {
|
||||
public:
|
||||
@ -48,7 +48,8 @@ public:
|
||||
static float ComputeSigma(Eigen::VectorXf *samplesY);
|
||||
|
||||
static int SixPointInConstFieldCal(double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3]);
|
||||
|
||||
static double listMean(QList<double> list);
|
||||
static double listVar(QList<double> list);
|
||||
private:
|
||||
static void EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
|
||||
Eigen::Vector3f *center,
|
||||
|
||||
@ -0,0 +1,385 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file sixpointcalibrationmodel.cpp
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
*
|
||||
* @brief Six point calibration for Magnetometer and Accelerometer
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
* @defgroup
|
||||
* @{
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#include "sixpointcalibrationmodel.h"
|
||||
#include <QThread>
|
||||
#include "extensionsystem/pluginmanager.h"
|
||||
#include <QMessageBox>
|
||||
#include "math.h"
|
||||
|
||||
#define POINT_SAMPLE_SIZE 50
|
||||
#define GRAVITY 9.81f
|
||||
#define sign(x) ((x < 0) ? -1 : 1)
|
||||
|
||||
namespace OpenPilot {
|
||||
SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
|
||||
QObject(parent),
|
||||
collectingData(false),
|
||||
calibratingMag(false),
|
||||
calibratingAccel(false),
|
||||
position(-1)
|
||||
{}
|
||||
|
||||
/********** Six point calibration **************/
|
||||
|
||||
void SixPointCalibrationModel::magStart()
|
||||
{
|
||||
start(false, true);
|
||||
}
|
||||
void SixPointCalibrationModel::accelStart()
|
||||
{
|
||||
start(true, false);
|
||||
}
|
||||
|
||||
/**
|
||||
* Called by the "Start" button. Sets up the meta data and enables the
|
||||
* buttons to perform six point calibration of the magnetometer (optionally
|
||||
* accel) to compute the scale and bias of this sensor based on the current
|
||||
* home location magnetic strength.
|
||||
*/
|
||||
void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag)
|
||||
{
|
||||
calibratingAccel = calibrateAccel;
|
||||
calibratingMag = calibrateMag;
|
||||
// Store and reset board rotation before calibration starts
|
||||
isBoardRotationStored = false;
|
||||
storeAndClearBoardRotation();
|
||||
|
||||
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
|
||||
HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
|
||||
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
|
||||
|
||||
Q_ASSERT(revoCalibration);
|
||||
Q_ASSERT(homeLocation);
|
||||
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
|
||||
savedSettings.revoCalibration = revoCalibration->getData();
|
||||
HomeLocation::DataFields homeLocationData = homeLocation->getData();
|
||||
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
|
||||
savedSettings.accelGyroSettings = accelGyroSettings->getData();
|
||||
|
||||
// check if Homelocation is set
|
||||
if (!homeLocationData.Set) {
|
||||
QMessageBox msgBox;
|
||||
msgBox.setInformativeText(tr("<p>HomeLocation not SET.</p><p>Please set your HomeLocation and try again. Aborting calibration!</p>"));
|
||||
msgBox.setStandardButtons(QMessageBox::Ok);
|
||||
msgBox.setDefaultButton(QMessageBox::Ok);
|
||||
msgBox.setIcon(QMessageBox::Information);
|
||||
msgBox.exec();
|
||||
return;
|
||||
}
|
||||
|
||||
// Calibration accel
|
||||
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] = 1;
|
||||
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] = 1;
|
||||
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] = 1;
|
||||
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] = 0;
|
||||
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] = 0;
|
||||
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] = 0;
|
||||
|
||||
accel_accum_x.clear();
|
||||
accel_accum_y.clear();
|
||||
accel_accum_z.clear();
|
||||
|
||||
// Calibration mag
|
||||
// Reset the transformation matrix to identity
|
||||
for (int i = 0; i < RevoCalibration::MAG_TRANSFORM_R2C2; i++) {
|
||||
revoCalibrationData.mag_transform[i] = 0;
|
||||
}
|
||||
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] = 1;
|
||||
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] = 1;
|
||||
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] = 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;
|
||||
|
||||
// Disable adaptive mag nulling
|
||||
initialMagCorrectionRate = revoCalibrationData.MagBiasNullingRate;
|
||||
revoCalibrationData.MagBiasNullingRate = 0;
|
||||
|
||||
revoCalibration->setData(revoCalibrationData);
|
||||
accelGyroSettings->setData(accelGyroSettingsData);
|
||||
|
||||
QThread::usleep(100000);
|
||||
|
||||
mag_accum_x.clear();
|
||||
mag_accum_y.clear();
|
||||
mag_accum_z.clear();
|
||||
|
||||
UAVObject::Metadata mdata;
|
||||
|
||||
/* Need to get as many accel updates as possible */
|
||||
AccelState *accelState = AccelState::GetInstance(getObjectManager());
|
||||
Q_ASSERT(accelState);
|
||||
|
||||
initialAccelStateMdata = accelState->getMetadata();
|
||||
mdata = initialAccelStateMdata;
|
||||
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
|
||||
mdata.flightTelemetryUpdatePeriod = 100;
|
||||
accelState->setMetadata(mdata);
|
||||
|
||||
/* Need to get as many mag updates as possible */
|
||||
MagState *mag = MagState::GetInstance(getObjectManager());
|
||||
Q_ASSERT(mag);
|
||||
initialMagStateMdata = mag->getMetadata();
|
||||
mdata = initialMagStateMdata;
|
||||
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
|
||||
mdata.flightTelemetryUpdatePeriod = 100;
|
||||
mag->setMetadata(mdata);
|
||||
|
||||
/* Show instructions and enable controls */
|
||||
displayInstructions("Place horizontally and click save position...", true);
|
||||
displayVisualHelp("plane-horizontal");
|
||||
disableAllCalibrations();
|
||||
savePositionEnabledChanged(true);
|
||||
position = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Saves the data from the aircraft in one of six positions.
|
||||
* This is called when they click "save position" and starts
|
||||
* averaging data for this position.
|
||||
*/
|
||||
void SixPointCalibrationModel::savePositionData()
|
||||
{
|
||||
QMutexLocker lock(&sensorsUpdateLock);
|
||||
|
||||
savePositionEnabledChanged(false);
|
||||
|
||||
accel_accum_x.clear();
|
||||
accel_accum_y.clear();
|
||||
accel_accum_z.clear();
|
||||
mag_accum_x.clear();
|
||||
mag_accum_y.clear();
|
||||
mag_accum_z.clear();
|
||||
|
||||
collectingData = true;
|
||||
|
||||
AccelState *accelState = AccelState::GetInstance(getObjectManager());
|
||||
Q_ASSERT(accelState);
|
||||
MagState *mag = MagState::GetInstance(getObjectManager());
|
||||
Q_ASSERT(mag);
|
||||
|
||||
connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
|
||||
connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
|
||||
|
||||
displayInstructions("Hold...", false);
|
||||
}
|
||||
|
||||
/**
|
||||
* Grab a sample of mag (optionally accel) data while in this position and
|
||||
* store it for averaging. When sufficient points are collected advance
|
||||
* to the next position (give message to user) or compute the scale and bias
|
||||
*/
|
||||
void SixPointCalibrationModel::getSample(UAVObject *obj)
|
||||
{
|
||||
QMutexLocker lock(&sensorsUpdateLock);
|
||||
|
||||
// This is necessary to prevent a race condition on disconnect signal and another update
|
||||
if (collectingData == true) {
|
||||
if (obj->getObjID() == AccelState::OBJID) {
|
||||
AccelState *accelState = AccelState::GetInstance(getObjectManager());
|
||||
Q_ASSERT(accelState);
|
||||
AccelState::DataFields accelStateData = accelState->getData();
|
||||
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 *mag = MagState::GetInstance(getObjectManager());
|
||||
Q_ASSERT(mag);
|
||||
MagState::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() >= POINT_SAMPLE_SIZE && mag_accum_x.size() >= POINT_SAMPLE_SIZE && collectingData == true) {
|
||||
collectingData = false;
|
||||
|
||||
savePositionEnabledChanged(true);
|
||||
|
||||
// Store the mean for this position for the accel
|
||||
AccelState *accelState = AccelState::GetInstance(getObjectManager());
|
||||
Q_ASSERT(accelState);
|
||||
disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
|
||||
accel_data_x[position] = CalibrationUtils::listMean(accel_accum_x);
|
||||
accel_data_y[position] = CalibrationUtils::listMean(accel_accum_y);
|
||||
accel_data_z[position] = CalibrationUtils::listMean(accel_accum_z);
|
||||
|
||||
// Store the mean for this position for the mag
|
||||
MagState *mag = MagState::GetInstance(getObjectManager());
|
||||
Q_ASSERT(mag);
|
||||
disconnect(mag, 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);
|
||||
|
||||
position = (position + 1) % 6;
|
||||
if (position == 1) {
|
||||
displayInstructions("Place with left side down and click save position...", false);
|
||||
displayVisualHelp("plane-left");
|
||||
}
|
||||
if (position == 2) {
|
||||
displayInstructions("Place upside down and click save position...", false);
|
||||
displayVisualHelp("plane-flip");
|
||||
}
|
||||
if (position == 3) {
|
||||
displayInstructions("Place with right side down and click save position...", false);
|
||||
displayVisualHelp("plane-right");
|
||||
}
|
||||
if (position == 4) {
|
||||
displayInstructions("Place with nose up and click save position...", false);
|
||||
displayVisualHelp("plane-up");
|
||||
}
|
||||
if (position == 5) {
|
||||
displayInstructions("Place with nose down and click save position...", false);
|
||||
displayVisualHelp("plane-down");
|
||||
}
|
||||
if (position == 0) {
|
||||
compute(calibratingMag, calibratingAccel);
|
||||
savePositionEnabledChanged(false);
|
||||
|
||||
enableAllCalibrations();
|
||||
|
||||
/* Cleanup original settings */
|
||||
accelState->setMetadata(initialAccelStateMdata);
|
||||
|
||||
mag->setMetadata(initialMagStateMdata);
|
||||
|
||||
// Recall saved board rotation
|
||||
recallBoardRotation();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Computes the scale and bias for the magnetomer and (compile option)
|
||||
* for the accel once all the data has been collected in 6 positions.
|
||||
*/
|
||||
void SixPointCalibrationModel::compute(bool mag, bool accel)
|
||||
{
|
||||
double S[3], b[3];
|
||||
double Be_length;
|
||||
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
|
||||
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
|
||||
HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
|
||||
|
||||
Q_ASSERT(revoCalibration);
|
||||
Q_ASSERT(homeLocation);
|
||||
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
|
||||
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
|
||||
HomeLocation::DataFields homeLocationData = homeLocation->getData();
|
||||
|
||||
// Calibration accel
|
||||
if (accel) {
|
||||
OpenPilot::CalibrationUtils::SixPointInConstFieldCal(homeLocationData.g_e, accel_data_x, accel_data_y, accel_data_z, S, b);
|
||||
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] = fabs(S[0]);
|
||||
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] = fabs(S[1]);
|
||||
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] = fabs(S[2]);
|
||||
|
||||
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] = -sign(S[0]) * b[0];
|
||||
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] = -sign(S[1]) * b[1];
|
||||
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] = -sign(S[2]) * b[2];
|
||||
}
|
||||
|
||||
// Calibration mag
|
||||
if (mag) {
|
||||
Be_length = sqrt(pow(homeLocationData.Be[0], 2) + pow(homeLocationData.Be[1], 2) + pow(homeLocationData.Be[2], 2));
|
||||
OpenPilot::CalibrationUtils::SixPointInConstFieldCal(Be_length, mag_data_x, mag_data_y, mag_data_z, S, b);
|
||||
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] = fabs(S[0]);
|
||||
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] = fabs(S[1]);
|
||||
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] = fabs(S[2]);
|
||||
|
||||
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];
|
||||
}
|
||||
// Restore the previous setting
|
||||
revoCalibrationData.MagBiasNullingRate = initialMagCorrectionRate;
|
||||
|
||||
|
||||
bool good_calibration = true;
|
||||
|
||||
// Check the mag calibration is good
|
||||
if (mag) {
|
||||
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] ==
|
||||
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0];
|
||||
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] ==
|
||||
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1];
|
||||
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] ==
|
||||
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2];
|
||||
good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] ==
|
||||
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X];
|
||||
good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] ==
|
||||
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y];
|
||||
good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] ==
|
||||
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z];
|
||||
}
|
||||
// Check the accel calibration is good
|
||||
if (accel) {
|
||||
good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] ==
|
||||
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X];
|
||||
good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] ==
|
||||
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y];
|
||||
good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] ==
|
||||
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z];
|
||||
good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] ==
|
||||
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X];
|
||||
good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] ==
|
||||
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y];
|
||||
good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] ==
|
||||
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z];
|
||||
}
|
||||
if (good_calibration) {
|
||||
if (mag) {
|
||||
revoCalibration->setData(revoCalibrationData);
|
||||
} else {
|
||||
revoCalibration->setData(savedSettings.revoCalibration);
|
||||
}
|
||||
|
||||
if (accel) {
|
||||
accelGyroSettings->setData(accelGyroSettingsData);
|
||||
} else {
|
||||
accelGyroSettings->setData(savedSettings.accelGyroSettings);
|
||||
}
|
||||
displayInstructions("Computed sensor scale and bias...", true);
|
||||
} else {
|
||||
displayInstructions("Bad calibration. Please repeat.", true);
|
||||
}
|
||||
position = -1; // set to run again
|
||||
}
|
||||
UAVObjectManager *SixPointCalibrationModel::getObjectManager()
|
||||
{
|
||||
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
|
||||
UAVObjectManager *objMngr = pm->getObject<UAVObjectManager>();
|
||||
|
||||
Q_ASSERT(objMngr);
|
||||
return objMngr;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,101 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file sixpointcalibrationmodel.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
*
|
||||
* @brief Six point calibration for Magnetometer and Accelerometer
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
* @defgroup
|
||||
* @{
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef SIXPOINTCALIBRATIONMODEL_H
|
||||
#define SIXPOINTCALIBRATIONMODEL_H
|
||||
#include <QMutex>
|
||||
#include <QObject>
|
||||
#include <QList>
|
||||
#include "calibration/calibrationutils.h"
|
||||
|
||||
#include <revocalibration.h>
|
||||
#include <accelgyrosettings.h>
|
||||
#include <homelocation.h>
|
||||
#include <accelstate.h>
|
||||
#include <magstate.h>
|
||||
namespace OpenPilot {
|
||||
class SixPointCalibrationModel : public QObject {
|
||||
Q_OBJECT
|
||||
|
||||
typedef struct {
|
||||
RevoCalibration::DataFields revoCalibration;
|
||||
AccelGyroSettings::DataFields accelGyroSettings;
|
||||
} SavedSettings;
|
||||
public:
|
||||
explicit SixPointCalibrationModel(QObject *parent = 0);
|
||||
|
||||
signals:
|
||||
void displayVisualHelp(QString elementID);
|
||||
void displayInstructions(QString instructions, bool replace);
|
||||
void disableAllCalibrations();
|
||||
void enableAllCalibrations();
|
||||
void storeAndClearBoardRotation();
|
||||
void recallBoardRotation();
|
||||
void savePositionEnabledChanged(bool state);
|
||||
|
||||
public slots:
|
||||
// Slots for calibrating the mags
|
||||
void magStart();
|
||||
void accelStart();
|
||||
void savePositionData();
|
||||
private slots:
|
||||
void getSample(UAVObject *obj);
|
||||
private:
|
||||
void start(bool calibrateAccel, bool calibrateMag);
|
||||
UAVObjectManager *getObjectManager();
|
||||
|
||||
|
||||
UAVObject::Metadata initialAccelStateMdata;
|
||||
UAVObject::Metadata initialMagStateMdata;
|
||||
float initialMagCorrectionRate;
|
||||
SavedSettings savedSettings;
|
||||
|
||||
int position;
|
||||
|
||||
bool calibratingMag;
|
||||
bool calibratingAccel;
|
||||
|
||||
bool isBoardRotationStored;
|
||||
|
||||
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];
|
||||
|
||||
QMutex sensorsUpdateLock;
|
||||
|
||||
// ! Computes the scale and bias of the mag based on collected data
|
||||
void compute(bool mag, bool accel);
|
||||
|
||||
bool collectingData;
|
||||
QList<double> accel_accum_x;
|
||||
QList<double> accel_accum_y;
|
||||
QList<double> accel_accum_z;
|
||||
QList<double> mag_accum_x;
|
||||
QList<double> mag_accum_y;
|
||||
QList<double> mag_accum_z;
|
||||
};
|
||||
}
|
||||
#endif // SIXPOINTCALIBRATIONMODEL_H
|
||||
@ -51,7 +51,8 @@ HEADERS += configplugin.h \
|
||||
calibration/thermal/boardsetuptransition.h \
|
||||
calibration/thermal/dataacquisitiontransition.h \
|
||||
calibration/thermal/settingshandlingtransitions.h \
|
||||
calibration/thermal/compensationcalculationtransition.h
|
||||
calibration/thermal/compensationcalculationtransition.h \
|
||||
calibration/sixpointcalibrationmodel.h
|
||||
|
||||
SOURCES += configplugin.cpp \
|
||||
configgadgetwidget.cpp \
|
||||
@ -86,7 +87,8 @@ SOURCES += configplugin.cpp \
|
||||
calibration/wizardmodel.cpp \
|
||||
calibration/thermal/thermalcalibration.cpp \
|
||||
calibration/thermal/thermalcalibrationhelper.cpp \
|
||||
calibration/thermal/thermalcalibrationmodel.cpp
|
||||
calibration/thermal/thermalcalibrationmodel.cpp \
|
||||
calibration/sixpointcalibrationmodel.cpp
|
||||
|
||||
FORMS += airframe.ui \
|
||||
airframe_ccpm.ui \
|
||||
|
||||
@ -50,10 +50,9 @@
|
||||
#include <gyrostate.h>
|
||||
#include <magstate.h>
|
||||
|
||||
#define GRAVITY 9.81f
|
||||
#include "assertions.h"
|
||||
#include "calibration.h"
|
||||
|
||||
#include "calibration/calibrationutils.h"
|
||||
#define sign(x) ((x < 0) ? -1 : 1)
|
||||
|
||||
// Uncomment this to enable 6 point calibration on the accels
|
||||
@ -76,8 +75,6 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
|
||||
ConfigTaskWidget(parent),
|
||||
m_ui(new Ui_RevoSensorsWidget()),
|
||||
collectingData(false),
|
||||
calibratingMag(false),
|
||||
calibratingAccel(false),
|
||||
position(-1),
|
||||
isBoardRotationStored(false)
|
||||
{
|
||||
@ -113,6 +110,20 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
|
||||
connect(m_thermalCalibrationModel, SIGNAL(progressChanged(int)), m_ui->thermalBiasProgress, SLOT(setValue(int)));
|
||||
// note: init for m_thermalCalibrationModel is done in showEvent to prevent cases wiht "Start" button not enabled due to some itming issue.
|
||||
|
||||
m_sixPointCalibrationModel = new OpenPilot::SixPointCalibrationModel(this);
|
||||
// six point calibrations
|
||||
connect(m_ui->sixPointsStartAccel, SIGNAL(clicked()), m_sixPointCalibrationModel, SLOT(accelStart()));
|
||||
connect(m_ui->sixPointsStartMag, SIGNAL(clicked()), m_sixPointCalibrationModel, SLOT(magStart()));
|
||||
connect(m_ui->sixPointsSave, SIGNAL(clicked()), m_sixPointCalibrationModel, SLOT(savePositionData()));
|
||||
|
||||
connect(m_sixPointCalibrationModel, SIGNAL(disableAllCalibrations()), this, SLOT(disableAllCalibrations()));
|
||||
connect(m_sixPointCalibrationModel, SIGNAL(enableAllCalibrations()), this, SLOT(enableAllCalibrations()));
|
||||
connect(m_sixPointCalibrationModel, SIGNAL(storeAndClearBoardRotation()), this, SLOT(storeAndClearBoardRotation()));
|
||||
connect(m_sixPointCalibrationModel, SIGNAL(recallBoardRotation()), this, SLOT(recallBoardRotation()));
|
||||
connect(m_sixPointCalibrationModel, SIGNAL(displayInstructions(QString, bool)), this, SLOT(displayInstructions(QString, bool)));
|
||||
connect(m_sixPointCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString)));
|
||||
connect(m_sixPointCalibrationModel, SIGNAL(savePositionEnabledChanged(bool)), this->m_ui->sixPointsSave, SLOT(setEnabled(bool)));
|
||||
|
||||
// Connect the signals
|
||||
// gyro zero calibration
|
||||
connect(m_ui->gyroBiasStart, SIGNAL(clicked()), this, SLOT(gyroCalibrationStart()));
|
||||
@ -121,10 +132,6 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
|
||||
connect(m_ui->boardLevelStart, SIGNAL(clicked()), this, SLOT(levelCalibrationStart()));
|
||||
connect(m_ui->boardLevelSavePos, SIGNAL(clicked()), this, SLOT(levelCalibrationSavePosition()));
|
||||
|
||||
// six point calibrations
|
||||
connect(m_ui->sixPointsStartAccel, SIGNAL(clicked()), this, SLOT(sixPointCalibrationAccelStart()));
|
||||
connect(m_ui->sixPointsStartMag, SIGNAL(clicked()), this, SLOT(sixPointCalibrationMagStart()));
|
||||
connect(m_ui->sixPointsSave, SIGNAL(clicked()), this, SLOT(sixPointCalibrationSavePositionData()));
|
||||
|
||||
connect(m_ui->hlClearButton, SIGNAL(clicked()), this, SLOT(clearHomeLocation()));
|
||||
|
||||
@ -241,7 +248,7 @@ void ConfigRevoWidget::gyroBiasCalibrationGetSample(UAVObject *obj)
|
||||
collectingData == true) {
|
||||
collectingData = false;
|
||||
|
||||
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
|
||||
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
|
||||
|
||||
disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *)));
|
||||
|
||||
@ -258,9 +265,9 @@ void ConfigRevoWidget::gyroBiasCalibrationGetSample(UAVObject *obj)
|
||||
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE;
|
||||
|
||||
// Update biases based on collected data
|
||||
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += listMean(gyro_accum_x);
|
||||
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += listMean(gyro_accum_y);
|
||||
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += listMean(gyro_accum_z);
|
||||
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += OpenPilot::CalibrationUtils::listMean(gyro_accum_x);
|
||||
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += OpenPilot::CalibrationUtils::listMean(gyro_accum_y);
|
||||
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += OpenPilot::CalibrationUtils::listMean(gyro_accum_z);
|
||||
|
||||
revoCalibration->setData(revoCalibrationData);
|
||||
revoCalibration->updated();
|
||||
@ -282,22 +289,19 @@ void ConfigRevoWidget::gyroBiasCalibrationGetSample(UAVObject *obj)
|
||||
}
|
||||
|
||||
|
||||
|
||||
/******* Level calibration *******/
|
||||
/**
|
||||
* Starts an accelerometer bias calibration.
|
||||
*/
|
||||
void ConfigRevoWidget::levelCalibrationStart()
|
||||
{
|
||||
|
||||
|
||||
// Store and reset board rotation before calibration starts
|
||||
|
||||
disableAllCalibrations();
|
||||
m_ui->boardLevelProgress->setValue(0);
|
||||
|
||||
rot_data_pitch = 0;
|
||||
rot_data_roll = 0;
|
||||
rot_data_roll = 0;
|
||||
UAVObject::Metadata mdata;
|
||||
|
||||
AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
|
||||
@ -314,11 +318,12 @@ void ConfigRevoWidget::levelCalibrationStart()
|
||||
disableAllCalibrations();
|
||||
m_ui->boardLevelSavePos->setEnabled(true);
|
||||
position = 0;
|
||||
|
||||
}
|
||||
|
||||
void ConfigRevoWidget::levelCalibrationSavePosition(){
|
||||
void ConfigRevoWidget::levelCalibrationSavePosition()
|
||||
{
|
||||
QMutexLocker lock(&sensorsUpdateLock);
|
||||
|
||||
Q_UNUSED(lock);
|
||||
|
||||
m_ui->boardLevelSavePos->setEnabled(false);
|
||||
@ -341,6 +346,7 @@ void ConfigRevoWidget::levelCalibrationSavePosition(){
|
||||
void ConfigRevoWidget::levelCalibrationGetSample(UAVObject *obj)
|
||||
{
|
||||
QMutexLocker lock(&sensorsUpdateLock);
|
||||
|
||||
Q_UNUSED(lock);
|
||||
|
||||
switch (obj->getObjID()) {
|
||||
@ -370,10 +376,10 @@ void ConfigRevoWidget::levelCalibrationGetSample(UAVObject *obj)
|
||||
disconnect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *)));
|
||||
|
||||
position++;
|
||||
switch(position){
|
||||
switch (position) {
|
||||
case 1:
|
||||
rot_data_pitch = listMean(rot_accum_pitch);
|
||||
rot_data_roll = listMean(rot_accum_roll);
|
||||
rot_data_pitch = OpenPilot::CalibrationUtils::listMean(rot_accum_pitch);
|
||||
rot_data_roll = OpenPilot::CalibrationUtils::listMean(rot_accum_roll);
|
||||
|
||||
displayInstructions("Leave horizontally, rotate 180° along yaw axis and click save position...", true);
|
||||
displayVisualHelp("plane-horizontal-rotated");
|
||||
@ -383,19 +389,19 @@ void ConfigRevoWidget::levelCalibrationGetSample(UAVObject *obj)
|
||||
m_ui->boardLevelSavePos->setEnabled(true);
|
||||
break;
|
||||
case 2:
|
||||
rot_data_pitch += listMean(rot_accum_pitch);
|
||||
rot_data_pitch += OpenPilot::CalibrationUtils::listMean(rot_accum_pitch);
|
||||
rot_data_pitch /= 2;
|
||||
rot_data_roll += listMean(rot_accum_roll);
|
||||
rot_data_roll /= 2;
|
||||
rot_data_roll += OpenPilot::CalibrationUtils::listMean(rot_accum_roll);
|
||||
rot_data_roll /= 2;
|
||||
attitudeState->setMetadata(initialAttitudeStateMdata);
|
||||
levelCalibrationCompute();
|
||||
enableAllCalibrations();
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
void ConfigRevoWidget::levelCalibrationCompute(){
|
||||
void ConfigRevoWidget::levelCalibrationCompute()
|
||||
{
|
||||
enableAllCalibrations();
|
||||
|
||||
AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
|
||||
@ -405,346 +411,10 @@ void ConfigRevoWidget::levelCalibrationCompute(){
|
||||
// Update the biases based on collected data
|
||||
// "rotate" the board in the opposite direction as the calculated offset
|
||||
attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] -= rot_data_pitch;
|
||||
attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] -= rot_data_roll;
|
||||
attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] -= rot_data_roll;
|
||||
|
||||
attitudeSettings->setData(attitudeSettingsData);
|
||||
attitudeSettings->updated();
|
||||
|
||||
|
||||
}
|
||||
|
||||
/********** Six point calibration **************/
|
||||
|
||||
void ConfigRevoWidget::sixPointCalibrationMagStart(){
|
||||
sixPointCalibrationStart(false, true);
|
||||
}
|
||||
void ConfigRevoWidget::sixPointCalibrationAccelStart(){
|
||||
sixPointCalibrationStart(true, false);
|
||||
}
|
||||
|
||||
/**
|
||||
* Called by the "Start" button. Sets up the meta data and enables the
|
||||
* buttons to perform six point calibration of the magnetometer (optionally
|
||||
* accel) to compute the scale and bias of this sensor based on the current
|
||||
* home location magnetic strength.
|
||||
*/
|
||||
void ConfigRevoWidget::sixPointCalibrationStart(bool calibrateAccel, bool calibrateMag)
|
||||
{
|
||||
calibratingAccel = calibrateAccel;
|
||||
calibratingMag = calibrateMag;
|
||||
// Store and reset board rotation before calibration starts
|
||||
isBoardRotationStored = false;
|
||||
storeAndClearBoardRotation();
|
||||
|
||||
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
|
||||
HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
|
||||
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
|
||||
|
||||
Q_ASSERT(revoCalibration);
|
||||
Q_ASSERT(homeLocation);
|
||||
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
|
||||
savedSettings.revoCalibration = revoCalibration->getData();
|
||||
HomeLocation::DataFields homeLocationData = homeLocation->getData();
|
||||
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
|
||||
savedSettings.accelGyroSettings = accelGyroSettings->getData();
|
||||
|
||||
// check if Homelocation is set
|
||||
if (!homeLocationData.Set) {
|
||||
QMessageBox msgBox;
|
||||
msgBox.setInformativeText(tr("<p>HomeLocation not SET.</p><p>Please set your HomeLocation and try again. Aborting calibration!</p>"));
|
||||
msgBox.setStandardButtons(QMessageBox::Ok);
|
||||
msgBox.setDefaultButton(QMessageBox::Ok);
|
||||
msgBox.setIcon(QMessageBox::Information);
|
||||
msgBox.exec();
|
||||
return;
|
||||
}
|
||||
|
||||
// Calibration accel
|
||||
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] = 1;
|
||||
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] = 1;
|
||||
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] = 1;
|
||||
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] = 0;
|
||||
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] = 0;
|
||||
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] = 0;
|
||||
|
||||
accel_accum_x.clear();
|
||||
accel_accum_y.clear();
|
||||
accel_accum_z.clear();
|
||||
|
||||
// Calibration mag
|
||||
// Reset the transformation matrix to identity
|
||||
for(int i = 0; i < RevoCalibration::MAG_TRANSFORM_R2C2; i++){
|
||||
revoCalibrationData.mag_transform[i] = 0;
|
||||
}
|
||||
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] = 1;
|
||||
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] = 1;
|
||||
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] = 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;
|
||||
|
||||
// Disable adaptive mag nulling
|
||||
initialMagCorrectionRate = revoCalibrationData.MagBiasNullingRate;
|
||||
revoCalibrationData.MagBiasNullingRate = 0;
|
||||
|
||||
revoCalibration->setData(revoCalibrationData);
|
||||
accelGyroSettings->setData(accelGyroSettingsData);
|
||||
|
||||
Thread::usleep(100000);
|
||||
|
||||
gyro_accum_x.clear();
|
||||
gyro_accum_y.clear();
|
||||
gyro_accum_z.clear();
|
||||
mag_accum_x.clear();
|
||||
mag_accum_y.clear();
|
||||
mag_accum_z.clear();
|
||||
|
||||
UAVObject::Metadata mdata;
|
||||
|
||||
/* Need to get as many accel updates as possible */
|
||||
AccelState *accelState = AccelState::GetInstance(getObjectManager());
|
||||
Q_ASSERT(accelState);
|
||||
|
||||
initialAccelStateMdata = accelState->getMetadata();
|
||||
mdata = initialAccelStateMdata;
|
||||
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
|
||||
mdata.flightTelemetryUpdatePeriod = 100;
|
||||
accelState->setMetadata(mdata);
|
||||
|
||||
/* Need to get as many mag updates as possible */
|
||||
MagState *mag = MagState::GetInstance(getObjectManager());
|
||||
Q_ASSERT(mag);
|
||||
initialMagStateMdata = mag->getMetadata();
|
||||
mdata = initialMagStateMdata;
|
||||
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
|
||||
mdata.flightTelemetryUpdatePeriod = 100;
|
||||
mag->setMetadata(mdata);
|
||||
|
||||
/* Show instructions and enable controls */
|
||||
displayInstructions("Place horizontally and click save position...", true);
|
||||
displayVisualHelp("plane-horizontal");
|
||||
disableAllCalibrations();
|
||||
m_ui->sixPointsSave->setEnabled(true);
|
||||
position = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Saves the data from the aircraft in one of six positions.
|
||||
* This is called when they click "save position" and starts
|
||||
* averaging data for this position.
|
||||
*/
|
||||
void ConfigRevoWidget::sixPointCalibrationSavePositionData()
|
||||
{
|
||||
QMutexLocker lock(&sensorsUpdateLock);
|
||||
|
||||
m_ui->sixPointsSave->setEnabled(false);
|
||||
|
||||
accel_accum_x.clear();
|
||||
accel_accum_y.clear();
|
||||
accel_accum_z.clear();
|
||||
mag_accum_x.clear();
|
||||
mag_accum_y.clear();
|
||||
mag_accum_z.clear();
|
||||
|
||||
collectingData = true;
|
||||
|
||||
AccelState *accelState = AccelState::GetInstance(getObjectManager());
|
||||
Q_ASSERT(accelState);
|
||||
MagState *mag = MagState::GetInstance(getObjectManager());
|
||||
Q_ASSERT(mag);
|
||||
|
||||
connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sixPointCalibrationGetSample(UAVObject *)));
|
||||
connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sixPointCalibrationGetSample(UAVObject *)));
|
||||
|
||||
displayInstructions("Hold...");
|
||||
}
|
||||
|
||||
/**
|
||||
* Grab a sample of mag (optionally accel) data while in this position and
|
||||
* store it for averaging. When sufficient points are collected advance
|
||||
* to the next position (give message to user) or compute the scale and bias
|
||||
*/
|
||||
void ConfigRevoWidget::sixPointCalibrationGetSample(UAVObject *obj)
|
||||
{
|
||||
QMutexLocker lock(&sensorsUpdateLock);
|
||||
|
||||
// This is necessary to prevent a race condition on disconnect signal and another update
|
||||
if (collectingData == true) {
|
||||
if (obj->getObjID() == AccelState::OBJID) {
|
||||
AccelState *accelState = AccelState::GetInstance(getObjectManager());
|
||||
Q_ASSERT(accelState);
|
||||
AccelState::DataFields accelStateData = accelState->getData();
|
||||
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 *mag = MagState::GetInstance(getObjectManager());
|
||||
Q_ASSERT(mag);
|
||||
MagState::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() >= SAMPLE_SIZE && mag_accum_x.size() >= SAMPLE_SIZE && collectingData == true) {
|
||||
|
||||
collectingData = false;
|
||||
|
||||
m_ui->sixPointsSave->setEnabled(true);
|
||||
|
||||
// Store the mean for this position for the accel
|
||||
AccelState *accelState = AccelState::GetInstance(getObjectManager());
|
||||
Q_ASSERT(accelState);
|
||||
disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sixPointCalibrationGetSample(UAVObject *)));
|
||||
accel_data_x[position] = listMean(accel_accum_x);
|
||||
accel_data_y[position] = listMean(accel_accum_y);
|
||||
accel_data_z[position] = listMean(accel_accum_z);
|
||||
|
||||
// Store the mean for this position for the mag
|
||||
MagState *mag = MagState::GetInstance(getObjectManager());
|
||||
Q_ASSERT(mag);
|
||||
disconnect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sixPointCalibrationGetSample(UAVObject *)));
|
||||
mag_data_x[position] = listMean(mag_accum_x);
|
||||
mag_data_y[position] = listMean(mag_accum_y);
|
||||
mag_data_z[position] = listMean(mag_accum_z);
|
||||
|
||||
position = (position + 1) % 6;
|
||||
if (position == 1) {
|
||||
displayInstructions("Place with left side down and click save position...");
|
||||
displayVisualHelp("plane-left");
|
||||
}
|
||||
if (position == 2) {
|
||||
displayInstructions("Place upside down and click save position...");
|
||||
displayVisualHelp("plane-flip");
|
||||
}
|
||||
if (position == 3) {
|
||||
displayInstructions("Place with right side down and click save position...");
|
||||
displayVisualHelp("plane-right");
|
||||
}
|
||||
if (position == 4) {
|
||||
displayInstructions("Place with nose up and click save position...");
|
||||
displayVisualHelp("plane-up");
|
||||
}
|
||||
if (position == 5) {
|
||||
displayInstructions("Place with nose down and click save position...");
|
||||
displayVisualHelp("plane-down");
|
||||
}
|
||||
if (position == 0) {
|
||||
sixPointCalibrationCompute(calibratingMag, calibratingAccel);
|
||||
m_ui->sixPointsSave->setEnabled(false);
|
||||
|
||||
enableAllCalibrations();
|
||||
|
||||
/* Cleanup original settings */
|
||||
accelState->setMetadata(initialAccelStateMdata);
|
||||
|
||||
mag->setMetadata(initialMagStateMdata);
|
||||
|
||||
// Recall saved board rotation
|
||||
recallBoardRotation();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Computes the scale and bias for the magnetomer and (compile option)
|
||||
* for the accel once all the data has been collected in 6 positions.
|
||||
*/
|
||||
void ConfigRevoWidget::sixPointCalibrationCompute(bool mag, bool accel)
|
||||
{
|
||||
double S[3], b[3];
|
||||
double Be_length;
|
||||
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
|
||||
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
|
||||
HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
|
||||
|
||||
Q_ASSERT(revoCalibration);
|
||||
Q_ASSERT(homeLocation);
|
||||
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
|
||||
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
|
||||
HomeLocation::DataFields homeLocationData = homeLocation->getData();
|
||||
|
||||
// Calibration accel
|
||||
if(accel) {
|
||||
OpenPilot::CalibrationUtils::SixPointInConstFieldCal(homeLocationData.g_e, accel_data_x, accel_data_y, accel_data_z, S, b);
|
||||
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] = fabs(S[0]);
|
||||
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] = fabs(S[1]);
|
||||
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] = fabs(S[2]);
|
||||
|
||||
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] = -sign(S[0]) * b[0];
|
||||
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] = -sign(S[1]) * b[1];
|
||||
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] = -sign(S[2]) * b[2];
|
||||
}
|
||||
|
||||
// Calibration mag
|
||||
if(mag){
|
||||
Be_length = sqrt(pow(homeLocationData.Be[0], 2) + pow(homeLocationData.Be[1], 2) + pow(homeLocationData.Be[2], 2));
|
||||
OpenPilot::CalibrationUtils::SixPointInConstFieldCal(Be_length, mag_data_x, mag_data_y, mag_data_z, S, b);
|
||||
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] = fabs(S[0]);
|
||||
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] = fabs(S[1]);
|
||||
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] = fabs(S[2]);
|
||||
|
||||
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];
|
||||
}
|
||||
// Restore the previous setting
|
||||
revoCalibrationData.MagBiasNullingRate = initialMagCorrectionRate;
|
||||
|
||||
|
||||
bool good_calibration = true;
|
||||
|
||||
// Check the mag calibration is good
|
||||
if(mag){
|
||||
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] ==
|
||||
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0];
|
||||
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] ==
|
||||
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1];
|
||||
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] ==
|
||||
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2];
|
||||
good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] ==
|
||||
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X];
|
||||
good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] ==
|
||||
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y];
|
||||
good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] ==
|
||||
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z];
|
||||
}
|
||||
// Check the accel calibration is good
|
||||
if(accel){
|
||||
good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] ==
|
||||
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X];
|
||||
good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] ==
|
||||
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y];
|
||||
good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] ==
|
||||
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z];
|
||||
good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] ==
|
||||
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X];
|
||||
good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] ==
|
||||
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y];
|
||||
good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] ==
|
||||
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z];
|
||||
}
|
||||
if (good_calibration) {
|
||||
if(mag){
|
||||
revoCalibration->setData(revoCalibrationData);
|
||||
} else {
|
||||
revoCalibration->setData(savedSettings.revoCalibration);
|
||||
}
|
||||
|
||||
if(accel){
|
||||
accelGyroSettings->setData(accelGyroSettingsData);
|
||||
} else {
|
||||
accelGyroSettings->setData(savedSettings.accelGyroSettings);
|
||||
}
|
||||
displayInstructions("Computed sensor scale and bias...", true);
|
||||
} else {
|
||||
displayInstructions("Bad calibration. Please repeat.", true);
|
||||
}
|
||||
position = -1; // set to run again
|
||||
}
|
||||
|
||||
void ConfigRevoWidget::storeAndClearBoardRotation()
|
||||
@ -795,11 +465,12 @@ void ConfigRevoWidget::displayVisualHelp(QString elementID)
|
||||
m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::KeepAspectRatioByExpanding);
|
||||
}
|
||||
|
||||
void ConfigRevoWidget::displayInstructions(QString instructions, bool replace){
|
||||
if(replace || instructions.isNull()){
|
||||
void ConfigRevoWidget::displayInstructions(QString instructions, bool replace)
|
||||
{
|
||||
if (replace || instructions.isNull()) {
|
||||
m_ui->calibrationInstructions->clear();
|
||||
}
|
||||
if(!instructions.isNull()){
|
||||
if (!instructions.isNull()) {
|
||||
m_ui->calibrationInstructions->append(instructions);
|
||||
}
|
||||
}
|
||||
|
||||
@ -40,33 +40,24 @@
|
||||
#include <QTimer>
|
||||
#include <QMutex>
|
||||
#include "calibration/thermal/thermalcalibrationmodel.h"
|
||||
#include "calibration/calibrationutils.h"
|
||||
#include "calibration/sixpointcalibrationmodel.h"
|
||||
|
||||
class Ui_Widget;
|
||||
|
||||
|
||||
class ConfigRevoWidget : public ConfigTaskWidget {
|
||||
Q_OBJECT
|
||||
typedef struct {
|
||||
RevoCalibration::DataFields revoCalibration;
|
||||
AccelGyroSettings::DataFields accelGyroSettings;
|
||||
} SavedSettings;
|
||||
|
||||
public:
|
||||
ConfigRevoWidget(QWidget *parent = 0);
|
||||
~ConfigRevoWidget();
|
||||
|
||||
private:
|
||||
void displayVisualHelp(QString elementID);
|
||||
|
||||
// ! Computes the scale and bias of the mag based on collected data
|
||||
void sixPointCalibrationCompute(bool mag, bool accel);
|
||||
|
||||
SavedSettings savedSettings;
|
||||
|
||||
OpenPilot::SixPointCalibrationModel *m_sixPointCalibrationModel;
|
||||
OpenPilot::ThermalCalibrationModel *m_thermalCalibrationModel;
|
||||
Ui_RevoSensorsWidget *m_ui;
|
||||
QMutex sensorsUpdateLock;
|
||||
double maxBarHeight;
|
||||
|
||||
int phaseCounter;
|
||||
const static double maxVarValue;
|
||||
const static int calibrationDelay = 10;
|
||||
@ -76,32 +67,17 @@ private:
|
||||
QList<double> gyro_accum_x;
|
||||
QList<double> gyro_accum_y;
|
||||
QList<double> gyro_accum_z;
|
||||
QList<double> accel_accum_x;
|
||||
QList<double> accel_accum_y;
|
||||
QList<double> accel_accum_z;
|
||||
QList<double> mag_accum_x;
|
||||
QList<double> mag_accum_y;
|
||||
QList<double> mag_accum_z;
|
||||
|
||||
QList<double> rot_accum_roll;
|
||||
QList<double> rot_accum_pitch;
|
||||
|
||||
|
||||
|
||||
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];
|
||||
|
||||
double rot_data_roll;
|
||||
double rot_data_pitch;
|
||||
|
||||
bool calibratingMag;
|
||||
bool calibratingAccel;
|
||||
|
||||
UAVObject::Metadata initialAttitudeStateMdata;
|
||||
UAVObject::Metadata initialAccelStateMdata;
|
||||
UAVObject::Metadata initialGyroStateMdata;
|
||||
UAVObject::Metadata initialMagStateMdata;
|
||||
float initialMagCorrectionRate;
|
||||
|
||||
int position;
|
||||
|
||||
@ -110,22 +86,16 @@ private:
|
||||
// Board rotation store/recall
|
||||
qint16 storedBoardRotation[3];
|
||||
bool isBoardRotationStored;
|
||||
|
||||
private slots:
|
||||
void displayVisualHelp(QString elementID);
|
||||
void storeAndClearBoardRotation();
|
||||
void recallBoardRotation();
|
||||
void displayInstructions(QString instructions = QString(), bool replace = false);
|
||||
|
||||
|
||||
private slots:
|
||||
// ! Overriden method from the configTaskWidget to update UI
|
||||
virtual void refreshWidgetsValues(UAVObject *object = NULL);
|
||||
|
||||
// Slots for calibrating the mags
|
||||
void sixPointCalibrationMagStart();
|
||||
void sixPointCalibrationAccelStart();
|
||||
void sixPointCalibrationStart(bool calibrateAccel, bool calibrateMag);
|
||||
void sixPointCalibrationGetSample(UAVObject *obj);
|
||||
void sixPointCalibrationSavePositionData();
|
||||
|
||||
// Slots for calibrating the accel and gyro
|
||||
void levelCalibrationStart();
|
||||
void levelCalibrationSavePosition();
|
||||
|
||||
@ -228,34 +228,6 @@ UAVObjectManager *ConfigTaskWidget::getObjectManager()
|
||||
return objMngr;
|
||||
}
|
||||
|
||||
double ConfigTaskWidget::listMean(QList<double> list)
|
||||
{
|
||||
double accum = 0;
|
||||
|
||||
for (int i = 0; i < list.size(); i++) {
|
||||
accum += list[i];
|
||||
}
|
||||
return accum / list.size();
|
||||
}
|
||||
|
||||
double ConfigTaskWidget::listVar(QList<double> list)
|
||||
{
|
||||
double mean_accum = 0;
|
||||
double var_accum = 0;
|
||||
double mean;
|
||||
|
||||
for (int i = 0; i < list.size(); i++) {
|
||||
mean_accum += list[i];
|
||||
}
|
||||
mean = mean_accum / list.size();
|
||||
|
||||
for (int i = 0; i < list.size(); i++) {
|
||||
var_accum += (list[i] - mean) * (list[i] - mean);
|
||||
}
|
||||
|
||||
// Use unbiased estimator
|
||||
return var_accum / (list.size() - 1);
|
||||
}
|
||||
|
||||
void ConfigTaskWidget::onAutopilotDisconnect()
|
||||
{
|
||||
|
||||
Loading…
Reference in New Issue
Block a user