forked from Archive/PX4-Autopilot
Checkpoint: Working, but non-verified full mag calibration
This commit is contained in:
parent
28171fb596
commit
096bf2dc93
|
@ -0,0 +1,175 @@
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
#include "calibration_routines.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.
|
||||||
|
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 = (aA == 0.0f) ? 1.0f : aA;
|
||||||
|
aB = (aB == 0.0f) ? 1.0f : aB;
|
||||||
|
aC = (aC == 0.0f) ? 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;
|
||||||
|
}
|
|
@ -0,0 +1,21 @@
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Least-squares fit of a sphere to a set of points.
|
||||||
|
*
|
||||||
|
* Fits a sphere to a set of points on the sphere surface.
|
||||||
|
*
|
||||||
|
* @param x point coordinates on the X axis
|
||||||
|
* @param y point coordinates on the Y axis
|
||||||
|
* @param z point coordinates on the Z axis
|
||||||
|
* @param size number of points
|
||||||
|
* @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100.
|
||||||
|
* @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times.
|
||||||
|
* @param sphere_x coordinate of the sphere center on the X axis
|
||||||
|
* @param sphere_y coordinate of the sphere center on the Y axis
|
||||||
|
* @param sphere_z coordinate of the sphere center on the Z axis
|
||||||
|
* @param sphere_radius sphere radius
|
||||||
|
*
|
||||||
|
* @return 0 on success, 1 on failure
|
||||||
|
*/
|
||||||
|
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);
|
|
@ -83,6 +83,8 @@
|
||||||
#include <drivers/drv_mag.h>
|
#include <drivers/drv_mag.h>
|
||||||
#include <drivers/drv_baro.h>
|
#include <drivers/drv_baro.h>
|
||||||
|
|
||||||
|
#include "calibration_routines.h"
|
||||||
|
|
||||||
|
|
||||||
PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */
|
PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */
|
||||||
//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */
|
//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */
|
||||||
|
@ -288,8 +290,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
||||||
status->flag_preflight_mag_calibration = true;
|
status->flag_preflight_mag_calibration = true;
|
||||||
state_machine_publish(status_pub, status, mavlink_fd);
|
state_machine_publish(status_pub, status, mavlink_fd);
|
||||||
|
|
||||||
int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
|
int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
|
||||||
struct sensor_combined_s raw;
|
orb_set_interval(sub_mag, 22);
|
||||||
|
struct mag_report mag;
|
||||||
|
|
||||||
/* 30 seconds */
|
/* 30 seconds */
|
||||||
uint64_t calibration_interval = 45 * 1000 * 1000;
|
uint64_t calibration_interval = 45 * 1000 * 1000;
|
||||||
|
@ -306,8 +309,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
||||||
|
|
||||||
int fd = open(MAG_DEVICE_PATH, 0);
|
int fd = open(MAG_DEVICE_PATH, 0);
|
||||||
|
|
||||||
|
/* erase old calibration */
|
||||||
|
|
||||||
struct mag_scale mscale_null = {
|
struct mag_scale mscale_null = {
|
||||||
0.0f,
|
0.0f,
|
||||||
1.0f,
|
1.0f,
|
||||||
|
@ -321,8 +323,15 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
||||||
mavlink_log_info(mavlink_fd, "[commander] failed to set scale / offsets for mag");
|
mavlink_log_info(mavlink_fd, "[commander] failed to set scale / offsets for mag");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* calibrate range */
|
||||||
|
if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
|
||||||
|
warnx("failed to calibrate scale");
|
||||||
|
}
|
||||||
|
|
||||||
close(fd);
|
close(fd);
|
||||||
|
|
||||||
|
/* calibrate offsets */
|
||||||
|
|
||||||
uint64_t calibration_start = hrt_absolute_time();
|
uint64_t calibration_start = hrt_absolute_time();
|
||||||
|
|
||||||
uint64_t axis_deadline = hrt_absolute_time();
|
uint64_t axis_deadline = hrt_absolute_time();
|
||||||
|
@ -331,10 +340,16 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
||||||
const char axislabels[3] = { 'Z', 'X', 'Y'};
|
const char axislabels[3] = { 'Z', 'X', 'Y'};
|
||||||
int axis_index = -1;
|
int axis_index = -1;
|
||||||
|
|
||||||
while (hrt_absolute_time() < calibration_deadline) {
|
const int calibration_maxcount = 2000;
|
||||||
|
float *x = malloc(sizeof(float) * calibration_maxcount);
|
||||||
|
float *y = malloc(sizeof(float) * calibration_maxcount);
|
||||||
|
float *z = malloc(sizeof(float) * calibration_maxcount);
|
||||||
|
|
||||||
|
while (hrt_absolute_time() < calibration_deadline &&
|
||||||
|
calibration_counter < calibration_maxcount) {
|
||||||
|
|
||||||
/* wait blocking for new data */
|
/* wait blocking for new data */
|
||||||
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
|
struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } };
|
||||||
|
|
||||||
/* user guidance */
|
/* user guidance */
|
||||||
if (hrt_absolute_time() >= axis_deadline &&
|
if (hrt_absolute_time() >= axis_deadline &&
|
||||||
|
@ -363,30 +378,34 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
||||||
// }
|
// }
|
||||||
|
|
||||||
if (poll(fds, 1, 1000)) {
|
if (poll(fds, 1, 1000)) {
|
||||||
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
|
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
|
||||||
|
|
||||||
|
x[calibration_counter] = mag.x;
|
||||||
|
y[calibration_counter] = mag.y;
|
||||||
|
z[calibration_counter] = mag.z;
|
||||||
|
|
||||||
/* get min/max values */
|
/* get min/max values */
|
||||||
|
|
||||||
/* ignore other axes */
|
// if (mag.x < mag_min[0]) {
|
||||||
if (raw.magnetometer_ga[0] < mag_min[0]) {
|
// mag_min[0] = mag.x;
|
||||||
mag_min[0] = raw.magnetometer_ga[0];
|
// }
|
||||||
}
|
// else if (mag.x > mag_max[0]) {
|
||||||
else if (raw.magnetometer_ga[0] > mag_max[0]) {
|
// mag_max[0] = mag.x;
|
||||||
mag_max[0] = raw.magnetometer_ga[0];
|
// }
|
||||||
}
|
|
||||||
|
|
||||||
if (raw.magnetometer_ga[1] < mag_min[1]) {
|
// if (raw.magnetometer_ga[1] < mag_min[1]) {
|
||||||
mag_min[1] = raw.magnetometer_ga[1];
|
// mag_min[1] = raw.magnetometer_ga[1];
|
||||||
}
|
// }
|
||||||
else if (raw.magnetometer_ga[1] > mag_max[1]) {
|
// else if (raw.magnetometer_ga[1] > mag_max[1]) {
|
||||||
mag_max[1] = raw.magnetometer_ga[1];
|
// mag_max[1] = raw.magnetometer_ga[1];
|
||||||
}
|
// }
|
||||||
|
|
||||||
if (raw.magnetometer_ga[2] < mag_min[2]) {
|
// if (raw.magnetometer_ga[2] < mag_min[2]) {
|
||||||
mag_min[2] = raw.magnetometer_ga[2];
|
// mag_min[2] = raw.magnetometer_ga[2];
|
||||||
}
|
// }
|
||||||
else if (raw.magnetometer_ga[2] > mag_max[2]) {
|
// else if (raw.magnetometer_ga[2] > mag_max[2]) {
|
||||||
mag_max[2] = raw.magnetometer_ga[2];
|
// mag_max[2] = raw.magnetometer_ga[2];
|
||||||
}
|
// }
|
||||||
|
|
||||||
calibration_counter++;
|
calibration_counter++;
|
||||||
} else {
|
} else {
|
||||||
|
@ -396,77 +415,90 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* disable calibration mode */
|
float sphere_x;
|
||||||
status->flag_preflight_mag_calibration = false;
|
float sphere_y;
|
||||||
state_machine_publish(status_pub, status, mavlink_fd);
|
float sphere_z;
|
||||||
|
float sphere_radius;
|
||||||
|
|
||||||
float mag_offset[3];
|
sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
|
||||||
|
|
||||||
/**
|
free(x);
|
||||||
* The offset is subtracted from the sensor values, so the result is the
|
free(y);
|
||||||
* POSITIVE number that has to be subtracted from the sensor data
|
free(z);
|
||||||
* to shift the center to zero
|
|
||||||
*
|
|
||||||
* offset = max - ((max - min) / 2.0f)
|
|
||||||
* max - max/2 + min/2
|
|
||||||
* max/2 + min/2
|
|
||||||
*
|
|
||||||
* which reduces to
|
|
||||||
*
|
|
||||||
* offset = (max + min) / 2.0f
|
|
||||||
*/
|
|
||||||
|
|
||||||
mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f;
|
float mag_offset[3] = {sphere_x, sphere_y, sphere_z};
|
||||||
mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f;
|
|
||||||
mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f;
|
// *
|
||||||
|
// * The offset is subtracted from the sensor values, so the result is the
|
||||||
|
// * POSITIVE number that has to be subtracted from the sensor data
|
||||||
|
// * to shift the center to zero
|
||||||
|
// *
|
||||||
|
// * offset = max - ((max - min) / 2.0f)
|
||||||
|
// * max - max/2 + min/2
|
||||||
|
// * max/2 + min/2
|
||||||
|
// *
|
||||||
|
// * which reduces to
|
||||||
|
// *
|
||||||
|
// * offset = (max + min) / 2.0f
|
||||||
|
|
||||||
|
|
||||||
|
// mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f;
|
||||||
|
// mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f;
|
||||||
|
// mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f;
|
||||||
|
|
||||||
if (isfinite(mag_offset[0]) && isfinite(mag_offset[1]) && isfinite(mag_offset[2])) {
|
if (isfinite(mag_offset[0]) && isfinite(mag_offset[1]) && isfinite(mag_offset[2])) {
|
||||||
|
|
||||||
/* announce and set new offset */
|
|
||||||
|
|
||||||
if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) {
|
|
||||||
fprintf(stderr, "[commander] Setting X mag offset failed!\n");
|
|
||||||
}
|
|
||||||
|
|
||||||
if (param_set(param_find("SENS_MAG_YOFF"), &(mag_offset[1]))) {
|
|
||||||
fprintf(stderr, "[commander] Setting Y mag offset failed!\n");
|
|
||||||
}
|
|
||||||
|
|
||||||
if (param_set(param_find("SENS_MAG_ZOFF"), &(mag_offset[2]))) {
|
|
||||||
fprintf(stderr, "[commander] Setting Z mag offset failed!\n");
|
|
||||||
}
|
|
||||||
|
|
||||||
if (param_set(param_find("SENS_MAG_XSCALE"), &(mag_offset[0]))) {
|
|
||||||
fprintf(stderr, "[commander] Setting X mag offset failed!\n");
|
|
||||||
}
|
|
||||||
|
|
||||||
if (param_set(param_find("SENS_MAG_YSCALE"), &(mag_offset[1]))) {
|
|
||||||
fprintf(stderr, "[commander] Setting Y mag offset failed!\n");
|
|
||||||
}
|
|
||||||
|
|
||||||
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mag_offset[2]))) {
|
|
||||||
fprintf(stderr, "[commander] Setting Z mag offset failed!\n");
|
|
||||||
}
|
|
||||||
|
|
||||||
fd = open(MAG_DEVICE_PATH, 0);
|
fd = open(MAG_DEVICE_PATH, 0);
|
||||||
struct mag_scale mscale = {
|
|
||||||
mag_offset[0],
|
struct mag_scale mscale;
|
||||||
1.0f,
|
|
||||||
mag_offset[1],
|
if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
|
||||||
1.0f,
|
warn("WARNING: failed to get scale / offsets for mag");
|
||||||
mag_offset[2],
|
|
||||||
1.0f,
|
mscale.x_offset = mag_offset[0];
|
||||||
};
|
mscale.y_offset = mag_offset[1];
|
||||||
|
mscale.z_offset = mag_offset[2];
|
||||||
|
|
||||||
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
|
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
|
||||||
warn("WARNING: failed to set scale / offsets for mag");
|
warn("WARNING: failed to set scale / offsets for mag");
|
||||||
close(fd);
|
close(fd);
|
||||||
|
|
||||||
|
/* announce and set new offset */
|
||||||
|
|
||||||
|
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
|
||||||
|
fprintf(stderr, "[commander] Setting X mag offset failed!\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
|
||||||
|
fprintf(stderr, "[commander] Setting Y mag offset failed!\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
|
||||||
|
fprintf(stderr, "[commander] Setting Z mag offset failed!\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
|
||||||
|
fprintf(stderr, "[commander] Setting X mag scale failed!\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
|
||||||
|
fprintf(stderr, "[commander] Setting Y mag scale failed!\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
|
||||||
|
fprintf(stderr, "[commander] Setting Z mag scale failed!\n");
|
||||||
|
}
|
||||||
|
|
||||||
/* auto-save to EEPROM */
|
/* auto-save to EEPROM */
|
||||||
int save_ret = pm_save_eeprom(false);
|
int save_ret = pm_save_eeprom(false);
|
||||||
if(save_ret != 0) {
|
if(save_ret != 0) {
|
||||||
warn("WARNING: auto-save of params to EEPROM failed");
|
warn("WARNING: auto-save of params to EEPROM failed");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
printf("[mag cal] scale: %.6f %.6f %.6f\n\t\toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
|
||||||
|
mscale.x_scale, mscale.y_scale, mscale.z_scale,
|
||||||
|
mscale.x_offset, mscale.y_offset, mscale.z_offset, sphere_radius);
|
||||||
|
|
||||||
// char buf[50];
|
// char buf[50];
|
||||||
// sprintf(buf, "[commander] mag cal: x:%d y:%d z:%d mGa", (int)(mag_offset[0]*1000), (int)(mag_offset[1]*1000), (int)(mag_offset[2]*1000));
|
// sprintf(buf, "[commander] mag cal: x:%d y:%d z:%d mGa", (int)(mag_offset[0]*1000), (int)(mag_offset[1]*1000), (int)(mag_offset[2]*1000));
|
||||||
// mavlink_log_info(mavlink_fd, buf);
|
// mavlink_log_info(mavlink_fd, buf);
|
||||||
|
@ -475,7 +507,11 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
||||||
mavlink_log_info(mavlink_fd, "[commander] mag calibration FAILED (NaN)");
|
mavlink_log_info(mavlink_fd, "[commander] mag calibration FAILED (NaN)");
|
||||||
}
|
}
|
||||||
|
|
||||||
close(sub_sensor_combined);
|
/* disable calibration mode */
|
||||||
|
status->flag_preflight_mag_calibration = false;
|
||||||
|
state_machine_publish(status_pub, status, mavlink_fd);
|
||||||
|
|
||||||
|
close(sub_mag);
|
||||||
}
|
}
|
||||||
|
|
||||||
void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
|
void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
|
||||||
|
|
|
@ -184,7 +184,7 @@ private:
|
||||||
*
|
*
|
||||||
* @param enable set to 1 to enable self-test strap, 0 to disable
|
* @param enable set to 1 to enable self-test strap, 0 to disable
|
||||||
*/
|
*/
|
||||||
int calibrate(unsigned enable);
|
int calibrate(struct file *filp, unsigned enable);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Perform the on-sensor scale calibration routine.
|
* Perform the on-sensor scale calibration routine.
|
||||||
|
@ -603,7 +603,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
return 0;
|
return 0;
|
||||||
|
|
||||||
case MAGIOCCALIBRATE:
|
case MAGIOCCALIBRATE:
|
||||||
return calibrate(arg);
|
return calibrate(filp, arg);
|
||||||
|
|
||||||
case MAGIOCEXSTRAP:
|
case MAGIOCEXSTRAP:
|
||||||
return set_excitement(arg);
|
return set_excitement(arg);
|
||||||
|
@ -813,41 +813,15 @@ out:
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
int HMC5883::calibrate(unsigned enable)
|
int HMC5883::calibrate(struct file *filp, unsigned enable)
|
||||||
{
|
{
|
||||||
struct mag_report report;
|
struct mag_report report;
|
||||||
ssize_t sz;
|
ssize_t sz;
|
||||||
int ret;
|
int ret = 1;
|
||||||
|
|
||||||
|
// XXX do something smarter here
|
||||||
int fd = (int)enable;
|
int fd = (int)enable;
|
||||||
|
|
||||||
/* do a simple demand read */
|
|
||||||
sz = read(fd, &report, sizeof(report));
|
|
||||||
if (sz != sizeof(report))
|
|
||||||
err(1, "immediate read failed");
|
|
||||||
|
|
||||||
warnx("starting mag scale calibration");
|
|
||||||
warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
|
|
||||||
warnx("time: %lld", report.timestamp);
|
|
||||||
warnx("sampling 500 samples for scaling offset");
|
|
||||||
|
|
||||||
/* set the queue depth to 10 */
|
|
||||||
if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
|
|
||||||
errx(1, "failed to set queue depth");
|
|
||||||
|
|
||||||
/* start the sensor polling at 50 Hz */
|
|
||||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 50))
|
|
||||||
errx(1, "failed to set 2Hz poll rate");
|
|
||||||
|
|
||||||
/* Set to 2.5 Gauss */
|
|
||||||
if (OK != ioctl(fd, MAGIOCSRANGE, 2)) {
|
|
||||||
warnx("failed to set 2.5 Ga range");
|
|
||||||
}
|
|
||||||
|
|
||||||
if (OK != ioctl(fd, MAGIOCPOSEX, 1)) {
|
|
||||||
warnx("failed to enable sensor calibration mode");
|
|
||||||
}
|
|
||||||
|
|
||||||
struct mag_scale mscale_previous = {
|
struct mag_scale mscale_previous = {
|
||||||
0.0f,
|
0.0f,
|
||||||
1.0f,
|
1.0f,
|
||||||
|
@ -857,10 +831,6 @@ int HMC5883::calibrate(unsigned enable)
|
||||||
1.0f,
|
1.0f,
|
||||||
};
|
};
|
||||||
|
|
||||||
if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
|
|
||||||
warn("WARNING: failed to get scale / offsets for mag");
|
|
||||||
}
|
|
||||||
|
|
||||||
struct mag_scale mscale_null = {
|
struct mag_scale mscale_null = {
|
||||||
0.0f,
|
0.0f,
|
||||||
1.0f,
|
1.0f,
|
||||||
|
@ -870,12 +840,61 @@ int HMC5883::calibrate(unsigned enable)
|
||||||
1.0f,
|
1.0f,
|
||||||
};
|
};
|
||||||
|
|
||||||
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
|
float avg_excited[3] = {0.0f, 0.0f, 0.0f};
|
||||||
warn("WARNING: failed to set null scale / offsets for mag");
|
unsigned i;
|
||||||
|
|
||||||
|
warnx("starting mag scale calibration");
|
||||||
|
|
||||||
|
/* do a simple demand read */
|
||||||
|
sz = read(filp, (char*)&report, sizeof(report));
|
||||||
|
if (sz != sizeof(report)) {
|
||||||
|
warn("immediate read failed");
|
||||||
|
ret = 1;
|
||||||
|
goto out;
|
||||||
}
|
}
|
||||||
|
|
||||||
float avg_excited[3];
|
warnx("current measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
|
||||||
unsigned i;
|
warnx("time: %lld", report.timestamp);
|
||||||
|
warnx("sampling 500 samples for scaling offset");
|
||||||
|
|
||||||
|
/* set the queue depth to 10 */
|
||||||
|
if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) {
|
||||||
|
warn("failed to set queue depth");
|
||||||
|
ret = 1;
|
||||||
|
goto out;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* start the sensor polling at 50 Hz */
|
||||||
|
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
|
||||||
|
warn("failed to set 2Hz poll rate");
|
||||||
|
ret = 1;
|
||||||
|
goto out;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Set to 2.5 Gauss */
|
||||||
|
if (OK != ioctl(filp, MAGIOCSRANGE, 2)) {
|
||||||
|
warnx("failed to set 2.5 Ga range");
|
||||||
|
ret = 1;
|
||||||
|
goto out;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
|
||||||
|
warnx("failed to enable sensor calibration mode");
|
||||||
|
ret = 1;
|
||||||
|
goto out;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
|
||||||
|
warn("WARNING: failed to get scale / offsets for mag");
|
||||||
|
ret = 1;
|
||||||
|
goto out;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
|
||||||
|
warn("WARNING: failed to set null scale / offsets for mag");
|
||||||
|
ret = 1;
|
||||||
|
goto out;
|
||||||
|
}
|
||||||
|
|
||||||
/* read the sensor 10x and report each value */
|
/* read the sensor 10x and report each value */
|
||||||
for (i = 0; i < 500; i++) {
|
for (i = 0; i < 500; i++) {
|
||||||
|
@ -884,56 +903,56 @@ int HMC5883::calibrate(unsigned enable)
|
||||||
/* wait for data to be ready */
|
/* wait for data to be ready */
|
||||||
fds.fd = fd;
|
fds.fd = fd;
|
||||||
fds.events = POLLIN;
|
fds.events = POLLIN;
|
||||||
ret = poll(&fds, 1, 2000);
|
ret = ::poll(&fds, 1, 2000);
|
||||||
|
|
||||||
if (ret != 1)
|
if (ret != 1) {
|
||||||
errx(1, "timed out waiting for sensor data");
|
warn("timed out waiting for sensor data");
|
||||||
|
goto out;
|
||||||
|
}
|
||||||
|
|
||||||
/* now go get it */
|
/* now go get it */
|
||||||
sz = read(fd, &report, sizeof(report));
|
sz = ::read(fd, &report, sizeof(report));
|
||||||
|
|
||||||
if (sz != sizeof(report)) {
|
if (sz != sizeof(report)) {
|
||||||
err(1, "periodic read failed");
|
warn("periodic read failed");
|
||||||
|
goto out;
|
||||||
} else {
|
} else {
|
||||||
avg_excited[0] += report.x;
|
avg_excited[0] += report.x;
|
||||||
avg_excited[1] += report.y;
|
avg_excited[1] += report.y;
|
||||||
avg_excited[2] += report.z;
|
avg_excited[2] += report.z;
|
||||||
}
|
}
|
||||||
|
|
||||||
// warnx("periodic read %u", i);
|
//warnx("periodic read %u", i);
|
||||||
// warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
|
//warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
|
||||||
// warnx("time: %lld", report.timestamp);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
avg_excited[0] /= i;
|
avg_excited[0] /= i;
|
||||||
avg_excited[1] /= i;
|
avg_excited[1] /= i;
|
||||||
avg_excited[2] /= i;
|
avg_excited[2] /= i;
|
||||||
|
|
||||||
warnx("periodic excited reads %u", i);
|
warnx("done. Performed %u reads", i);
|
||||||
warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]);
|
warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]);
|
||||||
|
|
||||||
/* Set to 1.1 Gauss and end calibration */
|
|
||||||
ret = ioctl(fd, MAGIOCNONEX, 0);
|
|
||||||
ret = ioctl(fd, MAGIOCSRANGE, 1);
|
|
||||||
|
|
||||||
float scaling[3];
|
float scaling[3];
|
||||||
|
|
||||||
/* calculate axis scaling */
|
/* calculate axis scaling */
|
||||||
scaling[0] = 1.16f / avg_excited[0];
|
scaling[0] = fabsf(1.16f / avg_excited[0]);
|
||||||
/* second axis inverted */
|
/* second axis inverted */
|
||||||
scaling[1] = 1.16f / -avg_excited[1];
|
scaling[1] = fabsf(1.16f / -avg_excited[1]);
|
||||||
scaling[2] = 1.08f / avg_excited[2];
|
scaling[2] = fabsf(1.08f / avg_excited[2]);
|
||||||
|
|
||||||
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
|
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
|
||||||
|
|
||||||
/* set back to normal mode */
|
/* set back to normal mode */
|
||||||
/* Set to 1.1 Gauss */
|
/* Set to 1.1 Gauss */
|
||||||
if (OK != ioctl(fd, MAGIOCSRANGE, 1)) {
|
if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
|
||||||
warnx("failed to set 1.1 Ga range");
|
warnx("failed to set 1.1 Ga range");
|
||||||
|
goto out;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (OK != ioctl(fd, MAGIOCCALIBRATE, 0)) {
|
if (OK != ioctl(filp, MAGIOCEXSTRAP, 0)) {
|
||||||
warnx("failed to disable sensor calibration mode");
|
warnx("failed to disable sensor calibration mode");
|
||||||
|
goto out;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* set scaling in device */
|
/* set scaling in device */
|
||||||
|
@ -941,9 +960,20 @@ int HMC5883::calibrate(unsigned enable)
|
||||||
mscale_previous.y_scale = scaling[1];
|
mscale_previous.y_scale = scaling[1];
|
||||||
mscale_previous.z_scale = scaling[2];
|
mscale_previous.z_scale = scaling[2];
|
||||||
|
|
||||||
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
|
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
|
||||||
warn("WARNING: failed to set new scale / offsets for mag");
|
warn("WARNING: failed to set new scale / offsets for mag");
|
||||||
|
goto out;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
ret = OK;
|
||||||
|
|
||||||
|
out:
|
||||||
|
if (ret == OK) {
|
||||||
|
warnx("calibration successfully finished.");
|
||||||
|
} else {
|
||||||
|
warnx("calibration failed.");
|
||||||
|
}
|
||||||
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
int HMC5883::set_excitement(unsigned enable)
|
int HMC5883::set_excitement(unsigned enable)
|
||||||
|
@ -954,7 +984,7 @@ int HMC5883::set_excitement(unsigned enable)
|
||||||
ret = read_reg(ADDR_CONF_A, conf_reg);
|
ret = read_reg(ADDR_CONF_A, conf_reg);
|
||||||
if (OK != ret)
|
if (OK != ret)
|
||||||
perf_count(_comms_errors);
|
perf_count(_comms_errors);
|
||||||
if (enable < 0) {
|
if (((int)enable) < 0) {
|
||||||
conf_reg |= 0x01;
|
conf_reg |= 0x01;
|
||||||
} else if (enable > 0) {
|
} else if (enable > 0) {
|
||||||
conf_reg |= 0x02;
|
conf_reg |= 0x02;
|
||||||
|
@ -1170,7 +1200,6 @@ test()
|
||||||
*/
|
*/
|
||||||
int calibrate()
|
int calibrate()
|
||||||
{
|
{
|
||||||
struct mag_report report;
|
|
||||||
ssize_t sz;
|
ssize_t sz;
|
||||||
int ret;
|
int ret;
|
||||||
|
|
||||||
|
@ -1178,13 +1207,17 @@ int calibrate()
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
|
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
|
||||||
|
|
||||||
if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
|
if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
|
||||||
warnx("failed to enable sensor calibration mode");
|
warnx("failed to enable sensor calibration mode");
|
||||||
}
|
}
|
||||||
|
|
||||||
close(fd);
|
close(fd);
|
||||||
|
|
||||||
errx(0, "PASS");
|
if (ret == OK) {
|
||||||
|
errx(0, "PASS");
|
||||||
|
} else {
|
||||||
|
errx(1, "FAIL");
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
Loading…
Reference in New Issue