Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 01:04:19 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity
Files
f811777789ed9901e7f94d3a1d9f4adfbdd8beb4
PX4-Autopilot/src/modules/commander/calibration_routines.cpp
T
Siddharth Bharat Purohit f811777789 commander: add new math for sphere fit for compass calibration
2017-02-01 08:38:26 +01:00

814 lines
25 KiB
C++
Raw Blame History

/****************************************************************************
*
* Copyright (c) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file calibration_routines.cpp
* Calibration routines implementations.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <px4_defines.h>
#include <px4_posix.h>
#include <px4_time.h>
#include <stdio.h>
#include <unistd.h>
#include <math.h>
#include <float.h>
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <systemlib/mavlink_log.h>
#include <geo/geo.h>
#include <string.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/sensor_combined.h>
#include "calibration_routines.h"
#include "calibration_messages.h"
#include "commander_helper.h"
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z,
float *sphere_radius)
{
float x_sumplain = 0.0f;
float x_sumsq = 0.0f;
float x_sumcube = 0.0f;
float y_sumplain = 0.0f;
float y_sumsq = 0.0f;
float y_sumcube = 0.0f;
float z_sumplain = 0.0f;
float z_sumsq = 0.0f;
float z_sumcube = 0.0f;
float xy_sum = 0.0f;
float xz_sum = 0.0f;
float yz_sum = 0.0f;
float x2y_sum = 0.0f;
float x2z_sum = 0.0f;
float y2x_sum = 0.0f;
float y2z_sum = 0.0f;
float z2x_sum = 0.0f;
float z2y_sum = 0.0f;
for (unsigned int i = 0; i < size; i++) {
float x2 = x[i] * x[i];
float y2 = y[i] * y[i];
float z2 = z[i] * z[i];
x_sumplain += x[i];
x_sumsq += x2;
x_sumcube += x2 * x[i];
y_sumplain += y[i];
y_sumsq += y2;
y_sumcube += y2 * y[i];
z_sumplain += z[i];
z_sumsq += z2;
z_sumcube += z2 * z[i];
xy_sum += x[i] * y[i];
xz_sum += x[i] * z[i];
yz_sum += y[i] * z[i];
x2y_sum += x2 * y[i];
x2z_sum += x2 * z[i];
y2x_sum += y2 * x[i];
y2z_sum += y2 * z[i];
z2x_sum += z2 * x[i];
z2y_sum += z2 * y[i];
}
//
//Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
//
// P is a structure that has been computed with the data earlier.
// P.npoints is the number of elements; the length of X,Y,Z are identical.
// P's members are logically named.
//
// X[n] is the x component of point n
// Y[n] is the y component of point n
// Z[n] is the z component of point n
//
// A is the x coordiante of the sphere
// B is the y coordiante of the sphere
// C is the z coordiante of the sphere
// Rsq is the radius squared of the sphere.
//
//This method should converge; maybe 5-100 iterations or more.
//
float x_sum = x_sumplain / size; //sum( X[n] )
float x_sum2 = x_sumsq / size; //sum( X[n]^2 )
float x_sum3 = x_sumcube / size; //sum( X[n]^3 )
float y_sum = y_sumplain / size; //sum( Y[n] )
float y_sum2 = y_sumsq / size; //sum( Y[n]^2 )
float y_sum3 = y_sumcube / size; //sum( Y[n]^3 )
float z_sum = z_sumplain / size; //sum( Z[n] )
float z_sum2 = z_sumsq / size; //sum( Z[n]^2 )
float z_sum3 = z_sumcube / size; //sum( Z[n]^3 )
float XY = xy_sum / size; //sum( X[n] * Y[n] )
float XZ = xz_sum / size; //sum( X[n] * Z[n] )
float YZ = yz_sum / size; //sum( Y[n] * Z[n] )
float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] )
float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] )
float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] )
float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] )
float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] )
float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] )
//Reduction of multiplications
float F0 = x_sum2 + y_sum2 + z_sum2;
float F1 = 0.5f * F0;
float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
float F4 = -8.0f * (X2Z + Y2Z + z_sum3);
//Set initial conditions:
float A = x_sum;
float B = y_sum;
float C = z_sum;
//First iteration computation:
float A2 = A * A;
float B2 = B * B;
float C2 = C * C;
float QS = A2 + B2 + C2;
float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
//Set initial conditions:
float Rsq = F0 + QB + QS;
//First iteration computation:
float Q0 = 0.5f * (QS - Rsq);
float Q1 = F1 + Q0;
float Q2 = 8.0f * (QS - Rsq + QB + F0);
float aA, aB, aC, nA, nB, nC, dA, dB, dC;
//Iterate N times, ignore stop condition.
unsigned int n = 0;
while (n < max_iterations) {
n++;
//Compute denominator:
aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA;
aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB;
aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC;
//Compute next iteration
nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB);
nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC);
//Check for stop condition
dA = (nA - A);
dB = (nB - B);
dC = (nC - C);
if ((dA * dA + dB * dB + dC * dC) <= delta) { break; }
//Compute next iteration's values
A = nA;
B = nB;
C = nC;
A2 = A * A;
B2 = B * B;
C2 = C * C;
QS = A2 + B2 + C2;
QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
Rsq = F0 + QB + QS;
Q0 = 0.5f * (QS - Rsq);
Q1 = F1 + Q0;
Q2 = 8.0f * (QS - Rsq + QB + F0);
}
*sphere_x = A;
*sphere_y = B;
*sphere_z = C;
*sphere_radius = sqrtf(Rsq);
return 0;
}
int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[],
unsigned int size, unsigned int max_iterations, float delta, float *offset_x, float *offset_y, float *offset_z,
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y, float *offdiag_z)
{
float _fitness = 1.0e30f, _sphere_lambda = 1.0f;
for(int i=0; i < max_iterations; i++) {
//printf("%d, offset: %.6f %.6f %.6f %.6f fitness: %.6f\n", i, (double)*offset_x, (double)*offset_y, (double)*offset_z, (double)*sphere_radius, (double)_fitness);
run_lm_sphere_fit(x, y, z, _fitness, _sphere_lambda,
size, offset_x, offset_y, offset_z,
sphere_radius, diag_x, diag_y, diag_z, offdiag_x, offdiag_y, offdiag_z);
}
return 0;
}
int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda,
unsigned int size, float *offset_x, float *offset_y, float *offset_z,
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y, float *offdiag_z)
{
//Run Sphere Fit using Levenberg Marquardt LSq Fit
const float lma_damping = 10.0f;
float _samples_collected = size;
float fitness = _fitness;
float fit1 = 0.0f, fit2 = 0.0f;
float JTJ[16];
float JTJ2[16];
float JTFI[4];
float residual = 0.0f;
memset(&JTJ,0,sizeof(JTJ));
memset(&JTJ2,0,sizeof(JTJ2));
memset(&JTFI,0,sizeof(JTFI));
// Gauss Newton Part common for all kind of extensions including LM
for(uint16_t k = 0; k<_samples_collected; k++) {
float sphere_jacob[4];
//Calculate Jacobian
float A = (*diag_x * (x[k] + *offset_x)) + (*offdiag_x * (y[k] + *offset_y)) + (*offdiag_y * (z[k] + *offset_z));
float B = (*offdiag_x * (x[k] + *offset_x)) + (*diag_y * (y[k] + *offset_y)) + (*offdiag_z * (z[k] + *offset_z));
float C = (*offdiag_y * (x[k] + *offset_x)) + (*offdiag_z * (y[k] + *offset_y)) + (*diag_z * (z[k] + *offset_z));
float length = sqrtf(A*A + B*B + C*C);
// 0: partial derivative (radius wrt fitness fn) fn operated on sample
sphere_jacob[0] = 1.0f;
// 1-3: partial derivative (offsets wrt fitness fn) fn operated on sample
sphere_jacob[1] = -1.0f * (((*diag_x * A) + (*offdiag_x * B) + (*offdiag_y * C))/length);
sphere_jacob[2] = -1.0f * (((*offdiag_x * A) + (*diag_y * B) + (*offdiag_z * C))/length);
sphere_jacob[3] = -1.0f * (((*offdiag_y * A) + (*offdiag_z * B) + (*diag_z * C))/length);
residual = *sphere_radius - length;
for(uint8_t i = 0;i < 4; i++) {
// compute JTJ
for(uint8_t j = 0; j < 4; j++) {
JTJ[i*4 + j] += sphere_jacob[i] * sphere_jacob[j];
JTJ2[i*4 + j] += sphere_jacob[i] * sphere_jacob[j]; //a backup JTJ for LM
}
JTFI[i] += sphere_jacob[i] * residual;
}
}
//------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
//refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
float fit1_params[4] = {*sphere_radius, *offset_x, *offset_y, *offset_z};
float fit2_params[4];
memcpy(fit2_params,fit1_params,sizeof(fit1_params));
for(uint8_t i = 0; i < 4; i++) {
JTJ[i*4 + i] += _sphere_lambda;
JTJ2[i*4 + i] += _sphere_lambda/lma_damping;
}
if(!inverse4x4(JTJ, JTJ)) {
return -1;
}
if(!inverse4x4(JTJ2, JTJ2)) {
return -1;
}
for(uint8_t row=0; row < 4; row++) {
for(uint8_t col=0; col < 4; col++) {
fit1_params[row] -= JTFI[col] * JTJ[row*4 + col];
fit2_params[row] -= JTFI[col] * JTJ2[row*4 + col];
}
}
//Calculate mean squared residuals
for(uint16_t k=0; k < _samples_collected; k++){
float A = (*diag_x * (x[k] + fit1_params[1])) + (*offdiag_x * (y[k] + fit1_params[2])) + (*offdiag_y * (z[k] + fit1_params[3]));
float B = (*offdiag_x * (x[k] + fit1_params[1])) + (*diag_y * (y[k] + fit1_params[2])) + (*offdiag_z * (z[k] + fit1_params[3]));
float C = (*offdiag_y * (x[k] + fit1_params[1])) + (*offdiag_z * (y[k] + fit1_params[2])) + (*diag_z * (z[k] + fit1_params[3]));
float length = sqrtf(A*A + B*B + C*C);
residual = fit1_params[0] - length;
fit1 += residual*residual;
A = (*diag_x * (x[k] + fit2_params[1])) + (*offdiag_x * (y[k] + fit2_params[2])) + (*offdiag_y * (z[k] + fit2_params[3]));
B = (*offdiag_x * (x[k] + fit2_params[1])) + (*diag_y * (y[k] + fit2_params[2])) + (*offdiag_z * (z[k] + fit2_params[3]));
C = (*offdiag_y * (x[k] + fit2_params[1])) + (*offdiag_z * (y[k] + fit2_params[2])) + (*diag_z * (z[k] + fit2_params[3]));
length = sqrtf(A*A + B*B + C*C);
residual = fit2_params[0] - length;
fit2 += residual*residual;
}
fit1 = sqrtf(fit1)/_samples_collected;
fit2 = sqrtf(fit2)/_samples_collected;
if(fit1 > _fitness && fit2 > _fitness){
_sphere_lambda *= lma_damping;
} else if(fit2 < _fitness && fit2 < fit1) {
_sphere_lambda /= lma_damping;
memcpy(fit1_params,fit2_params,sizeof(fit1_params));
fitness = fit2;
} else if(fit1 < _fitness){
fitness = fit1;
}
//--------------------Levenberg-Marquardt-part-ends-here--------------------------------//
if(!isnan(fitness) && fitness < _fitness) {
_fitness = fitness;
*sphere_radius = fit1_params[0];
*offset_x = fit1_params[1];
*offset_y = fit1_params[2];
*offset_z = fit1_params[3];
return 0;
} else {
return -1;
}
}
bool inverse4x4(float m[],float invOut[])
{
float inv[16], det;
uint8_t i;
inv[0] = m[5] * m[10] * m[15] -
m[5] * m[11] * m[14] -
m[9] * m[6] * m[15] +
m[9] * m[7] * m[14] +
m[13] * m[6] * m[11] -
m[13] * m[7] * m[10];
inv[4] = -m[4] * m[10] * m[15] +
m[4] * m[11] * m[14] +
m[8] * m[6] * m[15] -
m[8] * m[7] * m[14] -
m[12] * m[6] * m[11] +
m[12] * m[7] * m[10];
inv[8] = m[4] * m[9] * m[15] -
m[4] * m[11] * m[13] -
m[8] * m[5] * m[15] +
m[8] * m[7] * m[13] +
m[12] * m[5] * m[11] -
m[12] * m[7] * m[9];
inv[12] = -m[4] * m[9] * m[14] +
m[4] * m[10] * m[13] +
m[8] * m[5] * m[14] -
m[8] * m[6] * m[13] -
m[12] * m[5] * m[10] +
m[12] * m[6] * m[9];
inv[1] = -m[1] * m[10] * m[15] +
m[1] * m[11] * m[14] +
m[9] * m[2] * m[15] -
m[9] * m[3] * m[14] -
m[13] * m[2] * m[11] +
m[13] * m[3] * m[10];
inv[5] = m[0] * m[10] * m[15] -
m[0] * m[11] * m[14] -
m[8] * m[2] * m[15] +
m[8] * m[3] * m[14] +
m[12] * m[2] * m[11] -
m[12] * m[3] * m[10];
inv[9] = -m[0] * m[9] * m[15] +
m[0] * m[11] * m[13] +
m[8] * m[1] * m[15] -
m[8] * m[3] * m[13] -
m[12] * m[1] * m[11] +
m[12] * m[3] * m[9];
inv[13] = m[0] * m[9] * m[14] -
m[0] * m[10] * m[13] -
m[8] * m[1] * m[14] +
m[8] * m[2] * m[13] +
m[12] * m[1] * m[10] -
m[12] * m[2] * m[9];
inv[2] = m[1] * m[6] * m[15] -
m[1] * m[7] * m[14] -
m[5] * m[2] * m[15] +
m[5] * m[3] * m[14] +
m[13] * m[2] * m[7] -
m[13] * m[3] * m[6];
inv[6] = -m[0] * m[6] * m[15] +
m[0] * m[7] * m[14] +
m[4] * m[2] * m[15] -
m[4] * m[3] * m[14] -
m[12] * m[2] * m[7] +
m[12] * m[3] * m[6];
inv[10] = m[0] * m[5] * m[15] -
m[0] * m[7] * m[13] -
m[4] * m[1] * m[15] +
m[4] * m[3] * m[13] +
m[12] * m[1] * m[7] -
m[12] * m[3] * m[5];
inv[14] = -m[0] * m[5] * m[14] +
m[0] * m[6] * m[13] +
m[4] * m[1] * m[14] -
m[4] * m[2] * m[13] -
m[12] * m[1] * m[6] +
m[12] * m[2] * m[5];
inv[3] = -m[1] * m[6] * m[11] +
m[1] * m[7] * m[10] +
m[5] * m[2] * m[11] -
m[5] * m[3] * m[10] -
m[9] * m[2] * m[7] +
m[9] * m[3] * m[6];
inv[7] = m[0] * m[6] * m[11] -
m[0] * m[7] * m[10] -
m[4] * m[2] * m[11] +
m[4] * m[3] * m[10] +
m[8] * m[2] * m[7] -
m[8] * m[3] * m[6];
inv[11] = -m[0] * m[5] * m[11] +
m[0] * m[7] * m[9] +
m[4] * m[1] * m[11] -
m[4] * m[3] * m[9] -
m[8] * m[1] * m[7] +
m[8] * m[3] * m[5];
inv[15] = m[0] * m[5] * m[10] -
m[0] * m[6] * m[9] -
m[4] * m[1] * m[10] +
m[4] * m[2] * m[9] +
m[8] * m[1] * m[6] -
m[8] * m[2] * m[5];
det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
if(fabsf(det) < 1.1755e-38f){
return false;
}
det = 1.0f / det;
for (i = 0; i < 16; i++)
invOut[i] = inv[i] * det;
return true;
}
enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, int cancel_sub, int accel_sub, bool lenient_still_position)
{
const unsigned ndim = 3;
struct sensor_combined_s sensor;
float accel_ema[ndim] = { 0.0f }; // exponential moving average of accel
float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; // max-hold dispersion of accel
float ema_len = 0.5f; // EMA time constant in seconds
const float normal_still_thr = 0.25; // normal still threshold
float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
float accel_err_thr = 5.0f; // set accel error threshold to 5m/s^2
hrt_abstime still_time = lenient_still_position ? 500000 : 1300000; // still time required in us
px4_pollfd_struct_t fds[1];
fds[0].fd = accel_sub;
fds[0].events = POLLIN;
hrt_abstime t_start = hrt_absolute_time();
/* set timeout to 30s */
hrt_abstime timeout = 30000000;
hrt_abstime t_timeout = t_start + timeout;
hrt_abstime t = t_start;
hrt_abstime t_prev = t_start;
hrt_abstime t_still = 0;
unsigned poll_errcount = 0;
while (true) {
/* wait blocking for new data */
int poll_ret = px4_poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(sensor_combined), accel_sub, &sensor);
t = hrt_absolute_time();
float dt = (t - t_prev) / 1000000.0f;
t_prev = t;
float w = dt / ema_len;
for (unsigned i = 0; i < ndim; i++) {
float di = sensor.accelerometer_m_s2[i];
float d = di - accel_ema[i];
accel_ema[i] += d * w;
d = d * d;
accel_disp[i] = accel_disp[i] * (1.0f - w);
if (d > still_thr2 * 8.0f) {
d = still_thr2 * 8.0f;
}
if (d > accel_disp[i]) {
accel_disp[i] = d;
}
}
/* still detector with hysteresis */
if (accel_disp[0] < still_thr2 &&
accel_disp[1] < still_thr2 &&
accel_disp[2] < still_thr2) {
/* is still now */
if (t_still == 0) {
/* first time */
calibration_log_info(mavlink_log_pub, "[cal] detected rest position, hold still...");
t_still = t;
t_timeout = t + timeout;
} else {
/* still since t_still */
if (t > t_still + still_time) {
/* vehicle is still, exit from the loop to detection of its orientation */
break;
}
}
} else if (accel_disp[0] > still_thr2 * 4.0f ||
accel_disp[1] > still_thr2 * 4.0f ||
accel_disp[2] > still_thr2 * 4.0f) {
/* not still, reset still start time */
if (t_still != 0) {
calibration_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
usleep(200000);
t_still = 0;
}
}
} else if (poll_ret == 0) {
poll_errcount++;
}
if (t > t_timeout) {
poll_errcount++;
}
if (poll_errcount > 1000) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
return DETECT_ORIENTATION_ERROR;
}
}
if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr) {
return DETECT_ORIENTATION_TAIL_DOWN; // [ g, 0, 0 ]
}
if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr) {
return DETECT_ORIENTATION_NOSE_DOWN; // [ -g, 0, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr) {
return DETECT_ORIENTATION_LEFT; // [ 0, g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr) {
return DETECT_ORIENTATION_RIGHT; // [ 0, -g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
return DETECT_ORIENTATION_UPSIDE_DOWN; // [ 0, 0, g ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
return DETECT_ORIENTATION_RIGHTSIDE_UP; // [ 0, 0, -g ]
}
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: invalid orientation");
return DETECT_ORIENTATION_ERROR; // Can't detect orientation
}
const char* detect_orientation_str(enum detect_orientation_return orientation)
{
static const char* rgOrientationStrs[] = {
"back", // tail down
"front", // nose down
"left",
"right",
"up", // upside-down
"down", // right-side up
"error"
};
return rgOrientationStrs[orientation];
}
calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
int cancel_sub,
bool side_data_collected[detect_orientation_side_count],
calibration_from_orientation_worker_t calibration_worker,
void* worker_data,
bool lenient_still_position)
{
calibrate_return result = calibrate_return_ok;
// Setup subscriptions to onboard accel sensor
int sub_accel = orb_subscribe(ORB_ID(sensor_combined));
if (sub_accel < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "No onboard accel");
return calibrate_return_error;
}
unsigned orientation_failures = 0;
// Rotate through all requested orientation
while (true) {
if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) {
result = calibrate_return_cancelled;
break;
}
if (orientation_failures > 4) {
result = calibrate_return_error;
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "timeout: no motion");
break;
}
unsigned int side_complete_count = 0;
// Update the number of completed sides
for (unsigned i = 0; i < detect_orientation_side_count; i++) {
if (side_data_collected[i]) {
side_complete_count++;
}
}
if (side_complete_count == detect_orientation_side_count) {
// We have completed all sides, move on
break;
}
/* inform user which orientations are still needed */
char pendingStr[80];
pendingStr[0] = 0;
for (unsigned int cur_orientation=0; cur_orientation<detect_orientation_side_count; cur_orientation++) {
if (!side_data_collected[cur_orientation]) {
strncat(pendingStr, " ", sizeof(pendingStr) - 1);
strncat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation), sizeof(pendingStr) - 1);
}
}
calibration_log_info(mavlink_log_pub, "[cal] pending:%s", pendingStr);
usleep(20000);
calibration_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side");
usleep(20000);
enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, cancel_sub, sub_accel, lenient_still_position);
if (orient == DETECT_ORIENTATION_ERROR) {
orientation_failures++;
calibration_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
usleep(20000);
continue;
}
/* inform user about already handled side */
if (side_data_collected[orient]) {
orientation_failures++;
calibration_log_info(mavlink_log_pub, "[cal] %s side already completed", detect_orientation_str(orient));
usleep(20000);
continue;
}
calibration_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
usleep(20000);
calibration_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
usleep(20000);
orientation_failures = 0;
// Call worker routine
result = calibration_worker(orient, cancel_sub, worker_data);
if (result != calibrate_return_ok ) {
break;
}
calibration_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
usleep(20000);
calibration_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
usleep(20000);
// Note that this side is complete
side_data_collected[orient] = true;
tune_neutral(true);
usleep(200000);
}
if (sub_accel >= 0) {
px4_close(sub_accel);
}
return result;
}
int calibrate_cancel_subscribe(void)
{
return orb_subscribe(ORB_ID(vehicle_command));
}
void calibrate_cancel_unsubscribe(int cmd_sub)
{
orb_unsubscribe(cmd_sub);
}
static void calibrate_answer_command(orb_advert_t *mavlink_log_pub, struct vehicle_command_s &cmd, unsigned result)
{
switch (result) {
case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED:
tune_positive(true);
break;
case vehicle_command_s::VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(mavlink_log_pub, "command denied during calibration: %u", cmd.command);
tune_negative(true);
break;
default:
break;
}
}
bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, int cancel_sub)
{
px4_pollfd_struct_t fds[1];
fds[0].fd = cancel_sub;
fds[0].events = POLLIN;
if (px4_poll(&fds[0], 1, 0) > 0) {
struct vehicle_command_s cmd;
memset(&cmd, 0, sizeof(cmd));
orb_copy(ORB_ID(vehicle_command), cancel_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION &&
(int)cmd.param1 == 0 &&
(int)cmd.param2 == 0 &&
(int)cmd.param3 == 0 &&
(int)cmd.param4 == 0 &&
(int)cmd.param5 == 0 &&
(int)cmd.param6 == 0) {
calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
mavlink_log_critical(mavlink_log_pub, CAL_QGC_CANCELLED_MSG);
return true;
} else {
calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
}
}
return false;
}
Reference in New Issue View Git Blame Copy Permalink