rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Merge branch 'amorale/OP-975_calibration_process_changes' into next

Browse Source
This commit is contained in:
Alessio Morale 2014-05-08 22:56:16 +02:00
commit 7a85c0939c
39 changed files with 1770 additions and 1610 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
flight/modules/Attitude/attitude.c
View File

@ -693,8 +693,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
} }
// Indicates not to expend cycles on rotation // Indicates not to expend cycles on rotation
if (attitudeSettings.BoardRotation.Pitch == 0 && attitudeSettings.BoardRotation.Roll == 0 && if (fabsf(attitudeSettings.BoardRotation.Pitch) < 0.00001f &&
attitudeSettings.BoardRotation.Yaw == 0) { fabsf(attitudeSettings.BoardRotation.Roll) < 0.00001f &&
fabsf(attitudeSettings.BoardRotation.Yaw) < 0.00001f) {
rotate = 0; rotate = 0;
// Shouldn't be used but to be safe // Shouldn't be used but to be safe

78
flight/modules/Sensors/sensors.c
View File

@ -84,10 +84,11 @@ AccelGyroSettingsData agcal;
// These values are initialized by settings but can be updated by the attitude algorithm // These values are initialized by settings but can be updated by the attitude algorithm
static float mag_bias[3] = { 0, 0, 0 }; static float mag_bias[3] = { 0, 0, 0 };
static float mag_scale[3] = { 0, 0, 0 }; static float mag_transform[3][3]={{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1}};
// temp coefficient to calculate gyro bias // temp coefficient to calculate gyro bias
static volatile bool gyro_temp_calibrated = false; static volatile bool gyro_temp_calibrated = false;
static volatile bool accel_temp_calibrated = false; static volatile bool accel_temp_calibrated = false;
static float R[3][3] = { static float R[3][3] = {
{ 0 } { 0 }
}; };
@ -119,6 +120,7 @@ int32_t SensorsInitialize(void)
RevoCalibrationConnectCallback(&settingsUpdatedCb); RevoCalibrationConnectCallback(&settingsUpdatedCb);
AttitudeSettingsConnectCallback(&settingsUpdatedCb); AttitudeSettingsConnectCallback(&settingsUpdatedCb);
AccelGyroSettingsConnectCallback(&settingsUpdatedCb); AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
return 0; return 0;
} }
@ -359,7 +361,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
accels_out[1] -= agcal.accel_temp_coeff.Y * ctemp; accels_out[1] -= agcal.accel_temp_coeff.Y * ctemp;
accels_out[2] -= agcal.accel_temp_coeff.Z * ctemp; accels_out[2] -= agcal.accel_temp_coeff.Z * ctemp;
} }
if (rotate) { if (rotate) {
rot_mult(R, accels_out, accels); rot_mult(R, accels_out, accels);
accelSensorData.x = accels[0]; accelSensorData.x = accels[0];
@ -389,7 +390,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
gyros_out[1] -= (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp; gyros_out[1] -= (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyros_out[2] -= (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp; gyros_out[2] -= (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
} }
if (rotate) { if (rotate) {
rot_mult(R, gyros_out, gyros); rot_mult(R, gyros_out, gyros);
gyroSensorData.x = gyros[0]; gyroSensorData.x = gyros[0];
@ -411,20 +411,16 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
int16_t values[3]; int16_t values[3];
PIOS_HMC5883_ReadMag(values); PIOS_HMC5883_ReadMag(values);
float mags[3] = { (float)values[1] * mag_scale[0] - mag_bias[0], float mags[3] = { (float)values[1] - mag_bias[0],
(float)values[0] * mag_scale[1] - mag_bias[1], (float)values[0] - mag_bias[1],
-(float)values[2] * mag_scale[2] - mag_bias[2] }; -(float)values[2] - mag_bias[2] };
if (rotate) {
float mag_out[3]; float mag_out[3];
rot_mult(R, mags, mag_out); rot_mult(mag_transform, mags, mag_out);
mag.x = mag_out[0];
mag.y = mag_out[1]; mag.x = mag_out[0];
mag.z = mag_out[2]; mag.y = mag_out[1];
} else { mag.z = mag_out[2];
mag.x = mags[0];
mag.y = mags[1];
mag.z = mags[2];
}
MagSensorSet(&mag); MagSensorSet(&mag);
mag_update_time = PIOS_DELAY_GetRaw(); mag_update_time = PIOS_DELAY_GetRaw();
@ -449,9 +445,6 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
mag_bias[0] = cal.mag_bias.X; mag_bias[0] = cal.mag_bias.X;
mag_bias[1] = cal.mag_bias.Y; mag_bias[1] = cal.mag_bias.Y;
mag_bias[2] = cal.mag_bias.Z; mag_bias[2] = cal.mag_bias.Z;
mag_scale[0] = cal.mag_scale.X;
mag_scale[1] = cal.mag_scale.Y;
mag_scale[2] = cal.mag_scale.Z;
accel_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) && accel_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) &&
(fabsf(agcal.accel_temp_coeff.X) > 1e-9f || fabsf(agcal.accel_temp_coeff.Y) > 1e-9f || fabsf(agcal.accel_temp_coeff.Z) > 1e-9f); (fabsf(agcal.accel_temp_coeff.X) > 1e-9f || fabsf(agcal.accel_temp_coeff.Y) > 1e-9f || fabsf(agcal.accel_temp_coeff.Z) > 1e-9f);
@ -465,18 +458,49 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
AttitudeSettingsGet(&attitudeSettings); AttitudeSettingsGet(&attitudeSettings);
// Indicates not to expend cycles on rotation // Indicates not to expend cycles on rotation
if (attitudeSettings.BoardRotation.Roll == 0 && attitudeSettings.BoardRotation.Pitch == 0 && if (fabsf(attitudeSettings.BoardRotation.Roll) < 0.00001f
attitudeSettings.BoardRotation.Yaw == 0) { && fabsf(attitudeSettings.BoardRotation.Pitch) < 0.00001f &&
fabsf(attitudeSettings.BoardRotation.Yaw) <0.00001f ) {
rotate = 0; rotate = 0;
} else { } else {
float rotationQuat[4]; rotate = 1;
}
float rotationQuat[4];
const float rpy[3] = { attitudeSettings.BoardRotation.Roll, const float rpy[3] = { attitudeSettings.BoardRotation.Roll,
attitudeSettings.BoardRotation.Pitch, attitudeSettings.BoardRotation.Pitch,
attitudeSettings.BoardRotation.Yaw }; attitudeSettings.BoardRotation.Yaw };
RPY2Quaternion(rpy, rotationQuat); RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R); Quaternion2R(rotationQuat, R);
rotate = 1;
} mag_transform[0][0] = R[0][0] * cal.mag_transform.r0c0 +
R[1][0] * cal.mag_transform.r0c1 +
R[2][0] * cal.mag_transform.r0c2;
mag_transform[0][1] = R[0][1] * cal.mag_transform.r0c0 +
R[1][1] * cal.mag_transform.r0c1 +
R[2][1] * cal.mag_transform.r0c2;
mag_transform[0][2] = R[0][2] * cal.mag_transform.r0c0 +
R[1][2] * cal.mag_transform.r0c1 +
R[2][2] * cal.mag_transform.r0c2;
mag_transform[1][0] = R[0][0] * cal.mag_transform.r1c0 +
R[1][0] * cal.mag_transform.r1c1 +
R[2][0] * cal.mag_transform.r1c2;
mag_transform[1][1] = R[0][1] * cal.mag_transform.r1c0 +
R[1][1] * cal.mag_transform.r1c1 +
R[2][1] * cal.mag_transform.r1c2;
mag_transform[1][2] = R[0][2] * cal.mag_transform.r1c0 +
R[1][2] * cal.mag_transform.r1c1 +
R[2][2] * cal.mag_transform.r1c2;
mag_transform[1][0] = R[0][0] * cal.mag_transform.r2c0 +
R[1][0] * cal.mag_transform.r2c1 +
R[2][0] * cal.mag_transform.r2c2;
mag_transform[2][1] = R[0][1] * cal.mag_transform.r2c0 +
R[1][1] * cal.mag_transform.r2c1 +
R[2][1] * cal.mag_transform.r2c2;
mag_transform[2][2] = R[0][2] * cal.mag_transform.r2c0 +
R[1][2] * cal.mag_transform.r2c1 +
R[2][2] * cal.mag_transform.r2c2;
} }
/** /**
* @} * @}

16
ground/openpilotgcs/src/plugins/config/calibration/calibrationuiutils.h Normal file
View File

@ -0,0 +1,16 @@
#ifndef CALIBRATIONUIUTILS_H
#define CALIBRATIONUIUTILS_H
#define CALIBRATION_HELPER_IMAGE_NED QStringLiteral("ned")
#define CALIBRATION_HELPER_IMAGE_DWN QStringLiteral("dwn")
#define CALIBRATION_HELPER_IMAGE_ENU QStringLiteral("enu")
#define CALIBRATION_HELPER_IMAGE_SUW QStringLiteral("suw")
#define CALIBRATION_HELPER_IMAGE_SWD QStringLiteral("swd")
#define CALIBRATION_HELPER_IMAGE_USE QStringLiteral("use")
#define CALIBRATION_HELPER_IMAGE_WDS QStringLiteral("wds")
#define CALIBRATION_HELPER_IMAGE_EMPTY QStringLiteral("empty")
#define CALIBRATION_HELPER_BOARD_PREFIX QStringLiteral("board-")
#define CALIBRATION_HELPER_PLANE_PREFIX QStringLiteral("plane-")
#endif // CALIBRATIONUIUTILS_H

213
ground/openpilotgcs/src/plugins/config/calibration/calibrationutils.cpp
View File

@ -42,13 +42,16 @@ float CalibrationUtils::ComputeSigma(Eigen::VectorXf *samplesY)
* The following ellipsoid calibration code is based on RazorImu calibration samples that can be found here: * The following ellipsoid calibration code is based on RazorImu calibration samples that can be found here:
* https://github.com/ptrbrtz/razor-9dof-ahrs/tree/master/Matlab/magnetometer_calibration * https://github.com/ptrbrtz/razor-9dof-ahrs/tree/master/Matlab/magnetometer_calibration
*/ */
bool CalibrationUtils::EllipsoidCalibration(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ, float nominalRange, EllipsoidCalibrationResult *result) bool CalibrationUtils::EllipsoidCalibration(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
float nominalRange,
EllipsoidCalibrationResult *result,
bool fitAlongXYZ)
{ {
Eigen::VectorXf radii; Eigen::VectorXf radii;
Eigen::Vector3f center; Eigen::Vector3f center;
Eigen::MatrixXf evecs; Eigen::MatrixXf evecs;
EllipsoidFit(samplesX, samplesY, samplesZ, &center, &radii, &evecs); EllipsoidFit(samplesX, samplesY, samplesZ, &center, &radii, &evecs, fitAlongXYZ);
result->Scale.setZero(); result->Scale.setZero();
@ -280,26 +283,36 @@ void CalibrationUtils::ComputePoly(VectorXf *samplesX, Eigen::VectorXf *polynomi
*/ */
void CalibrationUtils::EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ, void CalibrationUtils::EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
Eigen::Vector3f *center, Eigen::Vector3f *center,
Eigen::VectorXf *radii, Eigen::VectorXf *radii,
Eigen::MatrixXf *evecs) Eigen::MatrixXf *evecs,
bool fitAlongXYZ)
{ {
int numSamples = (*samplesX).rows(); int numSamples = (*samplesX).rows();
Eigen::MatrixXf D(numSamples, 9); Eigen::MatrixXf D;
D.setZero();
D.col(0) = (*samplesX).cwiseProduct(*samplesX);
D.col(1) = (*samplesY).cwiseProduct(*samplesY);
D.col(2) = (*samplesZ).cwiseProduct(*samplesZ);
D.col(3) = (*samplesX).cwiseProduct(*samplesY) * 2;
D.col(4) = (*samplesX).cwiseProduct(*samplesZ) * 2;
D.col(5) = (*samplesY).cwiseProduct(*samplesZ) * 2;
D.col(6) = 2 * (*samplesX);
D.col(7) = 2 * (*samplesY);
D.col(8) = 2 * (*samplesZ);
if (!fitAlongXYZ) {
D.setZero(numSamples, 9);
D.col(0) = (*samplesX).cwiseProduct(*samplesX);
D.col(1) = (*samplesY).cwiseProduct(*samplesY);
D.col(2) = (*samplesZ).cwiseProduct(*samplesZ);
D.col(3) = (*samplesX).cwiseProduct(*samplesY) * 2;
D.col(4) = (*samplesX).cwiseProduct(*samplesZ) * 2;
D.col(5) = (*samplesY).cwiseProduct(*samplesZ) * 2;
D.col(6) = 2 * (*samplesX);
D.col(7) = 2 * (*samplesY);
D.col(8) = 2 * (*samplesZ);
} else {
D.setZero(numSamples, 6);
D.setZero();
D.col(0) = (*samplesX).cwiseProduct(*samplesX);
D.col(1) = (*samplesY).cwiseProduct(*samplesY);
D.col(2) = (*samplesZ).cwiseProduct(*samplesZ);
D.col(3) = 2 * (*samplesX);
D.col(4) = 2 * (*samplesY);
D.col(5) = 2 * (*samplesZ);
}
Eigen::VectorXf ones(numSamples); Eigen::VectorXf ones(numSamples);
ones.setOnes(numSamples); ones.setOnes(numSamples);
@ -307,25 +320,163 @@ void CalibrationUtils::EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *
Eigen::MatrixXf dt2 = (D.transpose() * ones); Eigen::MatrixXf dt2 = (D.transpose() * ones);
Eigen::VectorXf v = dt1.inverse() * dt2; Eigen::VectorXf v = dt1.inverse() * dt2;
Eigen::Matrix4f A; if (!fitAlongXYZ) {
A << v.coeff(0), v.coeff(3), v.coeff(4), v.coeff(6), Eigen::Matrix4f A;
v.coeff(3), v.coeff(1), v.coeff(5), v.coeff(7), A << v.coeff(0), v.coeff(3), v.coeff(4), v.coeff(6),
v.coeff(4), v.coeff(5), v.coeff(2), v.coeff(8), v.coeff(3), v.coeff(1), v.coeff(5), v.coeff(7),
v.coeff(6), v.coeff(7), v.coeff(8), -1; v.coeff(4), v.coeff(5), v.coeff(2), v.coeff(8),
v.coeff(6), v.coeff(7), v.coeff(8), -1;
(*center) = -1 * A.block(0, 0, 3, 3).inverse() * v.segment(6, 3); (*center) = -1 * A.block(0, 0, 3, 3).inverse() * v.segment(6, 3);
Eigen::Matrix4f t = Eigen::Matrix4f::Identity(); Eigen::Matrix4f t = Eigen::Matrix4f::Identity();
t.block(3, 0, 1, 3) = center->transpose(); t.block(3, 0, 1, 3) = center->transpose();
Eigen::Matrix4f r = t * A * t.transpose(); Eigen::Matrix4f r = t * A * t.transpose();
Eigen::Matrix3f tmp2 = r.block(0, 0, 3, 3) * -1 / r.coeff(3, 3); Eigen::Matrix3f tmp2 = r.block(0, 0, 3, 3) * -1 / r.coeff(3, 3);
Eigen::EigenSolver<Eigen::Matrix3f> es(tmp2); Eigen::EigenSolver<Eigen::Matrix3f> es(tmp2);
Eigen::VectorXf evals = es.eigenvalues().real(); Eigen::VectorXf evals = es.eigenvalues().real();
(*evecs) = es.eigenvectors().real();
radii->setZero(3);
(*radii) = (evals.segment(0, 3)).cwiseInverse().cwiseSqrt(); (*evecs) = es.eigenvectors().real();
radii->setZero(3);
(*radii) = (evals.segment(0, 3)).cwiseInverse().cwiseSqrt();
} else {
Eigen::VectorXf v1(9);
v1 << v.coeff(0), v.coeff(1), v.coeff(2), 0.0f, 0.0f, 0.0f, v.coeff(3), v.coeff(4), v.coeff(5);
(*center) = (-1) * v1.segment(6, 3).cwiseProduct(v1.segment(0, 3).cwiseInverse());
float gam = 1 + (v1.coeff(6) * v1.coeff(6) / v1.coeff(0) +
v1.coeff(7) * v1.coeff(7) / v1.coeff(1) +
v1.coeff(8) * v1.coeff(8) / v1.coeff(2));
radii->setZero(3);
(*radii) = (v1.segment(0, 3).cwiseInverse() * gam).cwiseSqrt();
evecs->setIdentity(3, 3);
}
}
int CalibrationUtils::SixPointInConstFieldCal(double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3])
{
int i;
double A[5][5];
double f[5], c[5];
double xp, yp, zp, Sx;
// Fill in matrix A -
// write six difference-in-magnitude equations of the form
// Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0
// or in other words
// 2*Sx*bx*(x2-x1)/Sx^2 + Sy^2(y2^2-y1^2)/Sx^2 + 2*Sy*by*(y2-y1)/Sx^2 + Sz^2(z2^2-z1^2)/Sx^2 + 2*Sz*bz*(z2-z1)/Sx^2 = (x1^2-x2^2)
for (i = 0; i < 5; i++) {
A[i][0] = 2.0 * (x[i + 1] - x[i]);
A[i][1] = y[i + 1] * y[i + 1] - y[i] * y[i];
A[i][2] = 2.0 * (y[i + 1] - y[i]);
A[i][3] = z[i + 1] * z[i + 1] - z[i] * z[i];
A[i][4] = 2.0 * (z[i + 1] - z[i]);
f[i] = x[i] * x[i] - x[i + 1] * x[i + 1];
}
// solve for c0=bx/Sx, c1=Sy^2/Sx^2; c2=Sy*by/Sx^2, c3=Sz^2/Sx^2, c4=Sz*bz/Sx^2
if (!LinearEquationsSolve(5, (double *)A, f, c)) {
return 0;
}
// use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer
xp = x[0]; yp = y[0]; zp = z[0];
Sx = sqrt(ConstMag * ConstMag / (xp * xp + 2 * c[0] * xp + c[0] * c[0] + c[1] * yp * yp + 2 * c[2] * yp + c[2] * c[2] / c[1] + c[3] * zp * zp + 2 * c[4] * zp + c[4] * c[4] / c[3]));
S[0] = Sx;
b[0] = Sx * c[0];
S[1] = sqrt(c[1] * Sx * Sx);
b[1] = c[2] * Sx * Sx / S[1];
S[2] = sqrt(c[3] * Sx * Sx);
b[2] = c[4] * Sx * Sx / S[2];
return 1;
}
int CalibrationUtils::LinearEquationsSolve(int nDim, double *pfMatr, double *pfVect, double *pfSolution)
{
double fMaxElem;
double fAcc;
int i, j, k, m;
for (k = 0; k < (nDim - 1); k++) { // base row of matrix
// search of line with max element
fMaxElem = fabs(pfMatr[k * nDim + k]);
m = k;
for (i = k + 1; i < nDim; i++) {
if (fMaxElem < fabs(pfMatr[i * nDim + k])) {
fMaxElem = pfMatr[i * nDim + k];
m = i;
}
}
// permutation of base line (index k) and max element line(index m)
if (m != k) {
for (i = k; i < nDim; i++) {
fAcc = pfMatr[k * nDim + i];
pfMatr[k * nDim + i] = pfMatr[m * nDim + i];
pfMatr[m * nDim + i] = fAcc;
}
fAcc = pfVect[k];
pfVect[k] = pfVect[m];
pfVect[m] = fAcc;
}
if (pfMatr[k * nDim + k] == 0.) {
return 0; // needs improvement !!!
}
// triangulation of matrix with coefficients
for (j = (k + 1); j < nDim; j++) { // current row of matrix
fAcc = -pfMatr[j * nDim + k] / pfMatr[k * nDim + k];
for (i = k; i < nDim; i++) {
pfMatr[j * nDim + i] = pfMatr[j * nDim + i] + fAcc * pfMatr[k * nDim + i];
}
pfVect[j] = pfVect[j] + fAcc * pfVect[k]; // free member recalculation
}
}
for (k = (nDim - 1); k >= 0; k--) {
pfSolution[k] = pfVect[k];
for (i = (k + 1); i < nDim; i++) {
pfSolution[k] -= (pfMatr[k * nDim + i] * pfSolution[i]);
}
pfSolution[k] = pfSolution[k] / pfMatr[k * nDim + k];
}
return 1;
}
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);
} }
} }

14
ground/openpilotgcs/src/plugins/config/calibration/calibrationutils.h
View File

@ -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:
@ -41,17 +41,25 @@ public:
Eigen::Vector3f Scale; Eigen::Vector3f Scale;
Eigen::Vector3f Bias; Eigen::Vector3f Bias;
}; };
static bool EllipsoidCalibration(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ, float nominalRange, EllipsoidCalibrationResult *result); static bool EllipsoidCalibration(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
float nominalRange,
EllipsoidCalibrationResult *result,
bool fitAlongXYZ);
static bool PolynomialCalibration(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, int degree, Eigen::Ref<Eigen::VectorXf> result, const double maxRelativeError); static bool PolynomialCalibration(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, int degree, Eigen::Ref<Eigen::VectorXf> result, const double maxRelativeError);
static void ComputePoly(Eigen::VectorXf *samplesX, Eigen::VectorXf *polynomial, Eigen::VectorXf *polyY); static void ComputePoly(Eigen::VectorXf *samplesX, Eigen::VectorXf *polynomial, Eigen::VectorXf *polyY);
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 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,
Eigen::VectorXf *radii, Eigen::VectorXf *radii,
Eigen::MatrixXf *evecs); Eigen::MatrixXf *evecs, bool fitAlongXYZ);
static int LinearEquationsSolve(int nDim, double *pfMatr, double *pfVect, double *pfSolution);
}; };
} }
#endif // CALIBRATIONUTILS_H #endif // CALIBRATIONUTILS_H

216
ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp Normal file
View File

@ -0,0 +1,216 @@
/**
******************************************************************************
*
* @file gyrobiascalibrationmodel.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @addtogroup board level calibration
* @{
* @addtogroup ConfigPlugin Config Plugin
* @{
* @brief Telemetry configuration panel
*****************************************************************************/
/*
* 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 <attitudestate.h>
#include <attitudesettings.h>
#include "extensionsystem/pluginmanager.h"
#include <gyrostate.h>
#include <gyrosensor.h>
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include "calibration/gyrobiascalibrationmodel.h"
#include "calibration/calibrationutils.h"
#include "calibration/calibrationuiutils.h"
static const int LEVEL_SAMPLES = 100;
#include "gyrobiascalibrationmodel.h"
namespace OpenPilot {
GyroBiasCalibrationModel::GyroBiasCalibrationModel(QObject *parent) :
QObject(parent),
collectingData(false)
{}
/******* gyro bias zero ******/
void GyroBiasCalibrationModel::start()
{
// Store and reset board rotation before calibration starts
storeAndClearBoardRotation();
disableAllCalibrations();
progressChanged(0);
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE;
revoCalibration->setData(revoCalibrationData);
revoCalibration->updated();
// Disable gyro bias correction while calibrating
AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
Q_ASSERT(attitudeSettings);
AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_NED);
displayInstructions(tr("Calibrating the gyroscopes. Keep the copter/plane steady..."), true);
gyro_accum_x.clear();
gyro_accum_y.clear();
gyro_accum_z.clear();
gyro_state_accum_x.clear();
gyro_state_accum_y.clear();
gyro_state_accum_z.clear();
UAVObject::Metadata metadata;
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
initialGyroStateMdata = gyroState->getMetadata();
metadata = initialGyroStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(metadata, UAVObject::UPDATEMODE_PERIODIC);
metadata.flightTelemetryUpdatePeriod = 100;
gyroState->setMetadata(metadata);
UAVObject::Metadata gyroSensorMetadata;
GyroSensor *gyroSensor = GyroSensor::GetInstance(getObjectManager());
Q_ASSERT(gyroSensor);
initialGyroSensorMdata = gyroSensor->getMetadata();
gyroSensorMetadata = initialGyroSensorMdata;
UAVObject::SetFlightTelemetryUpdateMode(gyroSensorMetadata, UAVObject::UPDATEMODE_PERIODIC);
gyroSensorMetadata.flightTelemetryUpdatePeriod = 100;
gyroSensor->setMetadata(gyroSensorMetadata);
// Now connect to the accels and mag updates, gather for 100 samples
collectingData = true;
connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
connect(gyroSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
}
/**
Updates the gyro bias raw values
*/
void GyroBiasCalibrationModel::getSample(UAVObject *obj)
{
QMutexLocker lock(&sensorsUpdateLock);
Q_UNUSED(lock);
switch (obj->getObjID()) {
case GyroState::OBJID:
{
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
GyroState::DataFields gyroStateData = gyroState->getData();
gyro_state_accum_x.append(gyroStateData.x);
gyro_state_accum_y.append(gyroStateData.y);
gyro_state_accum_z.append(gyroStateData.z);
break;
}
case GyroSensor::OBJID:
{
GyroSensor *gyroSensor = GyroSensor::GetInstance(getObjectManager());
Q_ASSERT(gyroSensor);
GyroSensor::DataFields gyroSensorData = gyroSensor->getData();
gyro_accum_x.append(gyroSensorData.x);
gyro_accum_y.append(gyroSensorData.y);
gyro_accum_z.append(gyroSensorData.z);
break;
}
default:
Q_ASSERT(0);
}
// Work out the progress based on whichever has less
double p1 = (double)gyro_state_accum_x.size() / (double)LEVEL_SAMPLES;
double p2 = (double)gyro_accum_y.size() / (double)LEVEL_SAMPLES;
progressChanged(((p1 > p2) ? p1 : p2) * 100);
if ((gyro_accum_y.size() >= LEVEL_SAMPLES || (gyro_accum_y.size() == 0 && gyro_state_accum_y.size() >= LEVEL_SAMPLES)) &&
collectingData == true) {
collectingData = false;
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
Q_ASSERT(gyroState);
GyroSensor *gyroSensor = GyroSensor::GetInstance(getObjectManager());
Q_ASSERT(gyroSensor);
disconnect(gyroSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
enableAllCalibrations();
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
Q_ASSERT(accelGyroSettings);
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE;
// Update biases based on collected data
// check whether the board does supports gyroSensor(updates were received)
if (gyro_accum_x.count() < LEVEL_SAMPLES / 10) {
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_x);
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_y);
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_z);
} else {
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();
accelGyroSettings->setData(accelGyroSettingsData);
accelGyroSettings->updated();
AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
Q_ASSERT(attitudeSettings);
AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE;
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
gyroState->setMetadata(initialGyroStateMdata);
gyroSensor->setMetadata(initialGyroSensorMdata);
displayInstructions(tr("Gyroscope calibration computed succesfully."), false);
displayVisualHelp(CALIBRATION_HELPER_IMAGE_EMPTY);
// Recall saved board rotation
recallBoardRotation();
}
}
UAVObjectManager *GyroBiasCalibrationModel::getObjectManager()
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objMngr = pm->getObject<UAVObjectManager>();
Q_ASSERT(objMngr);
return objMngr;
}
}

72
ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.h Normal file
View File

@ -0,0 +1,72 @@
/**
******************************************************************************
*
* @file gyrobiascalibrationmodel.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @addtogroup board level calibration
* @{
* @addtogroup ConfigPlugin Config Plugin
* @{
* @brief Telemetry configuration panel
*****************************************************************************/
/*
* 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 GYROBIASCALIBRATIONMODEL_H
#define GYROBIASCALIBRATIONMODEL_H
#include <QObject>
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
#include "uavobject.h"
namespace OpenPilot {
class GyroBiasCalibrationModel : public QObject {
Q_OBJECT
public:
explicit GyroBiasCalibrationModel(QObject *parent = 0);
signals:
void displayVisualHelp(QString elementID);
void displayInstructions(QString instructions, bool replace);
void disableAllCalibrations();
void enableAllCalibrations();
void storeAndClearBoardRotation();
void recallBoardRotation();
void progressChanged(int value);
public slots:
// Slots for gyro bias zero
void start();
private slots:
void getSample(UAVObject *obj);
private:
QMutex sensorsUpdateLock;
bool collectingData;
QList<double> gyro_accum_x;
QList<double> gyro_accum_y;
QList<double> gyro_accum_z;
QList<double> gyro_state_accum_x;
QList<double> gyro_state_accum_y;
QList<double> gyro_state_accum_z;
UAVObject::Metadata initialGyroStateMdata;
UAVObject::Metadata initialGyroSensorMdata;
UAVObjectManager *getObjectManager();
};
}
#endif // GYROBIASCALIBRATIONMODEL_H

178
ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.cpp Normal file
View File

@ -0,0 +1,178 @@
/**
******************************************************************************
*
* @file levelcalibrationmodel.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @addtogroup board level calibration
* @{
* @addtogroup ConfigPlugin Config Plugin
* @{
* @brief Telemetry configuration panel
*****************************************************************************/
/*
* 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 "levelcalibrationmodel.h"
#include <attitudestate.h>
#include <attitudesettings.h>
#include "extensionsystem/pluginmanager.h"
#include "calibration/calibrationuiutils.h"
static const int LEVEL_SAMPLES = 100;
namespace OpenPilot {
LevelCalibrationModel::LevelCalibrationModel(QObject *parent) :
QObject(parent)
{}
/******* Level calibration *******/
/**
* Starts an accelerometer bias calibration.
*/
void LevelCalibrationModel::start()
{
// Store and reset board rotation before calibration starts
disableAllCalibrations();
progressChanged(0);
rot_data_pitch = 0;
rot_data_roll = 0;
UAVObject::Metadata mdata;
AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
Q_ASSERT(attitudeState);
initialAttitudeStateMdata = attitudeState->getMetadata();
mdata = initialAttitudeStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
attitudeState->setMetadata(mdata);
/* Show instructions and enable controls */
displayInstructions(tr("Place horizontally and click Save Position button..."), true);
displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_NED);
disableAllCalibrations();
savePositionEnabledChanged(true);
position = 0;
}
void LevelCalibrationModel::savePosition()
{
QMutexLocker lock(&sensorsUpdateLock);
Q_UNUSED(lock);
savePositionEnabledChanged(false);
rot_accum_pitch.clear();
rot_accum_roll.clear();
collectingData = true;
AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
Q_ASSERT(attitudeState);
connect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
displayInstructions(tr("Hold..."), false);
}
/**
Updates the accel bias raw values
*/
void LevelCalibrationModel::getSample(UAVObject *obj)
{
QMutexLocker lock(&sensorsUpdateLock);
Q_UNUSED(lock);
switch (obj->getObjID()) {
case AttitudeState::OBJID:
{
AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
Q_ASSERT(attitudeState);
AttitudeState::DataFields attitudeStateData = attitudeState->getData();
rot_accum_roll.append(attitudeStateData.Roll);
rot_accum_pitch.append(attitudeStateData.Pitch);
break;
}
default:
Q_ASSERT(0);
}
// Work out the progress based on whichever has less
double p1 = (double)rot_accum_roll.size() / (double)LEVEL_SAMPLES;
progressChanged(p1 * 100);
if (rot_accum_roll.size() >= LEVEL_SAMPLES &&
collectingData == true) {
collectingData = false;
AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
disconnect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
position++;
switch (position) {
case 1:
rot_data_pitch = OpenPilot::CalibrationUtils::listMean(rot_accum_pitch);
rot_data_roll = OpenPilot::CalibrationUtils::listMean(rot_accum_roll);
displayInstructions(tr("Leave horizontally, rotate 180° along yaw axis and click Save Position button..."), false);
displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_SWD);
disableAllCalibrations();
savePositionEnabledChanged(true);
break;
case 2:
rot_data_pitch += OpenPilot::CalibrationUtils::listMean(rot_accum_pitch);
rot_data_pitch /= 2;
rot_data_roll += OpenPilot::CalibrationUtils::listMean(rot_accum_roll);
rot_data_roll /= 2;
attitudeState->setMetadata(initialAttitudeStateMdata);
compute();
enableAllCalibrations();
displayVisualHelp(CALIBRATION_HELPER_IMAGE_EMPTY);
displayInstructions(tr("Board leveling computed successfully."), false);
break;
}
}
}
void LevelCalibrationModel::compute()
{
enableAllCalibrations();
AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
Q_ASSERT(attitudeSettings);
AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
// 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;
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
}
UAVObjectManager *LevelCalibrationModel::getObjectManager()
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objMngr = pm->getObject<UAVObjectManager>();
Q_ASSERT(objMngr);
return objMngr;
}
}

74
ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.h Normal file
View File

@ -0,0 +1,74 @@
/**
******************************************************************************
*
* @file levelcalibrationmodel.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @addtogroup board level calibration
* @{
* @addtogroup ConfigPlugin Config Plugin
* @{
* @brief Telemetry configuration panel
*****************************************************************************/
/*
* 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 LEVELCALIBRATIONMODEL_H
#define LEVELCALIBRATIONMODEL_H
#include <QObject>
#include <QMutex>
#include <QList>
#include "calibration/calibrationutils.h"
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include <homelocation.h>
#include <accelstate.h>
#include <magstate.h>
namespace OpenPilot {
class LevelCalibrationModel : public QObject {
Q_OBJECT
public:
explicit LevelCalibrationModel(QObject *parent = 0);
signals:
void displayVisualHelp(QString elementID);
void displayInstructions(QString instructions, bool replace);
void disableAllCalibrations();
void enableAllCalibrations();
void savePositionEnabledChanged(bool state);
void progressChanged(int value);
public slots:
// Slots for calibrating the mags
void start();
void savePosition();
private slots:
void getSample(UAVObject *obj);
void compute();
private:
QMutex sensorsUpdateLock;
int position;
bool collectingData;
QList<double> rot_accum_roll;
QList<double> rot_accum_pitch;
double rot_data_roll;
double rot_data_pitch;
UAVObject::Metadata initialAttitudeStateMdata;
UAVObjectManager *getObjectManager();
};
}
#endif // LEVELCALIBRATIONMODEL_H

512
ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp Normal file
View File

@ -0,0 +1,512 @@
/**
******************************************************************************
*
* @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"
#include "calibration/calibrationuiutils.h"
#include "QDebug"
#define POINT_SAMPLE_SIZE 50
#define GRAVITY 9.81f
#define sign(x) ((x < 0) ? -1 : 1)
#define FITTING_USING_CONTINOUS_ACQUISITION
namespace OpenPilot {
SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
QObject(parent),
calibrationStepsMag(),
calibrationStepsAccelOnly(),
currentSteps(0),
position(-1),
calibratingMag(false),
calibratingAccel(false),
collectingData(false)
{
calibrationStepsMag.clear();
calibrationStepsMag
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_NED,
tr("Place horizontally, nose pointing north and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN,
tr("Place with nose down, right side west and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS,
tr("Place right side down, nose west and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU,
tr("Place upside down, nose east and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_USE,
tr("Place with nose up, left side north and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW,
tr("Place with left side down, nose south and click Save Position button..."));
calibrationStepsAccelOnly.clear();
calibrationStepsAccelOnly << CalibrationStep(CALIBRATION_HELPER_IMAGE_NED,
tr("Place horizontally and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN,
tr("Place with nose down and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS,
tr("Place right side down and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU,
tr("Place upside down and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_USE,
tr("Place with nose up and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW,
tr("Place with left side down and click Save Position button..."));
}
/********** 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
storeAndClearBoardRotation();
if (calibrateMag) {
currentSteps = &calibrationStepsMag;
} else {
currentSteps = &calibrationStepsAccelOnly;
}
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();
if (calibrateAccel) {
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();
if (calibrateMag) {
mdata = initialMagStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
mag->setMetadata(mdata);
}
/* Show instructions and enable controls */
displayInstructions((*currentSteps)[0].instructions, true);
showHelp((*currentSteps)[0].visualHelp);
disableAllCalibrations();
savePositionEnabledChanged(true);
position = 0;
mag_fit_x.clear();
mag_fit_y.clear();
mag_fit_z.clear();
}
/**
* 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);
if (calibratingMag) {
#ifdef FITTING_USING_CONTINOUS_ACQUISITION
// Mag samples are acquired during the whole calibration session, to be used for ellipsoid fit.
if (!position) {
connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
}
#endif // FITTING_USING_CONTINOUS_ACQUISITION
connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
}
if (calibratingAccel) {
connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
}
displayInstructions(tr("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);
#ifndef FITTING_USING_CONTINOUS_ACQUISITION
mag_fit_x.append(magData.x);
mag_fit_y.append(magData.y);
mag_fit_z.append(magData.z);
#endif // FITTING_USING_CONTINOUS_ACQUISITION
} else {
Q_ASSERT(0);
}
}
if ((!calibratingAccel || (accel_accum_x.size() >= POINT_SAMPLE_SIZE)) &&
(!calibratingMag || (mag_accum_x.size() >= POINT_SAMPLE_SIZE / 10)) &&
(collectingData == true)) {
collectingData = false;
savePositionEnabledChanged(true);
// Store the mean for this position for the accel
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
if (calibratingAccel) {
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);
if (calibratingMag) {
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 != 0) {
displayInstructions((*currentSteps)[position].instructions, false);
showHelp((*currentSteps)[position].visualHelp);
} else {
#ifdef FITTING_USING_CONTINOUS_ACQUISITION
if (calibratingMag) {
disconnect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
}
#endif // FITTING_USING_CONTINOUS_ACQUISITION
compute(calibratingMag, calibratingAccel);
savePositionEnabledChanged(false);
enableAllCalibrations();
showHelp(CALIBRATION_HELPER_IMAGE_EMPTY);
/* Cleanup original settings */
accelState->setMetadata(initialAccelStateMdata);
mag->setMetadata(initialMagStateMdata);
// Recall saved board rotation
recallBoardRotation();
}
}
}
void SixPointCalibrationModel::continouslyGetMagSamples(UAVObject *obj)
{
QMutexLocker lock(&sensorsUpdateLock);
if (obj->getObjID() == MagState::OBJID) {
MagState *mag = MagState::GetInstance(getObjectManager());
Q_ASSERT(mag);
MagState::DataFields magData = mag->getData();
mag_fit_x.append(magData.x);
mag_fit_y.append(magData.y);
mag_fit_z.append(magData.z);
}
}
/**
* 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));
int vectSize = mag_fit_x.count();
Eigen::VectorXf samples_x(vectSize);
Eigen::VectorXf samples_y(vectSize);
Eigen::VectorXf samples_z(vectSize);
for (int i = 0; i < vectSize; i++) {
samples_x(i) = mag_fit_x[i];
samples_y(i) = mag_fit_y[i];
samples_z(i) = mag_fit_z[i];
}
OpenPilot::CalibrationUtils::EllipsoidCalibrationResult result;
OpenPilot::CalibrationUtils::EllipsoidCalibration(&samples_x, &samples_y, &samples_z, Be_length, &result, true);
qDebug() << "-----------------------------------";
qDebug() << "Mag Calibration results: Fit";
qDebug() << "scale(" << result.Scale.coeff(0) << ", " << result.Scale.coeff(1) << ", " << result.Scale.coeff(2) << ")";
qDebug() << "bias(" << result.Bias.coeff(0) << ", " << result.Bias.coeff(1) << ", " << result.Bias.coeff(2) << ")";
OpenPilot::CalibrationUtils::SixPointInConstFieldCal(Be_length, mag_data_x, mag_data_y, mag_data_z, S, b);
qDebug() << "-----------------------------------";
qDebug() << "Mag Calibration results: Six Point";
qDebug() << "scale(" << S[0] << ", " << S[1] << ", " << S[2] << ")";
qDebug() << "bias(" << -sign(S[0]) * b[0] << ", " << -sign(S[1]) * b[1] << ", " << -sign(S[2]) * b[2] << ")";
qDebug() << "-----------------------------------";
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] = result.CalibrationMatrix.coeff(0, 0);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1] = result.CalibrationMatrix.coeff(0, 1);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C2] = result.CalibrationMatrix.coeff(0, 2);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C0] = result.CalibrationMatrix.coeff(1, 0);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] = result.CalibrationMatrix.coeff(1, 1);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C2] = result.CalibrationMatrix.coeff(1, 2);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C0] = result.CalibrationMatrix.coeff(2, 0);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C1] = result.CalibrationMatrix.coeff(2, 1);
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] = result.CalibrationMatrix.coeff(2, 2);
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] = result.Bias.coeff(0);
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = result.Bias.coeff(1);
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = result.Bias.coeff(2);
}
// Restore the previous setting
revoCalibrationData.MagBiasNullingRate = 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_R0C1] ==
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1];
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C2] ==
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C2];
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C0] ==
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C0];
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] ==
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1];
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C2] ==
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C2];
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C0] ==
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C0];
good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C1] ==
revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C1];
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(tr("Sensor scale and bias computed succesfully."), true);
} else {
displayInstructions(tr("Bad calibration. Please review the instructions and 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;
}
void SixPointCalibrationModel::showHelp(QString image)
{
if (image == CALIBRATION_HELPER_IMAGE_EMPTY) {
displayVisualHelp(image);
} else {
if (calibratingAccel) {
displayVisualHelp(CALIBRATION_HELPER_BOARD_PREFIX + image);
} else {
displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + image);
}
}
}
}

116
ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h Normal file
View File

@ -0,0 +1,116 @@
/**
******************************************************************************
*
* @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 <QString>
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include <homelocation.h>
#include <accelstate.h>
#include <magstate.h>
namespace OpenPilot {
class SixPointCalibrationModel : public QObject {
Q_OBJECT
class CalibrationStep {
public:
CalibrationStep(QString newVisualHelp, QString newInstructions)
{
visualHelp = newVisualHelp;
instructions = newInstructions;
}
QString visualHelp;
QString instructions;
};
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);
void continouslyGetMagSamples(UAVObject *obj);
private:
void start(bool calibrateAccel, bool calibrateMag);
UAVObjectManager *getObjectManager();
QList<CalibrationStep> calibrationStepsMag;
QList<CalibrationStep> calibrationStepsAccelOnly;
QList<CalibrationStep> *currentSteps;
UAVObject::Metadata initialAccelStateMdata;
UAVObject::Metadata initialMagStateMdata;
float initialMagCorrectionRate;
SavedSettings savedSettings;
int position;
bool calibratingMag;
bool calibratingAccel;
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;
QList<float> mag_fit_x;
QList<float> mag_fit_y;
QList<float> mag_fit_z;
void showHelp(QString image);
};
}
#endif // SIXPOINTCALIBRATIONMODEL_H

11
ground/openpilotgcs/src/plugins/config/config.pro
View File

@ -51,7 +51,11 @@ 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 \
calibration/levelcalibrationmodel.h \
calibration/gyrobiascalibrationmodel.h \
calibration/calibrationuiutils.h
SOURCES += configplugin.cpp \ SOURCES += configplugin.cpp \
configgadgetwidget.cpp \ configgadgetwidget.cpp \
@ -86,7 +90,10 @@ 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 \
calibration/levelcalibrationmodel.cpp \
calibration/gyrobiascalibrationmodel.cpp
FORMS += airframe.ui \ FORMS += airframe.ui \
airframe_ccpm.ui \ airframe_ccpm.ui \

14
ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp
View File

@ -38,7 +38,7 @@
#include "gyrostate.h" #include "gyrostate.h"
#include <extensionsystem/pluginmanager.h> #include <extensionsystem/pluginmanager.h>
#include <coreplugin/generalsettings.h> #include <coreplugin/generalsettings.h>
#include <calibration/calibrationutils.h>
ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) : ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) :
ConfigTaskWidget(parent), ConfigTaskWidget(parent),
ui(new Ui_ccattitude) ui(new Ui_ccattitude)
@ -114,13 +114,13 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject *obj)
disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *)));
disconnect(&timer, SIGNAL(timeout()), this, SLOT(timeout())); disconnect(&timer, SIGNAL(timeout()), this, SLOT(timeout()));
float x_bias = listMean(x_accum); float x_bias = OpenPilot::CalibrationUtils::listMean(x_accum);
float y_bias = listMean(y_accum); float y_bias = OpenPilot::CalibrationUtils::listMean(y_accum);
float z_bias = (listMean(z_accum) + 9.81); float z_bias = OpenPilot::CalibrationUtils::listMean(z_accum) + 9.81;
float x_gyro_bias = listMean(x_gyro_accum); float x_gyro_bias = OpenPilot::CalibrationUtils::listMean(x_gyro_accum);
float y_gyro_bias = listMean(y_gyro_accum); float y_gyro_bias = OpenPilot::CalibrationUtils::listMean(y_gyro_accum);
float z_gyro_bias = listMean(z_gyro_accum); float z_gyro_bias = OpenPilot::CalibrationUtils::listMean(z_gyro_accum);
accelState->setMetadata(initialAccelStateMdata); accelState->setMetadata(initialAccelStateMdata);
gyroState->setMetadata(initialGyroStateMdata); gyroState->setMetadata(initialGyroStateMdata);

16
ground/openpilotgcs/src/plugins/config/configgadget.qrc
View File

@ -2,7 +2,6 @@
<qresource prefix="/configgadget"> <qresource prefix="/configgadget">
<file>images/help2.png</file> <file>images/help2.png</file>
<file>images/ahrs-calib.svg</file> <file>images/ahrs-calib.svg</file>
<file>images/paper-plane.svg</file>
<file>images/multirotor-shapes.svg</file> <file>images/multirotor-shapes.svg</file>
<file>images/ccpm_setup.svg</file> <file>images/ccpm_setup.svg</file>
<file>images/PipXtreme.png</file> <file>images/PipXtreme.png</file>
@ -31,5 +30,20 @@
<file>images/pipx-normal.png</file> <file>images/pipx-normal.png</file>
<file>images/revolution_top.png</file> <file>images/revolution_top.png</file>
<file>calibration/WizardStepIndicator.qml</file> <file>calibration/WizardStepIndicator.qml</file>
<file>images/calibration/board-dwn.png</file>
<file>images/calibration/board-enu.png</file>
<file>images/calibration/plane-dwn.png</file>
<file>images/calibration/plane-enu.png</file>
<file>images/calibration/plane-ned.png</file>
<file>images/calibration/plane-suw.png</file>
<file>images/calibration/plane-use.png</file>
<file>images/calibration/plane-wds.png</file>
<file>images/calibration/board-ned.png</file>
<file>images/calibration/board-suw.png</file>
<file>images/calibration/board-use.png</file>
<file>images/calibration/board-wds.png</file>
<file>images/calibration/empty.png</file>
<file>images/calibration/plane-swd.png</file>
<file>images/calibration/board-swd.png</file>
</qresource> </qresource>
</RCC> </RCC>

1045
ground/openpilotgcs/src/plugins/config/configrevowidget.cpp
View File

@ -40,25 +40,23 @@
#include <iostream> #include <iostream>
#include <QDesktopServices> #include <QDesktopServices>
#include <QUrl> #include <QUrl>
#include <attitudestate.h>
#include <attitudesettings.h> #include <attitudesettings.h>
#include <ekfconfiguration.h>
#include <revocalibration.h> #include <revocalibration.h>
#include <accelgyrosettings.h> #include <accelgyrosettings.h>
#include <homelocation.h> #include <homelocation.h>
#include <accelstate.h> #include <accelstate.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
#define SIX_POINT_CAL_ACCEL #define NOISE_SAMPLES 50
const double ConfigRevoWidget::maxVarValue = 0.1;
// ***************** // *****************
@ -75,146 +73,18 @@ public:
ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
ConfigTaskWidget(parent), ConfigTaskWidget(parent),
m_ui(new Ui_RevoSensorsWidget()), m_ui(new Ui_RevoSensorsWidget()),
collectingData(false),
position(-1),
isBoardRotationStored(false) isBoardRotationStored(false)
{ {
m_ui->setupUi(this); m_ui->setupUi(this);
// Initialization of the Paper plane widget // Initialization of the Paper plane widget
m_ui->sixPointsHelp->setScene(new QGraphicsScene(this)); m_ui->calibrationVisualHelp->setScene(new QGraphicsScene(this));
m_ui->calibrationVisualHelp->setRenderHint(QPainter::HighQualityAntialiasing, true);
paperplane = new QGraphicsSvgItem(); m_ui->calibrationVisualHelp->setRenderHint(QPainter::SmoothPixmapTransform, true);
paperplane->setSharedRenderer(new QSvgRenderer()); displayVisualHelp("empty");
paperplane->renderer()->load(QString(":/configgadget/images/paper-plane.svg"));
paperplane->setElementId("plane-horizontal");
m_ui->sixPointsHelp->scene()->addItem(paperplane);
m_ui->sixPointsHelp->setSceneRect(paperplane->boundingRect());
// Initialization of the Revo sensor noise bargraph graph
m_ui->sensorsBargraph->setScene(new QGraphicsScene(this));
QSvgRenderer *renderer = new QSvgRenderer();
sensorsBargraph = new QGraphicsSvgItem();
renderer->load(QString(":/configgadget/images/ahrs-calib.svg"));
sensorsBargraph->setSharedRenderer(renderer);
sensorsBargraph->setElementId("background");
sensorsBargraph->setObjectName("background");
m_ui->sensorsBargraph->scene()->addItem(sensorsBargraph);
m_ui->sensorsBargraph->setSceneRect(sensorsBargraph->boundingRect());
// Initialize the 9 bargraph values:
QMatrix lineMatrix = renderer->matrixForElement("accel_x");
QRectF rect = lineMatrix.mapRect(renderer->boundsOnElement("accel_x"));
qreal startX = rect.x();
qreal startY = rect.y() + rect.height();
// maxBarHeight will be used for scaling it later.
maxBarHeight = rect.height();
// Then once we have the initial location, we can put it
// into a QGraphicsSvgItem which we will display at the same
// place: we do this so that the heading scale can be clipped to
// the compass dial region.
accel_x = new QGraphicsSvgItem();
accel_x->setSharedRenderer(renderer);
accel_x->setElementId("accel_x");
m_ui->sensorsBargraph->scene()->addItem(accel_x);
accel_x->setPos(startX, startY);
accel_x->setTransform(QTransform::fromScale(1, 0), true);
lineMatrix = renderer->matrixForElement("accel_y");
rect = lineMatrix.mapRect(renderer->boundsOnElement("accel_y"));
startX = rect.x();
startY = rect.y() + rect.height();
accel_y = new QGraphicsSvgItem();
accel_y->setSharedRenderer(renderer);
accel_y->setElementId("accel_y");
m_ui->sensorsBargraph->scene()->addItem(accel_y);
accel_y->setPos(startX, startY);
accel_y->setTransform(QTransform::fromScale(1, 0), true);
lineMatrix = renderer->matrixForElement("accel_z");
rect = lineMatrix.mapRect(renderer->boundsOnElement("accel_z"));
startX = rect.x();
startY = rect.y() + rect.height();
accel_z = new QGraphicsSvgItem();
accel_z->setSharedRenderer(renderer);
accel_z->setElementId("accel_z");
m_ui->sensorsBargraph->scene()->addItem(accel_z);
accel_z->setPos(startX, startY);
accel_z->setTransform(QTransform::fromScale(1, 0), true);
lineMatrix = renderer->matrixForElement("gyro_x");
rect = lineMatrix.mapRect(renderer->boundsOnElement("gyro_x"));
startX = rect.x();
startY = rect.y() + rect.height();
gyro_x = new QGraphicsSvgItem();
gyro_x->setSharedRenderer(renderer);
gyro_x->setElementId("gyro_x");
m_ui->sensorsBargraph->scene()->addItem(gyro_x);
gyro_x->setPos(startX, startY);
gyro_x->setTransform(QTransform::fromScale(1, 0), true);
lineMatrix = renderer->matrixForElement("gyro_y");
rect = lineMatrix.mapRect(renderer->boundsOnElement("gyro_y"));
startX = rect.x();
startY = rect.y() + rect.height();
gyro_y = new QGraphicsSvgItem();
gyro_y->setSharedRenderer(renderer);
gyro_y->setElementId("gyro_y");
m_ui->sensorsBargraph->scene()->addItem(gyro_y);
gyro_y->setPos(startX, startY);
gyro_y->setTransform(QTransform::fromScale(1, 0), true);
lineMatrix = renderer->matrixForElement("gyro_z");
rect = lineMatrix.mapRect(renderer->boundsOnElement("gyro_z"));
startX = rect.x();
startY = rect.y() + rect.height();
gyro_z = new QGraphicsSvgItem();
gyro_z->setSharedRenderer(renderer);
gyro_z->setElementId("gyro_z");
m_ui->sensorsBargraph->scene()->addItem(gyro_z);
gyro_z->setPos(startX, startY);
gyro_z->setTransform(QTransform::fromScale(1, 0), true);
lineMatrix = renderer->matrixForElement("mag_x");
rect = lineMatrix.mapRect(renderer->boundsOnElement("mag_x"));
startX = rect.x();
startY = rect.y() + rect.height();
mag_x = new QGraphicsSvgItem();
mag_x->setSharedRenderer(renderer);
mag_x->setElementId("mag_x");
m_ui->sensorsBargraph->scene()->addItem(mag_x);
mag_x->setPos(startX, startY);
mag_x->setTransform(QTransform::fromScale(1, 0), true);
lineMatrix = renderer->matrixForElement("mag_y");
rect = lineMatrix.mapRect(renderer->boundsOnElement("mag_y"));
startX = rect.x();
startY = rect.y() + rect.height();
mag_y = new QGraphicsSvgItem();
mag_y->setSharedRenderer(renderer);
mag_y->setElementId("mag_y");
m_ui->sensorsBargraph->scene()->addItem(mag_y);
mag_y->setPos(startX, startY);
mag_y->setTransform(QTransform::fromScale(1, 0), true);
lineMatrix = renderer->matrixForElement("mag_z");
rect = lineMatrix.mapRect(renderer->boundsOnElement("mag_z"));
startX = rect.x();
startY = rect.y() + rect.height();
mag_z = new QGraphicsSvgItem();
mag_z->setSharedRenderer(renderer);
mag_z->setElementId("mag_z");
m_ui->sensorsBargraph->scene()->addItem(mag_z);
mag_z->setPos(startX, startY);
mag_z->setTransform(QTransform::fromScale(1, 0), true);
// Must set up the UI (above) before setting up the UAVO mappings or refreshWidgetValues // Must set up the UI (above) before setting up the UAVO mappings or refreshWidgetValues
// will be dealing with some null pointers // will be dealing with some null pointers
addUAVObject("RevoCalibration"); addUAVObject("RevoCalibration");
addUAVObject("EKFConfiguration");
addUAVObject("HomeLocation"); addUAVObject("HomeLocation");
addUAVObject("AttitudeSettings"); addUAVObject("AttitudeSettings");
addUAVObject("RevoSettings"); addUAVObject("RevoSettings");
@ -238,11 +108,46 @@ 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)));
m_levelCalibrationModel = new OpenPilot::LevelCalibrationModel(this);
// level calibration
connect(m_ui->boardLevelStart, SIGNAL(clicked()), m_levelCalibrationModel, SLOT(start()));
connect(m_ui->boardLevelSavePos, SIGNAL(clicked()), m_levelCalibrationModel, SLOT(savePosition()));
connect(m_levelCalibrationModel, SIGNAL(disableAllCalibrations()), this, SLOT(disableAllCalibrations()));
connect(m_levelCalibrationModel, SIGNAL(enableAllCalibrations()), this, SLOT(enableAllCalibrations()));
connect(m_levelCalibrationModel, SIGNAL(displayInstructions(QString, bool)), this, SLOT(displayInstructions(QString, bool)));
connect(m_levelCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString)));
connect(m_levelCalibrationModel, SIGNAL(savePositionEnabledChanged(bool)), this->m_ui->boardLevelSavePos, SLOT(setEnabled(bool)));
connect(m_levelCalibrationModel, SIGNAL(progressChanged(int)), this->m_ui->boardLevelProgress, SLOT(setValue(int)));
// Connect the signals // Connect the signals
connect(m_ui->accelBiasStart, SIGNAL(clicked()), this, SLOT(doStartAccelGyroBiasCalibration())); // gyro zero calibration
connect(m_ui->sixPointsStart, SIGNAL(clicked()), this, SLOT(doStartSixPointCalibration())); m_gyroBiasCalibrationModel = new OpenPilot::GyroBiasCalibrationModel(this);
connect(m_ui->sixPointsSave, SIGNAL(clicked()), this, SLOT(savePositionData())); connect(m_ui->gyroBiasStart, SIGNAL(clicked()), m_gyroBiasCalibrationModel, SLOT(start()));
connect(m_ui->noiseMeasurementStart, SIGNAL(clicked()), this, SLOT(doStartNoiseMeasurement()));
connect(m_gyroBiasCalibrationModel, SIGNAL(progressChanged(int)), this->m_ui->gyroBiasProgress, SLOT(setValue(int)));
connect(m_gyroBiasCalibrationModel, SIGNAL(disableAllCalibrations()), this, SLOT(disableAllCalibrations()));
connect(m_gyroBiasCalibrationModel, SIGNAL(enableAllCalibrations()), this, SLOT(enableAllCalibrations()));
connect(m_gyroBiasCalibrationModel, SIGNAL(storeAndClearBoardRotation()), this, SLOT(storeAndClearBoardRotation()));
connect(m_gyroBiasCalibrationModel, SIGNAL(recallBoardRotation()), this, SLOT(recallBoardRotation()));
connect(m_gyroBiasCalibrationModel, SIGNAL(displayInstructions(QString, bool)), this, SLOT(displayInstructions(QString, bool)));
connect(m_gyroBiasCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString)));
connect(m_ui->hlClearButton, SIGNAL(clicked()), this, SLOT(clearHomeLocation())); connect(m_ui->hlClearButton, SIGNAL(clicked()), this, SLOT(clearHomeLocation()));
@ -256,6 +161,7 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
populateWidgets(); populateWidgets();
refreshWidgetsValues(); refreshWidgetsValues();
m_ui->tabWidget->setCurrentIndex(0); m_ui->tabWidget->setCurrentIndex(0);
enableAllCalibrations();
} }
ConfigRevoWidget::~ConfigRevoWidget() ConfigRevoWidget::~ConfigRevoWidget()
@ -266,614 +172,17 @@ ConfigRevoWidget::~ConfigRevoWidget()
void ConfigRevoWidget::showEvent(QShowEvent *event) void ConfigRevoWidget::showEvent(QShowEvent *event)
{ {
Q_UNUSED(event) Q_UNUSED(event);
// Thit fitInView method should only be called now, once the m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio);
// widget is shown, otherwise it cannot compute its values and
// the result is usually a sensorsBargraph that is way too small.
m_ui->sensorsBargraph->fitInView(sensorsBargraph, Qt::KeepAspectRatio);
m_ui->sixPointsHelp->fitInView(paperplane, Qt::KeepAspectRatio);
m_thermalCalibrationModel->init(); m_thermalCalibrationModel->init();
} }
void ConfigRevoWidget::resizeEvent(QResizeEvent *event) void ConfigRevoWidget::resizeEvent(QResizeEvent *event)
{ {
Q_UNUSED(event) Q_UNUSED(event);
m_ui->sensorsBargraph->fitInView(sensorsBargraph, Qt::KeepAspectRatio); m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio);
m_ui->sixPointsHelp->fitInView(paperplane, Qt::KeepAspectRatio);
} }
/**
* Starts an accelerometer bias calibration.
*/
void ConfigRevoWidget::doStartAccelGyroBiasCalibration()
{
// Store and reset board rotation before calibration starts
isBoardRotationStored = false;
storeAndClearBoardRotation();
m_ui->accelBiasStart->setEnabled(false);
m_ui->accelBiasProgress->setValue(0);
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE;
revoCalibration->setData(revoCalibrationData);
revoCalibration->updated();
// Disable gyro bias correction while calibrating
AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
Q_ASSERT(attitudeSettings);
AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
accel_accum_x.clear();
accel_accum_y.clear();
accel_accum_z.clear();
gyro_accum_x.clear();
gyro_accum_y.clear();
gyro_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);
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
initialGyroStateMdata = gyroState->getMetadata();
mdata = initialGyroStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
gyroState->setMetadata(mdata);
// Now connect to the accels and mag updates, gather for 100 samples
collectingData = true;
connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
}
/**
Updates the accel bias raw values
*/
void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj)
{
QMutexLocker lock(&sensorsUpdateLock);
Q_UNUSED(lock);
switch (obj->getObjID()) {
case 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);
break;
}
case GyroState::OBJID:
{
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
GyroState::DataFields gyroStateData = gyroState->getData();
gyro_accum_x.append(gyroStateData.x);
gyro_accum_y.append(gyroStateData.y);
gyro_accum_z.append(gyroStateData.z);
break;
}
default:
Q_ASSERT(0);
}
// Work out the progress based on whichever has less
double p1 = (double)accel_accum_x.size() / (double)NOISE_SAMPLES;
double p2 = (double)accel_accum_y.size() / (double)NOISE_SAMPLES;
m_ui->accelBiasProgress->setValue(((p1 < p2) ? p1 : p2) * 100);
if (accel_accum_x.size() >= NOISE_SAMPLES &&
gyro_accum_y.size() >= NOISE_SAMPLES &&
collectingData == true) {
collectingData = false;
AccelState *accelState = AccelState::GetInstance(getObjectManager());
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
m_ui->accelBiasStart->setEnabled(true);
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
Q_ASSERT(accelGyroSettings);
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE;
// Update the biases based on collected data
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] += listMean(accel_accum_x);
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] += listMean(accel_accum_y);
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] += (listMean(accel_accum_z) + GRAVITY);
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);
revoCalibration->setData(revoCalibrationData);
revoCalibration->updated();
accelGyroSettings->setData(accelGyroSettingsData);
accelGyroSettings->updated();
AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
Q_ASSERT(attitudeSettings);
AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE;
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
accelState->setMetadata(initialAccelStateMdata);
gyroState->setMetadata(initialGyroStateMdata);
// Recall saved board rotation
recallBoardRotation();
}
}
int LinearEquationsSolving(int nDim, double *pfMatr, double *pfVect, double *pfSolution)
{
double fMaxElem;
double fAcc;
int i, j, k, m;
for (k = 0; k < (nDim - 1); k++) { // base row of matrix
// search of line with max element
fMaxElem = fabs(pfMatr[k * nDim + k]);
m = k;
for (i = k + 1; i < nDim; i++) {
if (fMaxElem < fabs(pfMatr[i * nDim + k])) {
fMaxElem = pfMatr[i * nDim + k];
m = i;
}
}
// permutation of base line (index k) and max element line(index m)
if (m != k) {
for (i = k; i < nDim; i++) {
fAcc = pfMatr[k * nDim + i];
pfMatr[k * nDim + i] = pfMatr[m * nDim + i];
pfMatr[m * nDim + i] = fAcc;
}
fAcc = pfVect[k];
pfVect[k] = pfVect[m];
pfVect[m] = fAcc;
}
if (pfMatr[k * nDim + k] == 0.) {
return 0; // needs improvement !!!
}
// triangulation of matrix with coefficients
for (j = (k + 1); j < nDim; j++) { // current row of matrix
fAcc = -pfMatr[j * nDim + k] / pfMatr[k * nDim + k];
for (i = k; i < nDim; i++) {
pfMatr[j * nDim + i] = pfMatr[j * nDim + i] + fAcc * pfMatr[k * nDim + i];
}
pfVect[j] = pfVect[j] + fAcc * pfVect[k]; // free member recalculation
}
}
for (k = (nDim - 1); k >= 0; k--) {
pfSolution[k] = pfVect[k];
for (i = (k + 1); i < nDim; i++) {
pfSolution[k] -= (pfMatr[k * nDim + i] * pfSolution[i]);
}
pfSolution[k] = pfSolution[k] / pfMatr[k * nDim + k];
}
return 1;
}
int SixPointInConstFieldCal(double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3])
{
int i;
double A[5][5];
double f[5], c[5];
double xp, yp, zp, Sx;
// Fill in matrix A -
// write six difference-in-magnitude equations of the form
// Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0
// or in other words
// 2*Sx*bx*(x2-x1)/Sx^2 + Sy^2(y2^2-y1^2)/Sx^2 + 2*Sy*by*(y2-y1)/Sx^2 + Sz^2(z2^2-z1^2)/Sx^2 + 2*Sz*bz*(z2-z1)/Sx^2 = (x1^2-x2^2)
for (i = 0; i < 5; i++) {
A[i][0] = 2.0 * (x[i + 1] - x[i]);
A[i][1] = y[i + 1] * y[i + 1] - y[i] * y[i];
A[i][2] = 2.0 * (y[i + 1] - y[i]);
A[i][3] = z[i + 1] * z[i + 1] - z[i] * z[i];
A[i][4] = 2.0 * (z[i + 1] - z[i]);
f[i] = x[i] * x[i] - x[i + 1] * x[i + 1];
}
// solve for c0=bx/Sx, c1=Sy^2/Sx^2; c2=Sy*by/Sx^2, c3=Sz^2/Sx^2, c4=Sz*bz/Sx^2
if (!LinearEquationsSolving(5, (double *)A, f, c)) {
return 0;
}
// use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer
xp = x[0]; yp = y[0]; zp = z[0];
Sx = sqrt(ConstMag * ConstMag / (xp * xp + 2 * c[0] * xp + c[0] * c[0] + c[1] * yp * yp + 2 * c[2] * yp + c[2] * c[2] / c[1] + c[3] * zp * zp + 2 * c[4] * zp + c[4] * c[4] / c[3]));
S[0] = Sx;
b[0] = Sx * c[0];
S[1] = sqrt(c[1] * Sx * Sx);
b[1] = c[2] * Sx * Sx / S[1];
S[2] = sqrt(c[3] * Sx * Sx);
b[2] = c[4] * Sx * Sx / S[2];
return 1;
}
/********** Functions for six point calibration **************/
/**
* 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::doStartSixPointCalibration()
{
// 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();
HomeLocation::DataFields homeLocationData = homeLocation->getData();
AccelGyroSettings::DataFields accelGyroSettingsData = 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;
}
#ifdef SIX_POINT_CAL_ACCEL
// 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();
#endif
// Calibration mag
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] = 1;
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] = 1;
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] = 1;
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] = 0;
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = 0;
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = 0;
// 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;
#ifdef SIX_POINT_CAL_ACCEL
/* 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);
#endif
/* 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 */
m_ui->sixPointCalibInstructions->clear();
m_ui->sixPointCalibInstructions->append("Place horizontally and click save position...");
displayPlane("plane-horizontal");
m_ui->sixPointsStart->setEnabled(false);
m_ui->sixPointsSave->setEnabled(true);
position = 0;
}
/**
* 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::savePositionData()
{
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(doGetSixPointCalibrationMeasurement(UAVObject *)));
connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *)));
m_ui->sixPointCalibInstructions->append("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::doGetSixPointCalibrationMeasurement(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) {
#ifdef SIX_POINT_CAL_ACCEL
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);
#endif
} 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);
}
}
#ifdef SIX_POINT_CAL_ACCEL
if (accel_accum_x.size() >= 20 && mag_accum_x.size() >= 20 && collectingData == true) {
#else
if (mag_accum_x.size() >= 20 && collectingData == true) {
#endif
collectingData = false;
m_ui->sixPointsSave->setEnabled(true);
#ifdef SIX_POINT_CAL_ACCEL
// Store the mean for this position for the accel
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(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);
#endif
// Store the mean for this position for the mag
MagState *mag = MagState::GetInstance(getObjectManager());
Q_ASSERT(mag);
disconnect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(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) {
m_ui->sixPointCalibInstructions->append("Place with left side down and click save position...");
displayPlane("plane-left");
}
if (position == 2) {
m_ui->sixPointCalibInstructions->append("Place upside down and click save position...");
displayPlane("plane-flip");
}
if (position == 3) {
m_ui->sixPointCalibInstructions->append("Place with right side down and click save position...");
displayPlane("plane-right");
}
if (position == 4) {
m_ui->sixPointCalibInstructions->append("Place with nose up and click save position...");
displayPlane("plane-up");
}
if (position == 5) {
m_ui->sixPointCalibInstructions->append("Place with nose down and click save position...");
displayPlane("plane-down");
}
if (position == 0) {
computeScaleBias();
m_ui->sixPointsStart->setEnabled(true);
m_ui->sixPointsSave->setEnabled(false);
/* Cleanup original settings */
#ifdef SIX_POINT_CAL_ACCEL
accelState->setMetadata(initialAccelStateMdata);
#endif
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::computeScaleBias()
{
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();
#ifdef SIX_POINT_CAL_ACCEL
// Calibration accel
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];
#endif
// Calibration mag
Be_length = sqrt(pow(homeLocationData.Be[0], 2) + pow(homeLocationData.Be[1], 2) + pow(homeLocationData.Be[2], 2));
SixPointInConstFieldCal(Be_length, mag_data_x, mag_data_y, mag_data_z, S, b);
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] = fabs(S[0]);
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] = fabs(S[1]);
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] = fabs(S[2]);
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;
#ifdef SIX_POINT_CAL_ACCEL
bool good_calibration = true;
// Check the mag calibration is good
good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] ==
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X];
good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] ==
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y];
good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] ==
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z];
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
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) {
revoCalibration->setData(revoCalibrationData);
accelGyroSettings->setData(accelGyroSettingsData);
m_ui->sixPointCalibInstructions->append("Computed accel and mag scale and bias...");
} else {
revoCalibrationData = revoCalibration->getData();
accelGyroSettingsData = accelGyroSettings->getData();
m_ui->sixPointCalibInstructions->append("Bad calibration. Please repeat.");
}
#else // ifdef SIX_POINT_CAL_ACCEL
bool good_calibration = true;
good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] ==
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X];
good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] ==
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y];
good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] ==
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z];
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];
if (good_calibration) {
revoCalibration->setData(revoCalibrationData);
accelGyroSettings->setData(accelGyroSettingsData);
m_ui->sixPointCalibInstructions->append("Computed mag scale and bias...");
} else {
revoCalibrationData = revoCalibration->getData();
accelGyroSettingsData = accelGyroSettings->getData();
m_ui->sixPointCalibInstructions->append("Bad calibration. Please repeat.");
}
#endif // ifdef SIX_POINT_CAL_ACCEL
position = -1; // set to run again
}
void ConfigRevoWidget::storeAndClearBoardRotation() void ConfigRevoWidget::storeAndClearBoardRotation()
{ {
@ -912,232 +221,28 @@ void ConfigRevoWidget::recallBoardRotation()
} }
/** /**
Rotate the paper plane Show the selected visual aid
*/ */
void ConfigRevoWidget::displayPlane(QString elementID) void ConfigRevoWidget::displayVisualHelp(QString elementID)
{ {
paperplane->setElementId(elementID); m_ui->calibrationVisualHelp->scene()->clear();
m_ui->sixPointsHelp->setSceneRect(paperplane->boundingRect()); QPixmap pixmap = QPixmap(":/configgadget/images/calibration/" + elementID + ".png");
m_ui->sixPointsHelp->fitInView(paperplane, Qt::KeepAspectRatio); m_ui->calibrationVisualHelp->scene()->addPixmap(pixmap);
m_ui->calibrationVisualHelp->setSceneRect(pixmap.rect());
m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio);
} }
/*********** Noise measurement functions **************/ void ConfigRevoWidget::displayInstructions(QString instructions, bool replace)
/**
* Connect sensor updates and timeout for measuring the noise
*/
void ConfigRevoWidget::doStartNoiseMeasurement()
{ {
QMutexLocker lock(&sensorsUpdateLock); if (replace || instructions.isNull()) {
m_ui->calibrationInstructions->clear();
// Store and reset board rotation before calibration starts
isBoardRotationStored = false;
storeAndClearBoardRotation();
Q_UNUSED(lock);
HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(homeLocation);
HomeLocation::DataFields homeLocationData = homeLocation->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;
} }
if (!instructions.isNull()) {
accel_accum_x.clear(); m_ui->calibrationInstructions->append(instructions);
accel_accum_y.clear();
accel_accum_z.clear();
gyro_accum_x.clear();
gyro_accum_y.clear();
gyro_accum_z.clear();
mag_accum_x.clear();
mag_accum_y.clear();
mag_accum_z.clear();
/* Need to get as many accel, mag and gyro updates as possible */
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
MagState *mag = MagState::GetInstance(getObjectManager());
Q_ASSERT(mag);
UAVObject::Metadata mdata;
initialAccelStateMdata = accelState->getMetadata();
mdata = initialAccelStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
accelState->setMetadata(mdata);
initialGyroStateMdata = gyroState->getMetadata();
mdata = initialGyroStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
gyroState->setMetadata(mdata);
initialMagStateMdata = mag->getMetadata();
mdata = initialMagStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
mag->setMetadata(mdata);
/* Connect for updates */
connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
}
/**
* Called when any of the sensors are updated. Stores the sample for measuring the
* variance at the end
*/
void ConfigRevoWidget::doGetNoiseSample(UAVObject *obj)
{
QMutexLocker lock(&sensorsUpdateLock);
Q_UNUSED(lock);
Q_ASSERT(obj);
switch (obj->getObjID()) {
case GyroState::OBJID:
{
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
GyroState::DataFields gyroData = gyroState->getData();
gyro_accum_x.append(gyroData.x);
gyro_accum_y.append(gyroData.y);
gyro_accum_z.append(gyroData.z);
break;
}
case 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);
break;
}
case MagState::OBJID:
{
MagState *mags = MagState::GetInstance(getObjectManager());
Q_ASSERT(mags);
MagState::DataFields magData = mags->getData();
mag_accum_x.append(magData.x);
mag_accum_y.append(magData.y);
mag_accum_z.append(magData.z);
break;
}
default:
Q_ASSERT(0);
}
float p1 = (float)mag_accum_x.length() / (float)NOISE_SAMPLES;
float p2 = (float)gyro_accum_x.length() / (float)NOISE_SAMPLES;
float p3 = (float)accel_accum_x.length() / (float)NOISE_SAMPLES;
float prog = (p1 < p2) ? p1 : p2;
prog = (prog < p3) ? prog : p3;
m_ui->noiseMeasurementProgress->setValue(prog * 100);
if (mag_accum_x.length() >= NOISE_SAMPLES &&
gyro_accum_x.length() >= NOISE_SAMPLES &&
accel_accum_x.length() >= NOISE_SAMPLES) {
// No need to for more updates
MagState *mags = MagState::GetInstance(getObjectManager());
AccelState *accelState = AccelState::GetInstance(getObjectManager());
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
disconnect(mags, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
EKFConfiguration *ekfConfiguration = EKFConfiguration::GetInstance(getObjectManager());
Q_ASSERT(ekfConfiguration);
if (ekfConfiguration) {
EKFConfiguration::DataFields revoCalData = ekfConfiguration->getData();
revoCalData.Q[EKFConfiguration::Q_ACCELX] = listVar(accel_accum_x);
revoCalData.Q[EKFConfiguration::Q_ACCELY] = listVar(accel_accum_y);
revoCalData.Q[EKFConfiguration::Q_ACCELZ] = listVar(accel_accum_z);
revoCalData.Q[EKFConfiguration::Q_GYROX] = listVar(gyro_accum_x);
revoCalData.Q[EKFConfiguration::Q_GYROY] = listVar(gyro_accum_y);
revoCalData.Q[EKFConfiguration::Q_GYROZ] = listVar(gyro_accum_z);
revoCalData.R[EKFConfiguration::R_MAGX] = listVar(mag_accum_x);
revoCalData.R[EKFConfiguration::R_MAGY] = listVar(mag_accum_y);
revoCalData.R[EKFConfiguration::R_MAGZ] = listVar(mag_accum_z);
ekfConfiguration->setData(revoCalData);
}
// Recall saved board rotation
recallBoardRotation();
} }
} }
/********** UI Functions *************/ /********** UI Functions *************/
/**
Draws the sensor variances bargraph
*/
void ConfigRevoWidget::drawVariancesGraph()
{
EKFConfiguration *ekfConfiguration = EKFConfiguration::GetInstance(getObjectManager());
Q_ASSERT(ekfConfiguration);
if (!ekfConfiguration) {
return;
}
EKFConfiguration::DataFields ekfConfigurationData = ekfConfiguration->getData();
// The expected range is from 1E-6 to 1E-1
double steps = 6; // 6 bars on the graph
float accel_x_var = -1 / steps * (1 + steps + log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELX]));
if (accel_x) {
accel_x->setTransform(QTransform::fromScale(1, accel_x_var), false);
}
float accel_y_var = -1 / steps * (1 + steps + log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELY]));
if (accel_y) {
accel_y->setTransform(QTransform::fromScale(1, accel_y_var), false);
}
float accel_z_var = -1 / steps * (1 + steps + log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELZ]));
if (accel_z) {
accel_z->setTransform(QTransform::fromScale(1, accel_z_var), false);
}
float gyro_x_var = -1 / steps * (1 + steps + log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROX]));
if (gyro_x) {
gyro_x->setTransform(QTransform::fromScale(1, gyro_x_var), false);
}
float gyro_y_var = -1 / steps * (1 + steps + log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROY]));
if (gyro_y) {
gyro_y->setTransform(QTransform::fromScale(1, gyro_y_var), false);
}
float gyro_z_var = -1 / steps * (1 + steps + log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROZ]));
if (gyro_z) {
gyro_z->setTransform(QTransform::fromScale(1, gyro_z_var), false);
}
// Scale by 1e-3 because mag vars are much higher.
float mag_x_var = -1 / steps * (1 + steps + log10(1e-3 * ekfConfigurationData.R[EKFConfiguration::R_MAGX]));
if (mag_x) {
mag_x->setTransform(QTransform::fromScale(1, mag_x_var), false);
}
float mag_y_var = -1 / steps * (1 + steps + log10(1e-3 * ekfConfigurationData.R[EKFConfiguration::R_MAGY]));
if (mag_y) {
mag_y->setTransform(QTransform::fromScale(1, mag_y_var), false);
}
float mag_z_var = -1 / steps * (1 + steps + log10(1e-3 * ekfConfigurationData.R[EKFConfiguration::R_MAGZ]));
if (mag_z) {
mag_z->setTransform(QTransform::fromScale(1, mag_z_var), false);
}
}
/** /**
* Called by the ConfigTaskWidget parent when RevoCalibration is updated * Called by the ConfigTaskWidget parent when RevoCalibration is updated
@ -1147,11 +252,7 @@ void ConfigRevoWidget::refreshWidgetsValues(UAVObject *object)
{ {
ConfigTaskWidget::refreshWidgetsValues(object); ConfigTaskWidget::refreshWidgetsValues(object);
drawVariancesGraph();
m_ui->noiseMeasurementStart->setEnabled(true);
m_ui->sixPointsStart->setEnabled(true);
m_ui->accelBiasStart->setEnabled(true);
m_ui->calibInstructions->setText(QString("Press \"Start\" above to calibrate.")); m_ui->calibInstructions->setText(QString("Press \"Start\" above to calibrate."));
m_ui->isSetCheckBox->setEnabled(false); m_ui->isSetCheckBox->setEnabled(false);
@ -1180,3 +281,21 @@ void ConfigRevoWidget::clearHomeLocation()
homeLocationData.Set = HomeLocation::SET_FALSE; homeLocationData.Set = HomeLocation::SET_FALSE;
homeLocation->setData(homeLocationData); homeLocation->setData(homeLocationData);
} }
void ConfigRevoWidget::disableAllCalibrations()
{
m_ui->sixPointsStartAccel->setEnabled(false);
m_ui->sixPointsStartMag->setEnabled(false);
m_ui->boardLevelStart->setEnabled(false);
m_ui->gyroBiasStart->setEnabled(false);
m_ui->ThermalBiasStart->setEnabled(false);
}
void ConfigRevoWidget::enableAllCalibrations()
{
m_ui->sixPointsStartAccel->setEnabled(true);
m_ui->sixPointsStartMag->setEnabled(true);
m_ui->boardLevelStart->setEnabled(true);
m_ui->gyroBiasStart->setEnabled(true);
m_ui->ThermalBiasStart->setEnabled(true);
}

80
ground/openpilotgcs/src/plugins/config/configrevowidget.h
View File

@ -29,7 +29,6 @@
#include "ui_revosensors.h" #include "ui_revosensors.h"
#include "configtaskwidget.h" #include "configtaskwidget.h"
#include "../uavobjectwidgetutils/configtaskwidget.h" #include "../uavobjectwidgetutils/configtaskwidget.h"
#include "extensionsystem/pluginmanager.h" #include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h" #include "uavobjectmanager.h"
@ -41,8 +40,12 @@
#include <QTimer> #include <QTimer>
#include <QMutex> #include <QMutex>
#include "calibration/thermal/thermalcalibrationmodel.h" #include "calibration/thermal/thermalcalibrationmodel.h"
#include "calibration/sixpointcalibrationmodel.h"
#include "calibration/levelcalibrationmodel.h"
#include "calibration/gyrobiascalibrationmodel.h"
class Ui_Widget; class Ui_Widget;
class ConfigRevoWidget : public ConfigTaskWidget { class ConfigRevoWidget : public ConfigTaskWidget {
Q_OBJECT Q_OBJECT
@ -51,83 +54,32 @@ public:
~ConfigRevoWidget(); ~ConfigRevoWidget();
private: private:
void drawVariancesGraph(); OpenPilot::SixPointCalibrationModel *m_sixPointCalibrationModel;
void displayPlane(QString elementID);
// ! Computes the scale and bias of the mag based on collected data
void computeScaleBias();
OpenPilot::ThermalCalibrationModel *m_thermalCalibrationModel; OpenPilot::ThermalCalibrationModel *m_thermalCalibrationModel;
OpenPilot::LevelCalibrationModel *m_levelCalibrationModel;
OpenPilot::GyroBiasCalibrationModel *m_gyroBiasCalibrationModel;
Ui_RevoSensorsWidget *m_ui; Ui_RevoSensorsWidget *m_ui;
QGraphicsSvgItem *paperplane;
QGraphicsSvgItem *sensorsBargraph;
QGraphicsSvgItem *accel_x;
QGraphicsSvgItem *accel_y;
QGraphicsSvgItem *accel_z;
QGraphicsSvgItem *gyro_x;
QGraphicsSvgItem *gyro_y;
QGraphicsSvgItem *gyro_z;
QGraphicsSvgItem *mag_x;
QGraphicsSvgItem *mag_y;
QGraphicsSvgItem *mag_z;
QMutex sensorsUpdateLock;
double maxBarHeight;
int phaseCounter;
const static double maxVarValue;
const static int calibrationDelay = 10;
bool collectingData;
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;
double accel_data_x[6], accel_data_y[6], accel_data_z[6];
double mag_data_x[6], mag_data_y[6], mag_data_z[6];
UAVObject::Metadata initialAccelStateMdata;
UAVObject::Metadata initialGyroStateMdata;
UAVObject::Metadata initialMagStateMdata;
UAVObject::Metadata initialBaroSensorMdata;
float initialMagCorrectionRate;
int position;
static const int NOISE_SAMPLES = 100;
// Board rotation store/recall // Board rotation store/recall
qint16 storedBoardRotation[3]; qint16 storedBoardRotation[3];
bool isBoardRotationStored; bool isBoardRotationStored;
void storeAndClearBoardRotation();
void recallBoardRotation();
private slots: private slots:
void displayVisualHelp(QString elementID);
void storeAndClearBoardRotation();
void recallBoardRotation();
void displayInstructions(QString instructions = QString(), bool replace = false);
// ! 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 doStartSixPointCalibration();
void doGetSixPointCalibrationMeasurement(UAVObject *obj);
void savePositionData();
// Slots for calibrating the accel and gyro
void doStartAccelGyroBiasCalibration();
void doGetAccelGyroBiasData(UAVObject *);
// Slots for measuring the sensor noise
void doStartNoiseMeasurement();
void doGetNoiseSample(UAVObject *);
// Slot for clearing home location // Slot for clearing home location
void clearHomeLocation(); void clearHomeLocation();
void disableAllCalibrations();
void enableAllCalibrations();
protected: protected:
void showEvent(QShowEvent *event); void showEvent(QShowEvent *event);
void resizeEvent(QResizeEvent *event); void resizeEvent(QResizeEvent *event);

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/board-dwn.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 150 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/board-enu.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 86 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/board-ned.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 75 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/board-suw.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 57 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/board-swd.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 117 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/board-use.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 56 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/board-wds.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 176 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/empty.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 57 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/plane-dwn.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 115 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/plane-enu.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 75 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/plane-horizontal-rotated.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 80 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/plane-ned.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 70 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/plane-suw.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 61 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/plane-swd.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 79 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/plane-use.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 67 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/plane-wds.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 113 KiB

BIN
ground/openpilotgcs/src/plugins/config/images/calibration/planes.xcf Normal file
View File

Binary file not shown.

309
ground/openpilotgcs/src/plugins/config/images/paper-plane.svg
View File

@ -1,309 +0,0 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<!-- Created with Inkscape (http://www.inkscape.org/) -->
<svg
xmlns:dc="http://purl.org/dc/elements/1.1/"
xmlns:cc="http://creativecommons.org/ns#"
xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
xmlns:svg="http://www.w3.org/2000/svg"
xmlns="http://www.w3.org/2000/svg"
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
width="744.09448819"
height="1052.3622047"
id="svg2"
version="1.1"
inkscape:version="0.47 r22583"
sodipodi:docname="paper-plane.svg">
<title
id="title2859">Paper planes</title>
<defs
id="defs4">
<inkscape:path-effect
effect="envelope"
id="path-effect3160"
is_visible="true"
yy="true"
xx="true"
bendpath1="m 447.14285,362.36218 55.71429,0"
bendpath2="m 502.85714,362.36218 0,54.28572"
bendpath3="m 447.14285,416.6479 55.71429,0"
bendpath4="m 447.14285,362.36218 0,54.28572" />
<inkscape:perspective
sodipodi:type="inkscape:persp3d"
inkscape:vp_x="0 : 526.18109 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_z="744.09448 : 526.18109 : 1"
inkscape:persp3d-origin="372.04724 : 350.78739 : 1"
id="perspective10" />
<inkscape:perspective
id="perspective3143"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
sodipodi:type="inkscape:persp3d"
inkscape:vp_x="0 : 618.71844 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_z="1486.9843 : 618.71844 : 1"
inkscape:persp3d-origin="743.49213 : 412.47896 : 1"
id="perspective2567" />
</defs>
<sodipodi:namedview
id="base"
pagecolor="#ffffff"
bordercolor="#666666"
borderopacity="1.0"
inkscape:pageopacity="0.0"
inkscape:pageshadow="2"
inkscape:zoom="0.60408995"
inkscape:cx="160.9057"
inkscape:cy="659.88675"
inkscape:document-units="px"
inkscape:current-layer="layer1"
showgrid="false"
showguides="true"
inkscape:guide-bbox="true"
inkscape:window-width="1366"
inkscape:window-height="693"
inkscape:window-x="0"
inkscape:window-y="24"
inkscape:window-maximized="1" />
<metadata
id="metadata7">
<rdf:RDF>
<cc:Work
rdf:about="">
<dc:format>image/svg+xml</dc:format>
<dc:type
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
<dc:title>Paper planes</dc:title>
<dc:creator>
<cc:Agent>
<dc:title>Edouard Lafargue</dc:title>
</cc:Agent>
</dc:creator>
<cc:license
rdf:resource="http://creativecommons.org/licenses/by-sa/3.0/" />
<dc:date>2010.08.29</dc:date>
</cc:Work>
<cc:License
rdf:about="http://creativecommons.org/licenses/by-sa/3.0/">
<cc:permits
rdf:resource="http://creativecommons.org/ns#Reproduction" />
<cc:permits
rdf:resource="http://creativecommons.org/ns#Distribution" />
<cc:requires
rdf:resource="http://creativecommons.org/ns#Notice" />
<cc:requires
rdf:resource="http://creativecommons.org/ns#Attribution" />
<cc:permits
rdf:resource="http://creativecommons.org/ns#DerivativeWorks" />
<cc:requires
rdf:resource="http://creativecommons.org/ns#ShareAlike" />
</cc:License>
</rdf:RDF>
</metadata>
<g
inkscape:label="Layer 1"
inkscape:groupmode="layer"
id="layer1">
<g
id="plane-flip"
inkscape:label="#g4365">
<g
transform="translate(305.77675,-285.13719)"
id="g3972">
<path
sodipodi:nodetypes="ccccccc"
id="path3974"
d="m 185.71429,456.6479 110,0 10,-30 8.57143,30 111.42857,0 L 382.85714,338.07647 185.71429,456.6479 z"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
<path
id="path3978"
d="m 314.66252,456.62472 68.1853,-118.18785"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
<path
transform="translate(70,-226)"
id="path3982"
d="m 235.87062,652.34177 76.77159,-88.13581"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
</g>
</g>
<g
id="plane-horizontal"
inkscape:label="#g4349">
<g
inkscape:label="#g3946"
transform="translate(-158.56854,-296.98485)"
id="bla">
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 185.71429,456.6479 110,0 10,27.85714 8.57143,-27.85714 111.42857,0 L 382.85714,338.07647 185.71429,456.6479 z"
id="path2822"
sodipodi:nodetypes="ccccccc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 296.42857,456.6479 383.21429,338.43361"
id="path2826" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 314.66252,456.62472 68.1853,-118.18785"
id="path2828" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 305.57115,485.16153 15.65736,-28.28427"
id="path2830" />
</g>
<path
id="path4065"
d="m 146.38783,182.85785 c -0.32605,-0.90165 -2.2117,-6.11664 -4.19034,-11.58888 -1.97865,-5.47224 -3.49043,-10.36632 -3.35952,-10.87573 0.28104,-1.0936 72.47583,-99.552799 72.77633,-99.252297 0.19834,0.198344 -52.99468,92.816837 -55.68619,96.959537 -0.73197,1.12664 -2.97014,7.44751 -4.97371,14.04639 -2.00356,6.59887 -3.7173,12.07724 -3.8083,12.17415 -0.091,0.0969 -0.43223,-0.56152 -0.75827,-1.46317 l 0,0 z"
style="opacity:1;fill:#666666;fill-opacity:1" />
</g>
<g
id="plane-left"
inkscape:label="#g4357">
<g
transform="matrix(0.00256181,-0.99999672,-0.99999672,-0.00256181,780.50589,449.7941)"
id="g3994">
<path
sodipodi:nodetypes="ccccccc"
id="path3996"
d="m 185.71429,456.6479 110,0 10.14561,-28.97991 8.42582,28.97991 111.42857,0 L 382.85714,338.07647 185.71429,456.6479 z"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
<path
id="path4000"
d="m 314.66252,456.62472 68.1853,-118.18785"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
<path
sodipodi:nodetypes="cc"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 295.91153,456.54311 308.5568,440.11449"
id="path4006" />
<path
sodipodi:nodetypes="cc"
transform="matrix(0.00256181,-0.99999672,-0.99999672,-0.00256181,675.67433,927.38588)"
id="path4008"
d="m 498.79912,370.56922 89.10071,-76.12668"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
</g>
<path
id="path4075"
d="m 331.59817,148.54231 c 0.90991,-0.75045 3.69622,-2.88986 6.1918,-4.75425 l 4.53742,-3.3898 3.80262,1.08903 c 2.09145,0.59896 3.80263,1.2469 3.80263,1.43986 0,0.30789 -18.78631,6.9796 -19.65325,6.9796 -0.18458,0 0.40887,-0.614 1.31878,-1.36444 z"
style="opacity:1;fill:#666666;fill-opacity:1" />
</g>
<g
id="plane-right"
inkscape:label="#g4371">
<g
id="g3984"
transform="matrix(0.00256181,-0.99999672,-0.99999672,-0.00256181,1070.5332,666.10797)">
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 185.71429,456.6479 110,0 10,27.85714 8.57143,-27.85714 111.42857,0 L 382.85714,338.07647 185.71429,456.6479 z"
id="path3986"
sodipodi:nodetypes="ccccccc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 296.42857,456.6479 383.21429,338.43361"
id="path3988" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 314.66252,456.62472 68.1853,-118.18785"
id="path3990" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 305.57115,485.16153 15.65736,-28.28427"
id="path3992" />
</g>
<path
id="path4077"
d="m 602.06162,363.33367 c -6.43591,-2.39032 -11.37138,-4.37774 -10.96771,-4.41647 0.40366,-0.0387 5.79069,-1.627 11.97117,-3.52947 9.393,-2.89135 18.46611,-7.60947 55.28388,-28.74823 24.22566,-13.90905 44.2519,-25.08393 44.50276,-24.83307 0.25087,0.25086 -19.69141,15.17498 -44.31616,33.1647 l -44.77228,32.70858 -11.70166,-4.34604 z"
style="opacity:1;fill:#666666;fill-opacity:1" />
</g>
<g
id="plane-up"
inkscape:label="#g4391">
<g
transform="translate(-158.56854,5.01515)"
id="g4026">
<path
sodipodi:nodetypes="ccccccc"
id="path4012"
d="m 185.71429,456.6479 110,0 10,27.85714 8.57143,-27.85714 111.42857,0 -119.8167,-238.58667 -120.1833,238.58667 z"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
<path
sodipodi:nodetypes="cc"
id="path4014"
d="m 296.42857,456.6479 9.46902,-235.74646"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
<path
sodipodi:nodetypes="cc"
id="path4016"
d="M 314.66252,456.62472 305.89759,218.42163"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
<path
transform="translate(158.56854,-5.01515)"
id="path4022"
d="m 147.32905,489.5321 0,-4.13846 0,-263.20584"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 185.71429,456.6479 110,0 10,27.85714 8.57143,-27.85714 111.42857,0 -119.8167,-238.58667 -120.1833,238.58667 z"
id="path4024"
sodipodi:nodetypes="ccccccc" />
</g>
<path
id="path4081"
d="m 142.14371,473.11345 -3.82369,-10.68763 0.40417,-5.40719 c 0.22229,-2.97395 1.94295,-44.12254 3.82369,-91.4413 l 3.41951,-86.03413 0.14953,51.06447 c 0.0823,28.08546 0.0823,74.04349 0,102.12894 l -0.14953,51.06447 -3.82368,-10.68763 z"
style="opacity:1;fill:#666666;fill-opacity:1" />
<path
id="path4083"
d="m 148.16566,377.28265 c 0.0301,-58.26325 0.13214,-104.87969 0.22683,-103.59211 0.0947,1.28759 1.67362,43.76715 3.50873,94.39902 l 3.33657,92.05795 -2.16196,7.14466 c -1.18907,3.92956 -2.7926,9.11993 -3.56339,11.53415 l -1.40144,4.3895 0.0547,-105.93317 0,0 z"
style="opacity:1;fill:#666666;fill-opacity:1" />
</g>
<g
id="plane-down"
inkscape:label="#g4401">
<g
id="g4033"
transform="matrix(1,0,0,-1,-158.56854,1000.7047)">
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 185.71429,456.6479 110,0 10,27.85714 8.57143,-27.85714 111.42857,0 -119.8167,-238.58667 -120.1833,238.58667 z"
id="path4035"
sodipodi:nodetypes="ccccccc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 296.42857,456.6479 9.46902,-235.74646"
id="path4037"
sodipodi:nodetypes="cc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="M 314.66252,456.62472 305.89759,218.42163"
id="path4039"
sodipodi:nodetypes="cc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 147.32905,489.5321 0,-4.13846 0,-263.20584"
id="path4041"
transform="translate(158.56854,-5.01515)" />
<path
sodipodi:nodetypes="ccccccc"
id="path4043"
d="m 185.71429,456.6479 110,0 10,27.85714 8.57143,-27.85714 111.42857,0 -119.8167,-238.58667 -120.1833,238.58667 z"
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
</g>
<path
id="path4085"
d="m 148.16566,628.36182 -0.0547,-105.93317 1.40144,4.3895 c 0.77079,2.41422 2.37432,7.60459 3.56339,11.53415 l 2.16196,7.14466 -3.33657,92.05795 c -1.83511,50.63187 -3.41404,93.11143 -3.50873,94.39902 -0.0947,1.28758 -0.19676,-45.32887 -0.22683,-103.59211 z"
style="opacity:1;fill:#666666;fill-opacity:1" />
<path
id="path4087"
d="m 142.54602,640.35978 c -1.88175,-47.47972 -3.6008,-88.73996 -3.8201,-91.68942 l -0.39873,-5.36266 3.8201,-10.73216 3.8201,-10.73216 0.14953,51.01814 c 0.0822,28.05998 0.0822,74.14969 0,102.42158 l -0.14953,51.40343 -3.42137,-86.32675 z"
style="opacity:1;fill:#666666;fill-opacity:1" />
</g>
</g>
</svg>
Side by Side

Before

Width:  |  Height:  |  Size: 14 KiB

376
ground/openpilotgcs/src/plugins/config/revosensors.ui
View File

@ -7,7 +7,7 @@
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>836</width> <width>836</width>
<height>527</height> <height>605</height>
</rect> </rect>
</property> </property>
<property name="windowTitle"> <property name="windowTitle">
@ -62,118 +62,224 @@
<property name="verticalSpacing"> <property name="verticalSpacing">
<number>6</number> <number>6</number>
</property> </property>
<item row="0" column="0" rowspan="4">
<layout class="QVBoxLayout">
<item>
<widget class="QGraphicsView" name="calibrationVisualHelp">
<property name="verticalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
</widget>
</item>
</layout>
</item>
<item row="4" column="0" colspan="2">
<layout class="QVBoxLayout" name="instruction_layout">
<item>
<widget class="QTextEdit" name="calibrationInstructions">
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>Calibration status</string>
</property>
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
<property name="sizeAdjustPolicy">
<enum>QAbstractScrollArea::AdjustToContents</enum>
</property>
</widget>
</item>
<item>
<widget class="QTextEdit" name="textEdit">
<property name="toolTip">
<string>Calibration instructions</string>
</property>
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
<property name="html">
<string>&lt;!DOCTYPE HTML PUBLIC &quot;-//W3C//DTD HTML 4.0//EN&quot; &quot;http://www.w3.org/TR/REC-html40/strict.dtd&quot;&gt;
&lt;html&gt;&lt;head&gt;&lt;meta name=&quot;qrichtext&quot; content=&quot;1&quot; /&gt;&lt;style type=&quot;text/css&quot;&gt;
p, li { white-space: pre-wrap; }
&lt;/style&gt;&lt;/head&gt;&lt;body style=&quot; font-family:'Ubuntu'; font-size:9pt; font-weight:400; font-style:normal;&quot;&gt;
&lt;table border=&quot;0&quot; style=&quot;-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;&quot;&gt;
&lt;tr&gt;
&lt;td style=&quot;border: none;&quot;&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:16pt; font-weight:600; font-style:italic;&quot;&gt;Help&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;Following steps #1, #2 and #3 are necessary. Step #4 is optional, it may helps you achieve the best possible results.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
&lt;p align=&quot;center&quot; style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'MS Shell Dlg 2'; font-size:8pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:14pt; font-weight:600; font-style:italic;&quot;&gt;#1: Multi-Point calibration&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;This calibration will compute the scale for Magnetometer or Accelerometer sensors. &lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;Press &amp;quot;Calibrate Mag&amp;quot; or &amp;quot;Calibrate Accel&amp;quot; to begin calibration, and follow the instructions which will be displayed here. &lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;For optimal calibration perform the Accel calibration with the board not mounted to the craft. in this way you can accurately level the board on your desk/table during the process. &lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;Magnetometer calibration need to be performed inside your plane/copter to account for metal/magnetic stuffs on board.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt; font-weight:600; font-style:italic;&quot;&gt;Note 1:&lt;/span&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt; Your &lt;/span&gt;&lt;span style=&quot; font-size:11pt; font-weight:600;&quot;&gt;HomeLocation must be set first&lt;/span&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;, including the local magnetic field vector (Be) and acceleration due to gravity (g_e).&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt; font-weight:600; font-style:italic;&quot;&gt;Note 2:&lt;/span&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt; There is no need to point exactly at South/North/West/East. They are just used to easily tells the user how to point the plane/craft. &lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;You can simply assume that North is in front of you, right is East etc. and perform the calibration this way.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:14pt; font-weight:600; font-style:italic;&quot;&gt;#2: Board level calibration&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;This step will ensure that board leveling is accurate. Place your airframe as horizontally as possible (use a spirit level if necessary), then press Start below and do not move your airframe at all until the end of the calibration.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:14pt; font-weight:600; font-style:italic;&quot;&gt;#3: Gyro Bias calculation&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;This step allows to calibrate the gyro measured value when the board is steady. To perform the calibration leave the board/aircraft completely steady and click start. &lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:14pt; font-weight:600; font-style:italic;&quot;&gt;#4: Thermal Calibration&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;The calibration will compute sensors bias variations at different temperatures while the board warms up.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;This allow a certain amount of correction of those bias variations against temperature changes. It does improve both altitude hold and yaw performances.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:11pt;&quot;&gt;To perform this calibration leave the board to cool down at room temperature in the coldest places available. after 15-20 minutes attach the usb connector to the board and Click the Calibrate button leaving the board steady. Wait until completed&lt;/span&gt;&lt;/p&gt;
&lt;p align=&quot;center&quot; style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;&lt;/td&gt;&lt;/tr&gt;&lt;/table&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
</widget>
</item>
</layout>
</item>
<item row="0" column="1">
<widget class="QGroupBox" name="groupBox">
<property name="title">
<string>#1: Accelerometer/Magnetometer calibration</string>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_4">
<item>
<widget class="QPushButton" name="sixPointsStartAccel">
<property name="enabled">
<bool>false</bool>
</property>
<property name="toolTip">
<string>Launch accelerometer range and bias calibration.</string>
</property>
<property name="text">
<string>Calibrate Accel</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="sixPointsStartMag">
<property name="enabled">
<bool>false</bool>
</property>
<property name="toolTip">
<string>Launch magnetometer range and bias calibration.</string>
</property>
<property name="text">
<string>Calibrate Mag</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_thermalbias_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QPushButton" name="sixPointsSave">
<property name="enabled">
<bool>false</bool>
</property>
<property name="toolTip">
<string>Save settings (only enabled when calibration is running)</string>
</property>
<property name="text">
<string>Save Position</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item row="1" column="1"> <item row="1" column="1">
<widget class="QGroupBox" name="groupBox"> <widget class="QGroupBox" name="groupBox">
<property name="title"> <property name="title">
<string>#3: Sensor noise calibration</string> <string>#2: Board level calibration</string>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_4"> <layout class="QHBoxLayout">
<item> <item>
<widget class="QGraphicsView" name="sensorsBargraph"> <widget class="QPushButton" name="boardLevelStart">
<property name="toolTip"> <property name="enabled">
<string>These are the sensor variance values computed by the AHRS. <bool>false</bool>
Tip: lower is better!</string>
</property> </property>
<property name="verticalScrollBarPolicy"> <property name="text">
<enum>Qt::ScrollBarAlwaysOff</enum> <string>Start</string>
</property>
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property> </property>
</widget> </widget>
</item> </item>
<item> <item>
<layout class="QHBoxLayout" name="horizontalLayout"> <widget class="QProgressBar" name="boardLevelProgress">
<item> <property name="value">
<widget class="QPushButton" name="noiseMeasurementStart"> <number>0</number>
<property name="enabled"> </property>
<bool>false</bool> <property name="textVisible">
</property> <bool>true</bool>
<property name="toolTip">
<string>Press to start a calibration procedure, about 15 seconds.
Hint: run this with engines at cruising speed.</string>
</property>
<property name="text">
<string>Start</string>
</property>
</widget>
</item>
<item>
<widget class="QProgressBar" name="noiseMeasurementProgress">
<property name="enabled">
<bool>true</bool>
</property>
<property name="maximum">
<number>100</number>
</property>
<property name="value">
<number>0</number>
</property>
<property name="textVisible">
<bool>false</bool>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item row="1" column="0">
<widget class="QGroupBox" name="groupBox_5">
<property name="title">
<string>#2: Magnetometer calibration</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_sixPointCalib">
<item>
<widget class="QGraphicsView" name="sixPointsHelp">
<property name="toolTip">
<string>Nice paper plane, eh?</string>
</property> </property>
</widget> </widget>
</item> </item>
<item> <item>
<layout class="QHBoxLayout" name="horizontalLayout_4"> <widget class="QPushButton" name="boardLevelSavePos">
<item> <property name="enabled">
<widget class="QPushButton" name="sixPointsStart"> <bool>false</bool>
<property name="enabled"> </property>
<bool>false</bool> <property name="text">
</property> <string>Save Position</string>
<property name="toolTip"> </property>
<string>Launch a sensor range and bias calibration.</string> </widget>
</property>
<property name="text">
<string>Start</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="sixPointsSave">
<property name="enabled">
<bool>false</bool>
</property>
<property name="toolTip">
<string>Save settings (only enabled when calibration is running)</string>
</property>
<property name="text">
<string>Save Position</string>
</property>
</widget>
</item>
</layout>
</item> </item>
</layout> </layout>
</widget> </widget>
</item> </item>
<item row="0" column="0" colspan="2"> <item row="2" column="1">
<widget class="QGroupBox" name="groupBox">
<property name="title">
<string>#3: Gyro bias calibration</string>
</property>
<layout class="QHBoxLayout">
<item>
<widget class="QPushButton" name="gyroBiasStart">
<property name="enabled">
<bool>false</bool>
</property>
<property name="text">
<string>Start</string>
</property>
</widget>
</item>
<item>
<widget class="QProgressBar" name="gyroBiasProgress">
<property name="value">
<number>0</number>
</property>
<property name="textVisible">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item row="3" column="1">
<widget class="QGroupBox" name="groupBox_5"> <widget class="QGroupBox" name="groupBox_5">
<property name="title"> <property name="title">
<string>#1: Thermal calibration</string> <string>#4*: Thermal calibration</string>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_thermalbias"> <layout class="QVBoxLayout">
<item> <item>
<layout class="QHBoxLayout" name="horizontalLayout_thermalbias"> <layout class="QHBoxLayout">
<item> <item>
<widget class="QLabel" name="label_temperature"> <widget class="QLabel" name="label_temperature">
<property name="font"> <property name="font">
@ -287,7 +393,7 @@
<number>0</number> <number>0</number>
</property> </property>
<property name="textVisible"> <property name="textVisible">
<bool>false</bool> <bool>true</bool>
</property> </property>
</widget> </widget>
</item> </item>
@ -316,74 +422,6 @@
</layout> </layout>
</widget> </widget>
</item> </item>
<item row="2" column="0" colspan="2">
<widget class="QGroupBox" name="groupBox">
<property name="title">
<string>#4: Accelerometer Bias calibration</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_5">
<item>
<layout class="QHBoxLayout" name="horizontalLayout_3">
<item>
<widget class="QPushButton" name="accelBiasStart">
<property name="enabled">
<bool>false</bool>
</property>
<property name="text">
<string>Start</string>
</property>
</widget>
</item>
<item>
<widget class="QProgressBar" name="accelBiasProgress">
<property name="value">
<number>0</number>
</property>
<property name="textVisible">
<bool>false</bool>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item row="3" column="0" colspan="2">
<widget class="QTextEdit" name="sixPointCalibInstructions">
<property name="toolTip">
<string>Six Point Calibration instructions</string>
</property>
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
<property name="html">
<string>&lt;!DOCTYPE HTML PUBLIC &quot;-//W3C//DTD HTML 4.0//EN&quot; &quot;http://www.w3.org/TR/REC-html40/strict.dtd&quot;&gt;
&lt;html&gt;&lt;head&gt;&lt;meta name=&quot;qrichtext&quot; content=&quot;1&quot; /&gt;&lt;style type=&quot;text/css&quot;&gt;
p, li { white-space: pre-wrap; }
&lt;/style&gt;&lt;/head&gt;&lt;body style=&quot; font-family:'Sans Serif'; font-size:9pt; font-weight:400; font-style:normal;&quot;&gt;
&lt;table border=&quot;0&quot; style=&quot;-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;&quot;&gt;
&lt;tr&gt;
&lt;td style=&quot;border: none;&quot;&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt; font-weight:600;&quot;&gt;Help&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;Step #1 and #2 and 3 are really necessary. Step #4 will help you achieve the best possible results.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;#1: Thermal Calibration:&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;The calibration will compute the variation of all sensors bias at different temperatures while the board warm up.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;This allow a certain amount of correction of those bias variations against temperature changes. It does improve both altitude hold and yaw performances.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;To perform this calibration leave the board to cool down at room temperature in the coldest places available. after 15-20 minutes attach the usb connector to the board and Click the Calibrate button leaving the board steady. Wait until completed&lt;/span&gt;&lt;/p&gt;
&lt;p align=&quot;center&quot; style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;#1: Multi-Point calibration:&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;This calibration will compute the scale for all sensors on the INS. Press &amp;quot;Start&amp;quot; to begin calibration, and follow the instructions which will be displayed here. Note that your HomeLocation must be set first, including the local magnetic field vector (Be) and acceleration due to gravity (g_e).&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;#2: Sensor noise calibration:&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;This calibration will compute sensor variance under standard conditions. You can leave your engines running at low to mid throttle to take their vibration into account before pressing &amp;quot;Start&amp;quot;.&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;#3: Accelerometer bias calibration:&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;This step will ensure that accelerometer bias is accurate. Place your airframe as horizontally as possible (use a spirit level if necessary), then press Start below and do not move your airframe at all until the end of the calibration.&lt;/span&gt;&lt;/p&gt;&lt;/td&gt;&lt;/tr&gt;&lt;/table&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
</widget>
</item>
</layout> </layout>
</item> </item>
</layout> </layout>
@ -430,12 +468,12 @@ p, li { white-space: pre-wrap; }
</spacer> </spacer>
</item> </item>
<item row="1" column="3"> <item row="1" column="3">
<widget class="QSpinBox" name="pitchRotation"> <widget class="QDoubleSpinBox" name="pitchRotation">
<property name="minimum"> <property name="minimum">
<number>-90</number> <double>-90.000000000000000</double>
</property> </property>
<property name="maximum"> <property name="maximum">
<number>90</number> <double>90.000000000000000</double>
</property> </property>
</widget> </widget>
</item> </item>
@ -512,12 +550,12 @@ font:bold;</string>
</spacer> </spacer>
</item> </item>
<item row="1" column="1"> <item row="1" column="1">
<widget class="QSpinBox" name="rollRotation"> <widget class="QDoubleSpinBox" name="rollRotation">
<property name="minimum"> <property name="minimum">
<number>-180</number> <double>-180.000000000000000</double>
</property> </property>
<property name="maximum"> <property name="maximum">
<number>180</number> <double>180.000000000000000</double>
</property> </property>
</widget> </widget>
</item> </item>
@ -545,12 +583,12 @@ font:bold;</string>
</widget> </widget>
</item> </item>
<item row="1" column="5"> <item row="1" column="5">
<widget class="QSpinBox" name="yawRotation"> <widget class="QDoubleSpinBox" name="yawRotation">
<property name="minimum"> <property name="minimum">
<number>-180</number> <double>-180.000000000000000</double>
</property> </property>
<property name="maximum"> <property name="maximum">
<number>180</number> <double>180.000000000000000</double>
</property> </property>
</widget> </widget>
</item> </item>

28
ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp
View File

@ -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()
{ {

2
ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h
View File

@ -106,8 +106,6 @@ public:
void saveObjectToSD(UAVObject *obj); void saveObjectToSD(UAVObject *obj);
UAVObjectManager *getObjectManager(); UAVObjectManager *getObjectManager();
static double listMean(QList<double> list);
static double listVar(QList<double> list);
void addUAVObject(QString objectName, QList<int> *reloadGroups = NULL); void addUAVObject(QString objectName, QList<int> *reloadGroups = NULL);
void addUAVObject(UAVObject *objectName, QList<int> *reloadGroups = NULL); void addUAVObject(UAVObject *objectName, QList<int> *reloadGroups = NULL);

2
shared/uavobjectdefinition/attitudesettings.xml
View File

@ -1,7 +1,7 @@
<xml> <xml>
<object name="AttitudeSettings" singleinstance="true" settings="true" category="State"> <object name="AttitudeSettings" singleinstance="true" settings="true" category="State">
<description>Settings for the @ref Attitude module used on CopterControl</description> <description>Settings for the @ref Attitude module used on CopterControl</description>
<field name="BoardRotation" units="deg" type="int16" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0"/> <field name="BoardRotation" units="deg" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0"/>
<field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.05"/> <field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.05"/>
<field name="AccelKi" units="channel" type="float" elements="1" defaultvalue="0.0001"/> <field name="AccelKi" units="channel" type="float" elements="1" defaultvalue="0.0001"/>
<field name="MagKi" units="" type="float" elements="1" defaultvalue="0"/> <field name="MagKi" units="" type="float" elements="1" defaultvalue="0"/>

3
shared/uavobjectdefinition/revocalibration.xml
View File

@ -2,7 +2,8 @@
<object name="RevoCalibration" singleinstance="true" settings="true" category="Sensors"> <object name="RevoCalibration" singleinstance="true" settings="true" category="Sensors">
<description>Settings for the INS to control the algorithm and what is updated</description> <description>Settings for the INS to control the algorithm and what is updated</description>
<field name="mag_bias" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/> <field name="mag_bias" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/>
<field name="mag_scale" units="gain" type="float" elementnames="X,Y,Z" defaultvalue="1"/> <field name="mag_transform" units="gain" type="float" elementnames="r0c0,r0c1,r0c2,r1c0,r1c1,r1c2,r2c0,r2c1,r2c2"
defaultvalue="1,0,0,0,1,0,0,0,1"/>
<!-- These settings are related to how the sensors are post processed --> <!-- These settings are related to how the sensors are post processed -->
<!-- TODO: reimplement, put elsewhere (later) --> <!-- TODO: reimplement, put elsewhere (later) -->
<field name="BiasCorrectedRaw" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/> <field name="BiasCorrectedRaw" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>