From 096bf2dc93fe8360fa83bee409452f8db7bc3593 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Oct 2012 15:36:29 +0200 Subject: [PATCH] Checkpoint: Working, but non-verified full mag calibration --- apps/commander/calibration_routines.c | 175 +++++++++++++++++++++++ apps/commander/calibration_routines.h | 21 +++ apps/commander/commander.c | 198 +++++++++++++++----------- apps/drivers/hmc5883/hmc5883.cpp | 157 ++++++++++++-------- 4 files changed, 408 insertions(+), 143 deletions(-) create mode 100644 apps/commander/calibration_routines.c create mode 100644 apps/commander/calibration_routines.h diff --git a/apps/commander/calibration_routines.c b/apps/commander/calibration_routines.c new file mode 100644 index 0000000000..88b64a06a9 --- /dev/null +++ b/apps/commander/calibration_routines.c @@ -0,0 +1,175 @@ +#include + +#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 0000000000..c5a184341e --- /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 545569a651..285b11a457 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -83,6 +83,8 @@ #include #include +#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); + 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 */ - /* 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]; - } + // 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[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]; - } + // 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; - float mag_offset[3]; + sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - /** - * 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 - */ + free(x); + free(y); + free(z); - 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] = {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; 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); - struct mag_scale mscale = { - mag_offset[0], - 1.0f, - mag_offset[1], - 1.0f, - mag_offset[2], - 1.0f, - }; + + 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"), &(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 */ 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) diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index fc80c8d177..fc095bff89 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -184,7 +184,7 @@ private: * * @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. @@ -603,7 +603,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return 0; case MAGIOCCALIBRATE: - return calibrate(arg); + return calibrate(filp, arg); case MAGIOCEXSTRAP: return set_excitement(arg); @@ -813,41 +813,15 @@ out: return ret; } -int HMC5883::calibrate(unsigned enable) +int HMC5883::calibrate(struct file *filp, unsigned enable) { struct mag_report report; ssize_t sz; - int ret; + int ret = 1; + // XXX do something smarter here 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 = { 0.0f, 1.0f, @@ -857,10 +831,6 @@ int HMC5883::calibrate(unsigned enable) 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 = { 0.0f, 1.0f, @@ -870,12 +840,61 @@ int HMC5883::calibrate(unsigned enable) 1.0f, }; - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set null scale / offsets for mag"); + float avg_excited[3] = {0.0f, 0.0f, 0.0f}; + 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]; - unsigned i; + warnx("current 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(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 */ for (i = 0; i < 500; i++) { @@ -884,56 +903,56 @@ int HMC5883::calibrate(unsigned enable) /* wait for data to be ready */ fds.fd = fd; fds.events = POLLIN; - ret = poll(&fds, 1, 2000); + ret = ::poll(&fds, 1, 2000); - if (ret != 1) - errx(1, "timed out waiting for sensor data"); + if (ret != 1) { + warn("timed out waiting for sensor data"); + goto out; + } /* now go get it */ - sz = read(fd, &report, sizeof(report)); + sz = ::read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - err(1, "periodic read failed"); + warn("periodic read failed"); + goto out; } else { avg_excited[0] += report.x; avg_excited[1] += report.y; avg_excited[2] += report.z; } - // warnx("periodic read %u", i); - // warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - // warnx("time: %lld", report.timestamp); + //warnx("periodic read %u", i); + //warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); } avg_excited[0] /= i; avg_excited[1] /= 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]); - /* Set to 1.1 Gauss and end calibration */ - ret = ioctl(fd, MAGIOCNONEX, 0); - ret = ioctl(fd, MAGIOCSRANGE, 1); - float scaling[3]; /* calculate axis scaling */ - scaling[0] = 1.16f / avg_excited[0]; + scaling[0] = fabsf(1.16f / avg_excited[0]); /* second axis inverted */ - scaling[1] = 1.16f / -avg_excited[1]; - scaling[2] = 1.08f / avg_excited[2]; + scaling[1] = fabsf(1.16f / -avg_excited[1]); + scaling[2] = fabsf(1.08f / avg_excited[2]); warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); /* set back to normal mode */ /* 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"); + goto out; } - if (OK != ioctl(fd, MAGIOCCALIBRATE, 0)) { + if (OK != ioctl(filp, MAGIOCEXSTRAP, 0)) { warnx("failed to disable sensor calibration mode"); + goto out; } /* set scaling in device */ @@ -941,9 +960,20 @@ int HMC5883::calibrate(unsigned enable) mscale_previous.y_scale = scaling[1]; 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"); + goto out; } + + ret = OK; + + out: + if (ret == OK) { + warnx("calibration successfully finished."); + } else { + warnx("calibration failed."); + } + return ret; } int HMC5883::set_excitement(unsigned enable) @@ -954,7 +984,7 @@ int HMC5883::set_excitement(unsigned enable) ret = read_reg(ADDR_CONF_A, conf_reg); if (OK != ret) perf_count(_comms_errors); - if (enable < 0) { + if (((int)enable) < 0) { conf_reg |= 0x01; } else if (enable > 0) { conf_reg |= 0x02; @@ -1170,7 +1200,6 @@ test() */ int calibrate() { - struct mag_report report; ssize_t sz; int ret; @@ -1178,13 +1207,17 @@ int calibrate() if (fd < 0) 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"); } close(fd); - errx(0, "PASS"); + if (ret == OK) { + errx(0, "PASS"); + } else { + errx(1, "FAIL"); + } } /**