mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
Merge branch 'amorale/OP-975_calibration_process_changes' into next
This commit is contained in:
Alessio Morale
commit
7a85c0939c
@ -693,8 +693,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
|
||||
}
|
||||
|
||||
// Indicates not to expend cycles on rotation
|
||||
if (attitudeSettings.BoardRotation.Pitch == 0 && attitudeSettings.BoardRotation.Roll == 0 &&
|
||||
attitudeSettings.BoardRotation.Yaw == 0) {
|
||||
if (fabsf(attitudeSettings.BoardRotation.Pitch) < 0.00001f &&
|
||||
fabsf(attitudeSettings.BoardRotation.Roll) < 0.00001f &&
|
||||
fabsf(attitudeSettings.BoardRotation.Yaw) < 0.00001f) {
|
||||
rotate = 0;
|
||||
|
||||
// Shouldn't be used but to be safe
|
||||
|
||||
@ -84,10 +84,11 @@ AccelGyroSettingsData agcal;
|
||||
// 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_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
|
||||
static volatile bool gyro_temp_calibrated = false;
|
||||
static volatile bool accel_temp_calibrated = false;
|
||||
|
||||
static float R[3][3] = {
|
||||
{ 0 }
|
||||
};
|
||||
@ -119,6 +120,7 @@ int32_t SensorsInitialize(void)
|
||||
|
||||
RevoCalibrationConnectCallback(&settingsUpdatedCb);
|
||||
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
|
||||
|
||||
AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
|
||||
return 0;
|
||||
}
|
||||
@ -359,7 +361,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
|
||||
accels_out[1] -= agcal.accel_temp_coeff.Y * ctemp;
|
||||
accels_out[2] -= agcal.accel_temp_coeff.Z * ctemp;
|
||||
}
|
||||
|
||||
if (rotate) {
|
||||
rot_mult(R, accels_out, accels);
|
||||
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[2] -= (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
|
||||
}
|
||||
|
||||
if (rotate) {
|
||||
rot_mult(R, gyros_out, gyros);
|
||||
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) {
|
||||
int16_t values[3];
|
||||
PIOS_HMC5883_ReadMag(values);
|
||||
float mags[3] = { (float)values[1] * mag_scale[0] - mag_bias[0],
|
||||
(float)values[0] * mag_scale[1] - mag_bias[1],
|
||||
-(float)values[2] * mag_scale[2] - mag_bias[2] };
|
||||
if (rotate) {
|
||||
float mag_out[3];
|
||||
rot_mult(R, mags, mag_out);
|
||||
mag.x = mag_out[0];
|
||||
mag.y = mag_out[1];
|
||||
mag.z = mag_out[2];
|
||||
} else {
|
||||
mag.x = mags[0];
|
||||
mag.y = mags[1];
|
||||
mag.z = mags[2];
|
||||
}
|
||||
float mags[3] = { (float)values[1] - mag_bias[0],
|
||||
(float)values[0] - mag_bias[1],
|
||||
-(float)values[2] - mag_bias[2] };
|
||||
|
||||
float mag_out[3];
|
||||
rot_mult(mag_transform, mags, mag_out);
|
||||
|
||||
mag.x = mag_out[0];
|
||||
mag.y = mag_out[1];
|
||||
mag.z = mag_out[2];
|
||||
|
||||
MagSensorSet(&mag);
|
||||
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[1] = cal.mag_bias.Y;
|
||||
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) &&
|
||||
(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);
|
||||
|
||||
// Indicates not to expend cycles on rotation
|
||||
if (attitudeSettings.BoardRotation.Roll == 0 && attitudeSettings.BoardRotation.Pitch == 0 &&
|
||||
attitudeSettings.BoardRotation.Yaw == 0) {
|
||||
if (fabsf(attitudeSettings.BoardRotation.Roll) < 0.00001f
|
||||
&& fabsf(attitudeSettings.BoardRotation.Pitch) < 0.00001f &&
|
||||
fabsf(attitudeSettings.BoardRotation.Yaw) <0.00001f ) {
|
||||
rotate = 0;
|
||||
} else {
|
||||
float rotationQuat[4];
|
||||
rotate = 1;
|
||||
}
|
||||
float rotationQuat[4];
|
||||
const float rpy[3] = { attitudeSettings.BoardRotation.Roll,
|
||||
attitudeSettings.BoardRotation.Pitch,
|
||||
attitudeSettings.BoardRotation.Yaw };
|
||||
RPY2Quaternion(rpy, rotationQuat);
|
||||
Quaternion2R(rotationQuat, R);
|
||||
rotate = 1;
|
||||
}
|
||||
RPY2Quaternion(rpy, rotationQuat);
|
||||
Quaternion2R(rotationQuat, R);
|
||||
|
||||
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;
|
||||
}
|
||||
/**
|
||||
* @}
|
||||
|
||||
@ -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
|
||||
@ -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:
|
||||
* 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::Vector3f center;
|
||||
Eigen::MatrixXf evecs;
|
||||
|
||||
EllipsoidFit(samplesX, samplesY, samplesZ, ¢er, &radii, &evecs);
|
||||
EllipsoidFit(samplesX, samplesY, samplesZ, ¢er, &radii, &evecs, fitAlongXYZ);
|
||||
|
||||
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,
|
||||
Eigen::Vector3f *center,
|
||||
Eigen::VectorXf *radii,
|
||||
Eigen::MatrixXf *evecs)
|
||||
Eigen::MatrixXf *evecs,
|
||||
bool fitAlongXYZ)
|
||||
{
|
||||
int numSamples = (*samplesX).rows();
|
||||
Eigen::MatrixXf D(numSamples, 9);
|
||||
|
||||
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);
|
||||
Eigen::MatrixXf D;
|
||||
|
||||
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);
|
||||
ones.setOnes(numSamples);
|
||||
|
||||
@ -307,25 +320,163 @@ void CalibrationUtils::EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *
|
||||
Eigen::MatrixXf dt2 = (D.transpose() * ones);
|
||||
Eigen::VectorXf v = dt1.inverse() * dt2;
|
||||
|
||||
Eigen::Matrix4f A;
|
||||
A << v.coeff(0), v.coeff(3), v.coeff(4), v.coeff(6),
|
||||
v.coeff(3), v.coeff(1), v.coeff(5), v.coeff(7),
|
||||
v.coeff(4), v.coeff(5), v.coeff(2), v.coeff(8),
|
||||
v.coeff(6), v.coeff(7), v.coeff(8), -1;
|
||||
if (!fitAlongXYZ) {
|
||||
Eigen::Matrix4f A;
|
||||
A << v.coeff(0), v.coeff(3), v.coeff(4), v.coeff(6),
|
||||
v.coeff(3), v.coeff(1), v.coeff(5), v.coeff(7),
|
||||
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();
|
||||
t.block(3, 0, 1, 3) = center->transpose();
|
||||
Eigen::Matrix4f t = Eigen::Matrix4f::Identity();
|
||||
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::EigenSolver<Eigen::Matrix3f> es(tmp2);
|
||||
Eigen::VectorXf evals = es.eigenvalues().real();
|
||||
(*evecs) = es.eigenvectors().real();
|
||||
radii->setZero(3);
|
||||
Eigen::Matrix3f tmp2 = r.block(0, 0, 3, 3) * -1 / r.coeff(3, 3);
|
||||
Eigen::EigenSolver<Eigen::Matrix3f> es(tmp2);
|
||||
Eigen::VectorXf evals = es.eigenvalues().real();
|
||||
|
||||
(*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);
|
||||
}
|
||||
}
|
||||
|
||||
@ -32,7 +32,7 @@
|
||||
#include <Eigen/Eigenvalues>
|
||||
#include <Eigen/Dense>
|
||||
#include <Eigen/LU>
|
||||
|
||||
#include <QList>
|
||||
namespace OpenPilot {
|
||||
class CalibrationUtils {
|
||||
public:
|
||||
@ -41,17 +41,25 @@ public:
|
||||
Eigen::Vector3f Scale;
|
||||
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 void ComputePoly(Eigen::VectorXf *samplesX, Eigen::VectorXf *polynomial, Eigen::VectorXf *polyY);
|
||||
static float ComputeSigma(Eigen::VectorXf *samplesY);
|
||||
|
||||
static int SixPointInConstFieldCal(double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3]);
|
||||
static double listMean(QList<double> list);
|
||||
static double listVar(QList<double> list);
|
||||
private:
|
||||
static void EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
|
||||
Eigen::Vector3f *center,
|
||||
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
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
@ -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
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
@ -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
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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
|
||||
@ -51,7 +51,11 @@ HEADERS += configplugin.h \
|
||||
calibration/thermal/boardsetuptransition.h \
|
||||
calibration/thermal/dataacquisitiontransition.h \
|
||||
calibration/thermal/settingshandlingtransitions.h \
|
||||
calibration/thermal/compensationcalculationtransition.h
|
||||
calibration/thermal/compensationcalculationtransition.h \
|
||||
calibration/sixpointcalibrationmodel.h \
|
||||
calibration/levelcalibrationmodel.h \
|
||||
calibration/gyrobiascalibrationmodel.h \
|
||||
calibration/calibrationuiutils.h
|
||||
|
||||
SOURCES += configplugin.cpp \
|
||||
configgadgetwidget.cpp \
|
||||
@ -86,7 +90,10 @@ SOURCES += configplugin.cpp \
|
||||
calibration/wizardmodel.cpp \
|
||||
calibration/thermal/thermalcalibration.cpp \
|
||||
calibration/thermal/thermalcalibrationhelper.cpp \
|
||||
calibration/thermal/thermalcalibrationmodel.cpp
|
||||
calibration/thermal/thermalcalibrationmodel.cpp \
|
||||
calibration/sixpointcalibrationmodel.cpp \
|
||||
calibration/levelcalibrationmodel.cpp \
|
||||
calibration/gyrobiascalibrationmodel.cpp
|
||||
|
||||
FORMS += airframe.ui \
|
||||
airframe_ccpm.ui \
|
||||
|
||||
@ -38,7 +38,7 @@
|
||||
#include "gyrostate.h"
|
||||
#include <extensionsystem/pluginmanager.h>
|
||||
#include <coreplugin/generalsettings.h>
|
||||
|
||||
#include <calibration/calibrationutils.h>
|
||||
ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) :
|
||||
ConfigTaskWidget(parent),
|
||||
ui(new Ui_ccattitude)
|
||||
@ -114,13 +114,13 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject *obj)
|
||||
disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *)));
|
||||
disconnect(&timer, SIGNAL(timeout()), this, SLOT(timeout()));
|
||||
|
||||
float x_bias = listMean(x_accum);
|
||||
float y_bias = listMean(y_accum);
|
||||
float z_bias = (listMean(z_accum) + 9.81);
|
||||
float x_bias = OpenPilot::CalibrationUtils::listMean(x_accum);
|
||||
float y_bias = OpenPilot::CalibrationUtils::listMean(y_accum);
|
||||
float z_bias = OpenPilot::CalibrationUtils::listMean(z_accum) + 9.81;
|
||||
|
||||
float x_gyro_bias = listMean(x_gyro_accum);
|
||||
float y_gyro_bias = listMean(y_gyro_accum);
|
||||
float z_gyro_bias = listMean(z_gyro_accum);
|
||||
float x_gyro_bias = OpenPilot::CalibrationUtils::listMean(x_gyro_accum);
|
||||
float y_gyro_bias = OpenPilot::CalibrationUtils::listMean(y_gyro_accum);
|
||||
float z_gyro_bias = OpenPilot::CalibrationUtils::listMean(z_gyro_accum);
|
||||
accelState->setMetadata(initialAccelStateMdata);
|
||||
gyroState->setMetadata(initialGyroStateMdata);
|
||||
|
||||
|
||||
@ -2,7 +2,6 @@
|
||||
<qresource prefix="/configgadget">
|
||||
<file>images/help2.png</file>
|
||||
<file>images/ahrs-calib.svg</file>
|
||||
<file>images/paper-plane.svg</file>
|
||||
<file>images/multirotor-shapes.svg</file>
|
||||
<file>images/ccpm_setup.svg</file>
|
||||
<file>images/PipXtreme.png</file>
|
||||
@ -31,5 +30,20 @@
|
||||
<file>images/pipx-normal.png</file>
|
||||
<file>images/revolution_top.png</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>
|
||||
</RCC>
|
||||
|
||||
@ -40,25 +40,23 @@
|
||||
#include <iostream>
|
||||
#include <QDesktopServices>
|
||||
#include <QUrl>
|
||||
#include <attitudestate.h>
|
||||
#include <attitudesettings.h>
|
||||
#include <ekfconfiguration.h>
|
||||
#include <revocalibration.h>
|
||||
#include <accelgyrosettings.h>
|
||||
#include <homelocation.h>
|
||||
#include <accelstate.h>
|
||||
#include <gyrostate.h>
|
||||
|
||||
#include <magstate.h>
|
||||
|
||||
#define GRAVITY 9.81f
|
||||
#include "assertions.h"
|
||||
#include "calibration.h"
|
||||
|
||||
#include "calibration/calibrationutils.h"
|
||||
#define sign(x) ((x < 0) ? -1 : 1)
|
||||
|
||||
// Uncomment this to enable 6 point calibration on the accels
|
||||
#define SIX_POINT_CAL_ACCEL
|
||||
#define NOISE_SAMPLES 50
|
||||
|
||||
const double ConfigRevoWidget::maxVarValue = 0.1;
|
||||
|
||||
// *****************
|
||||
|
||||
@ -75,146 +73,18 @@ public:
|
||||
ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
|
||||
ConfigTaskWidget(parent),
|
||||
m_ui(new Ui_RevoSensorsWidget()),
|
||||
collectingData(false),
|
||||
position(-1),
|
||||
isBoardRotationStored(false)
|
||||
{
|
||||
m_ui->setupUi(this);
|
||||
|
||||
// Initialization of the Paper plane widget
|
||||
m_ui->sixPointsHelp->setScene(new QGraphicsScene(this));
|
||||
|
||||
paperplane = new QGraphicsSvgItem();
|
||||
paperplane->setSharedRenderer(new QSvgRenderer());
|
||||
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);
|
||||
|
||||
m_ui->calibrationVisualHelp->setScene(new QGraphicsScene(this));
|
||||
m_ui->calibrationVisualHelp->setRenderHint(QPainter::HighQualityAntialiasing, true);
|
||||
m_ui->calibrationVisualHelp->setRenderHint(QPainter::SmoothPixmapTransform, true);
|
||||
displayVisualHelp("empty");
|
||||
// Must set up the UI (above) before setting up the UAVO mappings or refreshWidgetValues
|
||||
// will be dealing with some null pointers
|
||||
addUAVObject("RevoCalibration");
|
||||
addUAVObject("EKFConfiguration");
|
||||
addUAVObject("HomeLocation");
|
||||
addUAVObject("AttitudeSettings");
|
||||
addUAVObject("RevoSettings");
|
||||
@ -238,11 +108,46 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
|
||||
connect(m_thermalCalibrationModel, SIGNAL(progressChanged(int)), m_ui->thermalBiasProgress, SLOT(setValue(int)));
|
||||
// note: init for m_thermalCalibrationModel is done in showEvent to prevent cases wiht "Start" button not enabled due to some itming issue.
|
||||
|
||||
m_sixPointCalibrationModel = new OpenPilot::SixPointCalibrationModel(this);
|
||||
// six point calibrations
|
||||
connect(m_ui->sixPointsStartAccel, SIGNAL(clicked()), m_sixPointCalibrationModel, SLOT(accelStart()));
|
||||
connect(m_ui->sixPointsStartMag, SIGNAL(clicked()), m_sixPointCalibrationModel, SLOT(magStart()));
|
||||
connect(m_ui->sixPointsSave, SIGNAL(clicked()), m_sixPointCalibrationModel, SLOT(savePositionData()));
|
||||
|
||||
connect(m_sixPointCalibrationModel, SIGNAL(disableAllCalibrations()), this, SLOT(disableAllCalibrations()));
|
||||
connect(m_sixPointCalibrationModel, SIGNAL(enableAllCalibrations()), this, SLOT(enableAllCalibrations()));
|
||||
connect(m_sixPointCalibrationModel, SIGNAL(storeAndClearBoardRotation()), this, SLOT(storeAndClearBoardRotation()));
|
||||
connect(m_sixPointCalibrationModel, SIGNAL(recallBoardRotation()), this, SLOT(recallBoardRotation()));
|
||||
connect(m_sixPointCalibrationModel, SIGNAL(displayInstructions(QString, bool)), this, SLOT(displayInstructions(QString, bool)));
|
||||
connect(m_sixPointCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString)));
|
||||
connect(m_sixPointCalibrationModel, SIGNAL(savePositionEnabledChanged(bool)), this->m_ui->sixPointsSave, SLOT(setEnabled(bool)));
|
||||
|
||||
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(m_ui->accelBiasStart, SIGNAL(clicked()), this, SLOT(doStartAccelGyroBiasCalibration()));
|
||||
connect(m_ui->sixPointsStart, SIGNAL(clicked()), this, SLOT(doStartSixPointCalibration()));
|
||||
connect(m_ui->sixPointsSave, SIGNAL(clicked()), this, SLOT(savePositionData()));
|
||||
connect(m_ui->noiseMeasurementStart, SIGNAL(clicked()), this, SLOT(doStartNoiseMeasurement()));
|
||||
// gyro zero calibration
|
||||
m_gyroBiasCalibrationModel = new OpenPilot::GyroBiasCalibrationModel(this);
|
||||
connect(m_ui->gyroBiasStart, SIGNAL(clicked()), m_gyroBiasCalibrationModel, SLOT(start()));
|
||||
|
||||
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()));
|
||||
|
||||
@ -256,6 +161,7 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
|
||||
populateWidgets();
|
||||
refreshWidgetsValues();
|
||||
m_ui->tabWidget->setCurrentIndex(0);
|
||||
enableAllCalibrations();
|
||||
}
|
||||
|
||||
ConfigRevoWidget::~ConfigRevoWidget()
|
||||
@ -266,614 +172,17 @@ ConfigRevoWidget::~ConfigRevoWidget()
|
||||
|
||||
void ConfigRevoWidget::showEvent(QShowEvent *event)
|
||||
{
|
||||
Q_UNUSED(event)
|
||||
// Thit fitInView method should only be called now, once the
|
||||
// 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);
|
||||
|
||||
Q_UNUSED(event);
|
||||
m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio);
|
||||
m_thermalCalibrationModel->init();
|
||||
}
|
||||
|
||||
void ConfigRevoWidget::resizeEvent(QResizeEvent *event)
|
||||
{
|
||||
Q_UNUSED(event)
|
||||
m_ui->sensorsBargraph->fitInView(sensorsBargraph, Qt::KeepAspectRatio);
|
||||
m_ui->sixPointsHelp->fitInView(paperplane, Qt::KeepAspectRatio);
|
||||
Q_UNUSED(event);
|
||||
m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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()
|
||||
{
|
||||
@ -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->sixPointsHelp->setSceneRect(paperplane->boundingRect());
|
||||
m_ui->sixPointsHelp->fitInView(paperplane, Qt::KeepAspectRatio);
|
||||
m_ui->calibrationVisualHelp->scene()->clear();
|
||||
QPixmap pixmap = QPixmap(":/configgadget/images/calibration/" + elementID + ".png");
|
||||
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 **************/
|
||||
/**
|
||||
* Connect sensor updates and timeout for measuring the noise
|
||||
*/
|
||||
void ConfigRevoWidget::doStartNoiseMeasurement()
|
||||
void ConfigRevoWidget::displayInstructions(QString instructions, bool replace)
|
||||
{
|
||||
QMutexLocker lock(&sensorsUpdateLock);
|
||||
|
||||
// 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 (replace || instructions.isNull()) {
|
||||
m_ui->calibrationInstructions->clear();
|
||||
}
|
||||
|
||||
accel_accum_x.clear();
|
||||
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();
|
||||
if (!instructions.isNull()) {
|
||||
m_ui->calibrationInstructions->append(instructions);
|
||||
}
|
||||
}
|
||||
|
||||
/********** 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
|
||||
@ -1147,11 +252,7 @@ void ConfigRevoWidget::refreshWidgetsValues(UAVObject *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->isSetCheckBox->setEnabled(false);
|
||||
@ -1180,3 +281,21 @@ void ConfigRevoWidget::clearHomeLocation()
|
||||
homeLocationData.Set = HomeLocation::SET_FALSE;
|
||||
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);
|
||||
}
|
||||
|
||||
@ -29,7 +29,6 @@
|
||||
|
||||
#include "ui_revosensors.h"
|
||||
#include "configtaskwidget.h"
|
||||
|
||||
#include "../uavobjectwidgetutils/configtaskwidget.h"
|
||||
#include "extensionsystem/pluginmanager.h"
|
||||
#include "uavobjectmanager.h"
|
||||
@ -41,8 +40,12 @@
|
||||
#include <QTimer>
|
||||
#include <QMutex>
|
||||
#include "calibration/thermal/thermalcalibrationmodel.h"
|
||||
#include "calibration/sixpointcalibrationmodel.h"
|
||||
#include "calibration/levelcalibrationmodel.h"
|
||||
#include "calibration/gyrobiascalibrationmodel.h"
|
||||
class Ui_Widget;
|
||||
|
||||
|
||||
class ConfigRevoWidget : public ConfigTaskWidget {
|
||||
Q_OBJECT
|
||||
|
||||
@ -51,83 +54,32 @@ public:
|
||||
~ConfigRevoWidget();
|
||||
|
||||
private:
|
||||
void drawVariancesGraph();
|
||||
void displayPlane(QString elementID);
|
||||
|
||||
// ! Computes the scale and bias of the mag based on collected data
|
||||
void computeScaleBias();
|
||||
|
||||
OpenPilot::SixPointCalibrationModel *m_sixPointCalibrationModel;
|
||||
OpenPilot::ThermalCalibrationModel *m_thermalCalibrationModel;
|
||||
OpenPilot::LevelCalibrationModel *m_levelCalibrationModel;
|
||||
OpenPilot::GyroBiasCalibrationModel *m_gyroBiasCalibrationModel;
|
||||
|
||||
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
|
||||
qint16 storedBoardRotation[3];
|
||||
bool isBoardRotationStored;
|
||||
void storeAndClearBoardRotation();
|
||||
void recallBoardRotation();
|
||||
|
||||
|
||||
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
|
||||
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
|
||||
void clearHomeLocation();
|
||||
|
||||
void disableAllCalibrations();
|
||||
void enableAllCalibrations();
|
||||
|
||||
protected:
|
||||
void showEvent(QShowEvent *event);
|
||||
void resizeEvent(QResizeEvent *event);
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 150 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 86 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 75 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 57 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 117 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 56 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 176 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 57 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 115 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 75 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 80 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 70 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 61 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 79 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 67 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 113 KiB |
Binary file not shown.
@ -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>
|
||||
|
Before Width: | Height: | Size: 14 KiB |
@ -7,7 +7,7 @@
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>836</width>
|
||||
<height>527</height>
|
||||
<height>605</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="windowTitle">
|
||||
@ -62,118 +62,224 @@
|
||||
<property name="verticalSpacing">
|
||||
<number>6</number>
|
||||
</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><!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd">
|
||||
<html><head><meta name="qrichtext" content="1" /><style type="text/css">
|
||||
p, li { white-space: pre-wrap; }
|
||||
</style></head><body style=" font-family:'Ubuntu'; font-size:9pt; font-weight:400; font-style:normal;">
|
||||
<table border="0" style="-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;">
|
||||
<tr>
|
||||
<td style="border: none;">
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:16pt; font-weight:600; font-style:italic;">Help</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Following steps #1, #2 and #3 are necessary. Step #4 is optional, it may helps you achieve the best possible results.</span></p>
|
||||
<p style="-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;"><br /></p>
|
||||
<p align="center" style="-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;"><br /></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14pt; font-weight:600; font-style:italic;">#1: Multi-Point calibration</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">This calibration will compute the scale for Magnetometer or Accelerometer sensors. </span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Press &quot;Calibrate Mag&quot; or &quot;Calibrate Accel&quot; to begin calibration, and follow the instructions which will be displayed here. </span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">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. </span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Magnetometer calibration need to be performed inside your plane/copter to account for metal/magnetic stuffs on board.</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt; font-weight:600; font-style:italic;">Note 1:</span><span style=" font-size:11pt;"> Your </span><span style=" font-size:11pt; font-weight:600;">HomeLocation must be set first</span><span style=" font-size:11pt;">, including the local magnetic field vector (Be) and acceleration due to gravity (g_e).</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt; font-weight:600; font-style:italic;">Note 2:</span><span style=" font-size:11pt;"> 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. </span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">You can simply assume that North is in front of you, right is East etc. and perform the calibration this way.</span></p>
|
||||
<p style="-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;"><br /></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14pt; font-weight:600; font-style:italic;">#2: Board level calibration</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">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.</span></p>
|
||||
<p style="-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;"><br /></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14pt; font-weight:600; font-style:italic;">#3: Gyro Bias calculation</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">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. </span></p>
|
||||
<p style="-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;"><br /></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14pt; font-weight:600; font-style:italic;">#4: Thermal Calibration</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">The calibration will compute sensors bias variations at different temperatures while the board warms up.</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">This allow a certain amount of correction of those bias variations against temperature changes. It does improve both altitude hold and yaw performances.</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">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</span></p>
|
||||
<p align="center" style="-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;"><br /></p></td></tr></table></body></html></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">
|
||||
<widget class="QGroupBox" name="groupBox">
|
||||
<property name="title">
|
||||
<string>#3: Sensor noise calibration</string>
|
||||
<string>#2: Board level calibration</string>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_4">
|
||||
<layout class="QHBoxLayout">
|
||||
<item>
|
||||
<widget class="QGraphicsView" name="sensorsBargraph">
|
||||
<property name="toolTip">
|
||||
<string>These are the sensor variance values computed by the AHRS.
|
||||
Tip: lower is better!</string>
|
||||
<widget class="QPushButton" name="boardLevelStart">
|
||||
<property name="enabled">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="verticalScrollBarPolicy">
|
||||
<enum>Qt::ScrollBarAlwaysOff</enum>
|
||||
</property>
|
||||
<property name="horizontalScrollBarPolicy">
|
||||
<enum>Qt::ScrollBarAlwaysOff</enum>
|
||||
<property name="text">
|
||||
<string>Start</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout">
|
||||
<item>
|
||||
<widget class="QPushButton" name="noiseMeasurementStart">
|
||||
<property name="enabled">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<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>
|
||||
<widget class="QProgressBar" name="boardLevelProgress">
|
||||
<property name="value">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="textVisible">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_4">
|
||||
<item>
|
||||
<widget class="QPushButton" name="sixPointsStart">
|
||||
<property name="enabled">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string>Launch a sensor range and bias calibration.</string>
|
||||
</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>
|
||||
<widget class="QPushButton" name="boardLevelSavePos">
|
||||
<property name="enabled">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Save Position</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</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">
|
||||
<property name="title">
|
||||
<string>#1: Thermal calibration</string>
|
||||
<string>#4*: Thermal calibration</string>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_thermalbias">
|
||||
<layout class="QVBoxLayout">
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_thermalbias">
|
||||
<layout class="QHBoxLayout">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_temperature">
|
||||
<property name="font">
|
||||
@ -287,7 +393,7 @@
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="textVisible">
|
||||
<bool>false</bool>
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
@ -316,74 +422,6 @@
|
||||
</layout>
|
||||
</widget>
|
||||
</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><!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd">
|
||||
<html><head><meta name="qrichtext" content="1" /><style type="text/css">
|
||||
p, li { white-space: pre-wrap; }
|
||||
</style></head><body style=" font-family:'Sans Serif'; font-size:9pt; font-weight:400; font-style:normal;">
|
||||
<table border="0" style="-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;">
|
||||
<tr>
|
||||
<td style="border: none;">
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600;">Help</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">Step #1 and #2 and 3 are really necessary. Step #4 will help you achieve the best possible results.</span></p>
|
||||
<p style="-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;"><br /></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">#1: Thermal Calibration:</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">The calibration will compute the variation of all sensors bias at different temperatures while the board warm up.</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">This allow a certain amount of correction of those bias variations against temperature changes. It does improve both altitude hold and yaw performances.</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">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</span></p>
|
||||
<p align="center" style="-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;"><br /></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">#1: Multi-Point calibration:</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">This calibration will compute the scale for all sensors on the INS. Press &quot;Start&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).</span></p>
|
||||
<p style="-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;"><br /></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">#2: Sensor noise calibration:</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">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 &quot;Start&quot;.</span></p>
|
||||
<p style="-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;"><br /></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">#3: Accelerometer bias calibration:</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">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.</span></p></td></tr></table></body></html></string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
@ -430,12 +468,12 @@ p, li { white-space: pre-wrap; }
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="1" column="3">
|
||||
<widget class="QSpinBox" name="pitchRotation">
|
||||
<widget class="QDoubleSpinBox" name="pitchRotation">
|
||||
<property name="minimum">
|
||||
<number>-90</number>
|
||||
<double>-90.000000000000000</double>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>90</number>
|
||||
<double>90.000000000000000</double>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
@ -512,12 +550,12 @@ font:bold;</string>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="QSpinBox" name="rollRotation">
|
||||
<widget class="QDoubleSpinBox" name="rollRotation">
|
||||
<property name="minimum">
|
||||
<number>-180</number>
|
||||
<double>-180.000000000000000</double>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>180</number>
|
||||
<double>180.000000000000000</double>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
@ -545,12 +583,12 @@ font:bold;</string>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="5">
|
||||
<widget class="QSpinBox" name="yawRotation">
|
||||
<widget class="QDoubleSpinBox" name="yawRotation">
|
||||
<property name="minimum">
|
||||
<number>-180</number>
|
||||
<double>-180.000000000000000</double>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>180</number>
|
||||
<double>180.000000000000000</double>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
|
||||
@ -228,34 +228,6 @@ UAVObjectManager *ConfigTaskWidget::getObjectManager()
|
||||
return objMngr;
|
||||
}
|
||||
|
||||
double ConfigTaskWidget::listMean(QList<double> list)
|
||||
{
|
||||
double accum = 0;
|
||||
|
||||
for (int i = 0; i < list.size(); i++) {
|
||||
accum += list[i];
|
||||
}
|
||||
return accum / list.size();
|
||||
}
|
||||
|
||||
double ConfigTaskWidget::listVar(QList<double> list)
|
||||
{
|
||||
double mean_accum = 0;
|
||||
double var_accum = 0;
|
||||
double mean;
|
||||
|
||||
for (int i = 0; i < list.size(); i++) {
|
||||
mean_accum += list[i];
|
||||
}
|
||||
mean = mean_accum / list.size();
|
||||
|
||||
for (int i = 0; i < list.size(); i++) {
|
||||
var_accum += (list[i] - mean) * (list[i] - mean);
|
||||
}
|
||||
|
||||
// Use unbiased estimator
|
||||
return var_accum / (list.size() - 1);
|
||||
}
|
||||
|
||||
void ConfigTaskWidget::onAutopilotDisconnect()
|
||||
{
|
||||
|
||||
@ -106,8 +106,6 @@ public:
|
||||
|
||||
void saveObjectToSD(UAVObject *obj);
|
||||
UAVObjectManager *getObjectManager();
|
||||
static double listMean(QList<double> list);
|
||||
static double listVar(QList<double> list);
|
||||
|
||||
void addUAVObject(QString objectName, QList<int> *reloadGroups = NULL);
|
||||
void addUAVObject(UAVObject *objectName, QList<int> *reloadGroups = NULL);
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
<xml>
|
||||
<object name="AttitudeSettings" singleinstance="true" settings="true" category="State">
|
||||
<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="AccelKi" units="channel" type="float" elements="1" defaultvalue="0.0001"/>
|
||||
<field name="MagKi" units="" type="float" elements="1" defaultvalue="0"/>
|
||||
|
||||
@ -2,7 +2,8 @@
|
||||
<object name="RevoCalibration" singleinstance="true" settings="true" category="Sensors">
|
||||
<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_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 -->
|
||||
<!-- TODO: reimplement, put elsewhere (later) -->
|
||||
<field name="BiasCorrectedRaw" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user