aboutsummaryrefslogtreecommitdiff
path: root/apps/commander
diff options
context:
space:
mode:
Diffstat (limited to 'apps/commander')
-rw-r--r--apps/commander/calibration_routines.c175
-rw-r--r--apps/commander/calibration_routines.h21
-rw-r--r--apps/commander/commander.c180
3 files changed, 304 insertions, 72 deletions
diff --git a/apps/commander/calibration_routines.c b/apps/commander/calibration_routines.c
new file mode 100644
index 000000000..88b64a06a
--- /dev/null
+++ b/apps/commander/calibration_routines.c
@@ -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;
+}
diff --git a/apps/commander/calibration_routines.h b/apps/commander/calibration_routines.h
new file mode 100644
index 000000000..c5a184341
--- /dev/null
+++ b/apps/commander/calibration_routines.h
@@ -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); \ No newline at end of file
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 545569a65..285b11a45 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -83,6 +83,8 @@
#include <drivers/drv_mag.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_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;
state_machine_publish(status_pub, status, mavlink_fd);
- int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
- struct sensor_combined_s raw;
+ int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
+ orb_set_interval(sub_mag, 22);
+ struct mag_report mag;
/* 30 seconds */
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);
-
-
+ /* erase old calibration */
struct mag_scale mscale_null = {
0.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");
}
+ /* calibrate range */
+ if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
+ warnx("failed to calibrate scale");
+ }
+
close(fd);
+ /* calibrate offsets */
+
uint64_t calibration_start = 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'};
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 */
- struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
+ struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } };
/* user guidance */
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)) {
- orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
- /* get min/max values */
+ orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
- /* ignore other axes */
- if (raw.magnetometer_ga[0] < mag_min[0]) {
- mag_min[0] = raw.magnetometer_ga[0];
- }
- else if (raw.magnetometer_ga[0] > mag_max[0]) {
- mag_max[0] = raw.magnetometer_ga[0];
- }
+ x[calibration_counter] = mag.x;
+ y[calibration_counter] = mag.y;
+ z[calibration_counter] = mag.z;
- if (raw.magnetometer_ga[1] < mag_min[1]) {
- mag_min[1] = raw.magnetometer_ga[1];
- }
- else if (raw.magnetometer_ga[1] > mag_max[1]) {
- mag_max[1] = raw.magnetometer_ga[1];
- }
+ /* get min/max values */
- if (raw.magnetometer_ga[2] < mag_min[2]) {
- mag_min[2] = raw.magnetometer_ga[2];
- }
- else if (raw.magnetometer_ga[2] > mag_max[2]) {
- mag_max[2] = raw.magnetometer_ga[2];
- }
+ // if (mag.x < mag_min[0]) {
+ // mag_min[0] = mag.x;
+ // }
+ // else if (mag.x > mag_max[0]) {
+ // mag_max[0] = mag.x;
+ // }
+
+ // if (raw.magnetometer_ga[1] < mag_min[1]) {
+ // mag_min[1] = raw.magnetometer_ga[1];
+ // }
+ // else if (raw.magnetometer_ga[1] > mag_max[1]) {
+ // mag_max[1] = raw.magnetometer_ga[1];
+ // }
+
+ // if (raw.magnetometer_ga[2] < mag_min[2]) {
+ // mag_min[2] = raw.magnetometer_ga[2];
+ // }
+ // else if (raw.magnetometer_ga[2] > mag_max[2]) {
+ // mag_max[2] = raw.magnetometer_ga[2];
+ // }
calibration_counter++;
} else {
@@ -396,76 +415,89 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
}
}
- /* disable calibration mode */
- status->flag_preflight_mag_calibration = false;
- state_machine_publish(status_pub, status, mavlink_fd);
+ float sphere_x;
+ float sphere_y;
+ float sphere_z;
+ float sphere_radius;
+
+ sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
+
+ free(x);
+ free(y);
+ free(z);
+
+ float mag_offset[3] = {sphere_x, sphere_y, sphere_z};
+
+ // *
+ // * 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;
- float mag_offset[3];
-
- /**
- * 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
- */
+ if (isfinite(mag_offset[0]) && isfinite(mag_offset[1]) && isfinite(mag_offset[2])) {
- 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;
+ fd = open(MAG_DEVICE_PATH, 0);
- if (isfinite(mag_offset[0]) && isfinite(mag_offset[1]) && isfinite(mag_offset[2])) {
+ struct mag_scale mscale;
+
+ if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
+ warn("WARNING: failed to get scale / offsets for mag");
+
+ 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))
+ warn("WARNING: failed to set scale / offsets for mag");
+ close(fd);
/* announce and set new offset */
- if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) {
+ 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"), &(mag_offset[1]))) {
+ 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"), &(mag_offset[2]))) {
+ 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"), &(mag_offset[0]))) {
- fprintf(stderr, "[commander] Setting X 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"), &(mag_offset[1]))) {
- fprintf(stderr, "[commander] Setting Y mag offset 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"), &(mag_offset[2]))) {
- fprintf(stderr, "[commander] Setting Z mag offset failed!\n");
+ if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
+ fprintf(stderr, "[commander] Setting Z mag scale failed!\n");
}
- fd = open(MAG_DEVICE_PATH, 0);
- struct mag_scale mscale = {
- mag_offset[0],
- 1.0f,
- mag_offset[1],
- 1.0f,
- mag_offset[2],
- 1.0f,
- };
- if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
- warn("WARNING: failed to set scale / offsets for mag");
- close(fd);
-
/* auto-save to EEPROM */
int save_ret = pm_save_eeprom(false);
if(save_ret != 0) {
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];
// 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));
@@ -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)");
}
- 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)
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-16 12:10:09 +0000