Checkpoint: Working, but non-verified full mag calibration

4 files changed, 398 insertions, 133 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)
diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
index fc80c8d17..fc095bff8 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,70 +813,89 @@ 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");
+	struct mag_scale mscale_previous = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+
+	struct mag_scale mscale_null = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+
+	float avg_excited[3] = {0.0f, 0.0f, 0.0f};
+	unsigned i;
 
 	warnx("starting mag scale calibration");
-	warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
+
+	/* do a simple demand read */
+	sz = read(filp, (char*)&report, sizeof(report));
+	if (sz != sizeof(report)) {
+		warn("immediate read failed");
+		ret = 1;
+		goto out;
+	}
+
+	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(fd, SENSORIOCSQUEUEDEPTH, 10))
-		errx(1, "failed to set queue depth");
+	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(fd, SENSORIOCSPOLLRATE, 50))
-		errx(1, "failed to set 2Hz poll rate");
+	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(fd, MAGIOCSRANGE, 2)) {
+	if (OK != ioctl(filp, MAGIOCSRANGE, 2)) {
 		warnx("failed to set 2.5 Ga range");
+		ret = 1;
+		goto out;
 	}
 
-	if (OK != ioctl(fd, MAGIOCPOSEX, 1)) {
+	if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
 		warnx("failed to enable sensor calibration mode");
+		ret = 1;
+		goto out;
 	}
 
-	struct mag_scale mscale_previous = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
+	if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
 		warn("WARNING: failed to get scale / offsets for mag");
+		ret = 1;
+		goto out;
 	}
 
-	struct mag_scale mscale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
+	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
 		warn("WARNING: failed to set null scale / offsets for mag");
+		ret = 1;
+		goto out;
 	}
 
-	float avg_excited[3];
-	unsigned i;
-
 	/* read the sensor 10x and report each value */
 	for (i = 0; i < 500; i++) {
 		struct pollfd fds;
@@ -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");
+	}
 }
 
 /**