mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-30 08:24:11 +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;
|
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/Eigenvalues>
|
||||||
#include <Eigen/Dense>
|
#include <Eigen/Dense>
|
||||||
#include <Eigen/LU>
|
#include <Eigen/LU>
|
||||||
|
#include <QList>
|
||||||
namespace OpenPilot {
|
namespace OpenPilot {
|
||||||
class CalibrationUtils {
|
class CalibrationUtils {
|
||||||
public:
|
public:
|
||||||
@ -48,7 +48,8 @@ public:
|
|||||||
static float ComputeSigma(Eigen::VectorXf *samplesY);
|
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 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:
|
private:
|
||||||
static void EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
|
static void EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
|
||||||
Eigen::Vector3f *center,
|
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/boardsetuptransition.h \
|
||||||
calibration/thermal/dataacquisitiontransition.h \
|
calibration/thermal/dataacquisitiontransition.h \
|
||||||
calibration/thermal/settingshandlingtransitions.h \
|
calibration/thermal/settingshandlingtransitions.h \
|
||||||
calibration/thermal/compensationcalculationtransition.h
|
calibration/thermal/compensationcalculationtransition.h \
|
||||||
|
calibration/sixpointcalibrationmodel.h
|
||||||
|
|
||||||
SOURCES += configplugin.cpp \
|
SOURCES += configplugin.cpp \
|
||||||
configgadgetwidget.cpp \
|
configgadgetwidget.cpp \
|
||||||
@ -86,7 +87,8 @@ SOURCES += configplugin.cpp \
|
|||||||
calibration/wizardmodel.cpp \
|
calibration/wizardmodel.cpp \
|
||||||
calibration/thermal/thermalcalibration.cpp \
|
calibration/thermal/thermalcalibration.cpp \
|
||||||
calibration/thermal/thermalcalibrationhelper.cpp \
|
calibration/thermal/thermalcalibrationhelper.cpp \
|
||||||
calibration/thermal/thermalcalibrationmodel.cpp
|
calibration/thermal/thermalcalibrationmodel.cpp \
|
||||||
|
calibration/sixpointcalibrationmodel.cpp
|
||||||
|
|
||||||
FORMS += airframe.ui \
|
FORMS += airframe.ui \
|
||||||
airframe_ccpm.ui \
|
airframe_ccpm.ui \
|
||||||
|
|||||||
@ -50,10 +50,9 @@
|
|||||||
#include <gyrostate.h>
|
#include <gyrostate.h>
|
||||||
#include <magstate.h>
|
#include <magstate.h>
|
||||||
|
|
||||||
#define GRAVITY 9.81f
|
|
||||||
#include "assertions.h"
|
#include "assertions.h"
|
||||||
#include "calibration.h"
|
#include "calibration.h"
|
||||||
|
#include "calibration/calibrationutils.h"
|
||||||
#define sign(x) ((x < 0) ? -1 : 1)
|
#define sign(x) ((x < 0) ? -1 : 1)
|
||||||
|
|
||||||
// Uncomment this to enable 6 point calibration on the accels
|
// Uncomment this to enable 6 point calibration on the accels
|
||||||
@ -76,8 +75,6 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
|
|||||||
ConfigTaskWidget(parent),
|
ConfigTaskWidget(parent),
|
||||||
m_ui(new Ui_RevoSensorsWidget()),
|
m_ui(new Ui_RevoSensorsWidget()),
|
||||||
collectingData(false),
|
collectingData(false),
|
||||||
calibratingMag(false),
|
|
||||||
calibratingAccel(false),
|
|
||||||
position(-1),
|
position(-1),
|
||||||
isBoardRotationStored(false)
|
isBoardRotationStored(false)
|
||||||
{
|
{
|
||||||
@ -113,6 +110,20 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
|
|||||||
connect(m_thermalCalibrationModel, SIGNAL(progressChanged(int)), m_ui->thermalBiasProgress, SLOT(setValue(int)));
|
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.
|
// 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
|
// Connect the signals
|
||||||
// gyro zero calibration
|
// gyro zero calibration
|
||||||
connect(m_ui->gyroBiasStart, SIGNAL(clicked()), this, SLOT(gyroCalibrationStart()));
|
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->boardLevelStart, SIGNAL(clicked()), this, SLOT(levelCalibrationStart()));
|
||||||
connect(m_ui->boardLevelSavePos, SIGNAL(clicked()), this, SLOT(levelCalibrationSavePosition()));
|
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()));
|
connect(m_ui->hlClearButton, SIGNAL(clicked()), this, SLOT(clearHomeLocation()));
|
||||||
|
|
||||||
@ -258,9 +265,9 @@ void ConfigRevoWidget::gyroBiasCalibrationGetSample(UAVObject *obj)
|
|||||||
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE;
|
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE;
|
||||||
|
|
||||||
// Update biases based on collected data
|
// Update biases based on collected data
|
||||||
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += listMean(gyro_accum_x);
|
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += OpenPilot::CalibrationUtils::listMean(gyro_accum_x);
|
||||||
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += listMean(gyro_accum_y);
|
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += OpenPilot::CalibrationUtils::listMean(gyro_accum_y);
|
||||||
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += listMean(gyro_accum_z);
|
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += OpenPilot::CalibrationUtils::listMean(gyro_accum_z);
|
||||||
|
|
||||||
revoCalibration->setData(revoCalibrationData);
|
revoCalibration->setData(revoCalibrationData);
|
||||||
revoCalibration->updated();
|
revoCalibration->updated();
|
||||||
@ -282,15 +289,12 @@ void ConfigRevoWidget::gyroBiasCalibrationGetSample(UAVObject *obj)
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/******* Level calibration *******/
|
/******* Level calibration *******/
|
||||||
/**
|
/**
|
||||||
* Starts an accelerometer bias calibration.
|
* Starts an accelerometer bias calibration.
|
||||||
*/
|
*/
|
||||||
void ConfigRevoWidget::levelCalibrationStart()
|
void ConfigRevoWidget::levelCalibrationStart()
|
||||||
{
|
{
|
||||||
|
|
||||||
|
|
||||||
// Store and reset board rotation before calibration starts
|
// Store and reset board rotation before calibration starts
|
||||||
|
|
||||||
disableAllCalibrations();
|
disableAllCalibrations();
|
||||||
@ -314,11 +318,12 @@ void ConfigRevoWidget::levelCalibrationStart()
|
|||||||
disableAllCalibrations();
|
disableAllCalibrations();
|
||||||
m_ui->boardLevelSavePos->setEnabled(true);
|
m_ui->boardLevelSavePos->setEnabled(true);
|
||||||
position = 0;
|
position = 0;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConfigRevoWidget::levelCalibrationSavePosition(){
|
void ConfigRevoWidget::levelCalibrationSavePosition()
|
||||||
|
{
|
||||||
QMutexLocker lock(&sensorsUpdateLock);
|
QMutexLocker lock(&sensorsUpdateLock);
|
||||||
|
|
||||||
Q_UNUSED(lock);
|
Q_UNUSED(lock);
|
||||||
|
|
||||||
m_ui->boardLevelSavePos->setEnabled(false);
|
m_ui->boardLevelSavePos->setEnabled(false);
|
||||||
@ -341,6 +346,7 @@ void ConfigRevoWidget::levelCalibrationSavePosition(){
|
|||||||
void ConfigRevoWidget::levelCalibrationGetSample(UAVObject *obj)
|
void ConfigRevoWidget::levelCalibrationGetSample(UAVObject *obj)
|
||||||
{
|
{
|
||||||
QMutexLocker lock(&sensorsUpdateLock);
|
QMutexLocker lock(&sensorsUpdateLock);
|
||||||
|
|
||||||
Q_UNUSED(lock);
|
Q_UNUSED(lock);
|
||||||
|
|
||||||
switch (obj->getObjID()) {
|
switch (obj->getObjID()) {
|
||||||
@ -370,10 +376,10 @@ void ConfigRevoWidget::levelCalibrationGetSample(UAVObject *obj)
|
|||||||
disconnect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *)));
|
disconnect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *)));
|
||||||
|
|
||||||
position++;
|
position++;
|
||||||
switch(position){
|
switch (position) {
|
||||||
case 1:
|
case 1:
|
||||||
rot_data_pitch = listMean(rot_accum_pitch);
|
rot_data_pitch = OpenPilot::CalibrationUtils::listMean(rot_accum_pitch);
|
||||||
rot_data_roll = listMean(rot_accum_roll);
|
rot_data_roll = OpenPilot::CalibrationUtils::listMean(rot_accum_roll);
|
||||||
|
|
||||||
displayInstructions("Leave horizontally, rotate 180° along yaw axis and click save position...", true);
|
displayInstructions("Leave horizontally, rotate 180° along yaw axis and click save position...", true);
|
||||||
displayVisualHelp("plane-horizontal-rotated");
|
displayVisualHelp("plane-horizontal-rotated");
|
||||||
@ -383,19 +389,19 @@ void ConfigRevoWidget::levelCalibrationGetSample(UAVObject *obj)
|
|||||||
m_ui->boardLevelSavePos->setEnabled(true);
|
m_ui->boardLevelSavePos->setEnabled(true);
|
||||||
break;
|
break;
|
||||||
case 2:
|
case 2:
|
||||||
rot_data_pitch += listMean(rot_accum_pitch);
|
rot_data_pitch += OpenPilot::CalibrationUtils::listMean(rot_accum_pitch);
|
||||||
rot_data_pitch /= 2;
|
rot_data_pitch /= 2;
|
||||||
rot_data_roll += listMean(rot_accum_roll);
|
rot_data_roll += OpenPilot::CalibrationUtils::listMean(rot_accum_roll);
|
||||||
rot_data_roll /= 2;
|
rot_data_roll /= 2;
|
||||||
attitudeState->setMetadata(initialAttitudeStateMdata);
|
attitudeState->setMetadata(initialAttitudeStateMdata);
|
||||||
levelCalibrationCompute();
|
levelCalibrationCompute();
|
||||||
enableAllCalibrations();
|
enableAllCalibrations();
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
void ConfigRevoWidget::levelCalibrationCompute(){
|
void ConfigRevoWidget::levelCalibrationCompute()
|
||||||
|
{
|
||||||
enableAllCalibrations();
|
enableAllCalibrations();
|
||||||
|
|
||||||
AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
|
AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
|
||||||
@ -409,342 +415,6 @@ void ConfigRevoWidget::levelCalibrationCompute(){
|
|||||||
|
|
||||||
attitudeSettings->setData(attitudeSettingsData);
|
attitudeSettings->setData(attitudeSettingsData);
|
||||||
attitudeSettings->updated();
|
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()
|
void ConfigRevoWidget::storeAndClearBoardRotation()
|
||||||
@ -795,11 +465,12 @@ void ConfigRevoWidget::displayVisualHelp(QString elementID)
|
|||||||
m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::KeepAspectRatioByExpanding);
|
m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::KeepAspectRatioByExpanding);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConfigRevoWidget::displayInstructions(QString instructions, bool replace){
|
void ConfigRevoWidget::displayInstructions(QString instructions, bool replace)
|
||||||
if(replace || instructions.isNull()){
|
{
|
||||||
|
if (replace || instructions.isNull()) {
|
||||||
m_ui->calibrationInstructions->clear();
|
m_ui->calibrationInstructions->clear();
|
||||||
}
|
}
|
||||||
if(!instructions.isNull()){
|
if (!instructions.isNull()) {
|
||||||
m_ui->calibrationInstructions->append(instructions);
|
m_ui->calibrationInstructions->append(instructions);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -40,33 +40,24 @@
|
|||||||
#include <QTimer>
|
#include <QTimer>
|
||||||
#include <QMutex>
|
#include <QMutex>
|
||||||
#include "calibration/thermal/thermalcalibrationmodel.h"
|
#include "calibration/thermal/thermalcalibrationmodel.h"
|
||||||
#include "calibration/calibrationutils.h"
|
#include "calibration/sixpointcalibrationmodel.h"
|
||||||
|
|
||||||
class Ui_Widget;
|
class Ui_Widget;
|
||||||
|
|
||||||
|
|
||||||
class ConfigRevoWidget : public ConfigTaskWidget {
|
class ConfigRevoWidget : public ConfigTaskWidget {
|
||||||
Q_OBJECT
|
Q_OBJECT
|
||||||
typedef struct {
|
|
||||||
RevoCalibration::DataFields revoCalibration;
|
|
||||||
AccelGyroSettings::DataFields accelGyroSettings;
|
|
||||||
} SavedSettings;
|
|
||||||
public:
|
public:
|
||||||
ConfigRevoWidget(QWidget *parent = 0);
|
ConfigRevoWidget(QWidget *parent = 0);
|
||||||
~ConfigRevoWidget();
|
~ConfigRevoWidget();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void displayVisualHelp(QString elementID);
|
OpenPilot::SixPointCalibrationModel *m_sixPointCalibrationModel;
|
||||||
|
|
||||||
// ! Computes the scale and bias of the mag based on collected data
|
|
||||||
void sixPointCalibrationCompute(bool mag, bool accel);
|
|
||||||
|
|
||||||
SavedSettings savedSettings;
|
|
||||||
|
|
||||||
OpenPilot::ThermalCalibrationModel *m_thermalCalibrationModel;
|
OpenPilot::ThermalCalibrationModel *m_thermalCalibrationModel;
|
||||||
Ui_RevoSensorsWidget *m_ui;
|
Ui_RevoSensorsWidget *m_ui;
|
||||||
QMutex sensorsUpdateLock;
|
QMutex sensorsUpdateLock;
|
||||||
double maxBarHeight;
|
|
||||||
int phaseCounter;
|
int phaseCounter;
|
||||||
const static double maxVarValue;
|
const static double maxVarValue;
|
||||||
const static int calibrationDelay = 10;
|
const static int calibrationDelay = 10;
|
||||||
@ -76,32 +67,17 @@ private:
|
|||||||
QList<double> gyro_accum_x;
|
QList<double> gyro_accum_x;
|
||||||
QList<double> gyro_accum_y;
|
QList<double> gyro_accum_y;
|
||||||
QList<double> gyro_accum_z;
|
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_roll;
|
||||||
QList<double> rot_accum_pitch;
|
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_roll;
|
||||||
double rot_data_pitch;
|
double rot_data_pitch;
|
||||||
|
|
||||||
bool calibratingMag;
|
|
||||||
bool calibratingAccel;
|
|
||||||
|
|
||||||
UAVObject::Metadata initialAttitudeStateMdata;
|
UAVObject::Metadata initialAttitudeStateMdata;
|
||||||
UAVObject::Metadata initialAccelStateMdata;
|
|
||||||
UAVObject::Metadata initialGyroStateMdata;
|
UAVObject::Metadata initialGyroStateMdata;
|
||||||
UAVObject::Metadata initialMagStateMdata;
|
|
||||||
float initialMagCorrectionRate;
|
|
||||||
|
|
||||||
int position;
|
int position;
|
||||||
|
|
||||||
@ -110,22 +86,16 @@ private:
|
|||||||
// Board rotation store/recall
|
// Board rotation store/recall
|
||||||
qint16 storedBoardRotation[3];
|
qint16 storedBoardRotation[3];
|
||||||
bool isBoardRotationStored;
|
bool isBoardRotationStored;
|
||||||
|
|
||||||
|
private slots:
|
||||||
|
void displayVisualHelp(QString elementID);
|
||||||
void storeAndClearBoardRotation();
|
void storeAndClearBoardRotation();
|
||||||
void recallBoardRotation();
|
void recallBoardRotation();
|
||||||
void displayInstructions(QString instructions = QString(), bool replace = false);
|
void displayInstructions(QString instructions = QString(), bool replace = false);
|
||||||
|
|
||||||
|
|
||||||
private slots:
|
|
||||||
// ! Overriden method from the configTaskWidget to update UI
|
// ! Overriden method from the configTaskWidget to update UI
|
||||||
virtual void refreshWidgetsValues(UAVObject *object = NULL);
|
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
|
// Slots for calibrating the accel and gyro
|
||||||
void levelCalibrationStart();
|
void levelCalibrationStart();
|
||||||
void levelCalibrationSavePosition();
|
void levelCalibrationSavePosition();
|
||||||
|
|||||||
@ -228,34 +228,6 @@ UAVObjectManager *ConfigTaskWidget::getObjectManager()
|
|||||||
return objMngr;
|
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()
|
void ConfigTaskWidget::onAutopilotDisconnect()
|
||||||
{
|
{
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user