From c402d0c2f7f856e8fd6b94be3ef2824a423f7843 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 25 Jun 2015 13:20:43 +0200
Subject: [PATCH] Commander: updated mag calibration routine, matlab script
 updates

---
 Tools/Matlab/plot_mag.m                       | 34 +++++++++++++------
 .../commander/calibration_routines.cpp        |  6 ++--
 2 files changed, 27 insertions(+), 13 deletions(-)

diff --git a/Tools/Matlab/plot_mag.m b/Tools/Matlab/plot_mag.m
index b344e41bc0..c9f0c29925 100644
--- a/Tools/Matlab/plot_mag.m
+++ b/Tools/Matlab/plot_mag.m
@@ -13,6 +13,12 @@
 %                  MATLAB: 0.5122, 0.5065, 0.4915
 %
 %
+% User-guided values:
+%
+% telem> [cal] mag #0 off: x:0.12 y:0.09 z:0.14 Ga
+% telem> [cal] mag #0 scale: x:0.88 y:0.99 z:0.95
+% telem> [cal] mag #1 off: x:-0.18 y:0.11 z:-0.09 Ga
+% telem> [cal] mag #1 scale: x:1.00 y:1.00 z:1.00
 
 close all;
 clear all;
@@ -26,11 +32,11 @@ ymin = -ymax;
 zmax = plot_scale;
 zmin = -zmax;
 
-mag0_raw = load('../../mag0_raw2.csv');
-mag1_raw = load('../../mag1_raw2.csv');
+mag0_raw = load('../../mag0_raw3.csv');
+mag1_raw = load('../../mag1_raw3.csv');
 
-mag0_cal = load('../../mag0_cal2.csv');
-mag1_cal = load('../../mag1_cal2.csv');
+mag0_cal = load('../../mag0_cal3.csv');
+mag1_cal = load('../../mag1_cal3.csv');
 
 fm0r = figure();
 
@@ -39,10 +45,11 @@ mag0_y_scale = 0.99;
 mag0_z_scale = 0.95;
 
 plot3(mag0_raw(:,1), mag0_raw(:,2), mag0_raw(:,3), '*r');
-[center, radii, evecs, pars ] = ellipsoid_fit( [mag0_raw(:,1) mag0_raw(:,2) mag0_raw(:,3)] );
-center
-radii
+[mag0_raw_center, mag0_raw_radii, evecs, pars ] = ellipsoid_fit( [mag0_raw(:,1) mag0_raw(:,2) mag0_raw(:,3)] );
+mag0_raw_center
+mag0_raw_radii
 axis([xmin xmax ymin ymax zmin zmax])
+viscircles([mag0_raw_center(1), mag0_raw_center(2)], [mag0_raw_radii(1)]);
 
 fm1r = figure();
 plot3(mag1_raw(:,1), mag1_raw(:,2), mag1_raw(:,3), '*r');
@@ -53,11 +60,18 @@ axis([xmin xmax ymin ymax zmin zmax])
 
 fm0c = figure();
 plot3(mag0_cal(:,1) .* mag0_x_scale, mag0_cal(:,2) .* mag0_y_scale, mag0_cal(:,3) .* mag0_z_scale, '*b');
-[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] );
-center
-radii
+[mag0_cal_center, mag0_cal_radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) .* mag0_x_scale mag1_raw(:,2) .* mag0_y_scale mag1_raw(:,3) .* mag0_z_scale] );
+mag0_cal_center
+mag0_cal_radii
 axis([xmin xmax ymin ymax zmin zmax])
+viscircles([0, 0], [mag0_cal_radii(3)]);
 
 fm1c = figure();
 plot3(mag1_cal(:,1), mag1_cal(:,2), mag1_cal(:,3), '*b');
 axis([xmin xmax ymin ymax zmin zmax])
+[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] );
+viscircles([0, 0], [radii(3)]);
+
+mag0_x_scale_matlab = 1 / (mag0_cal_radii(1) / mag0_raw_radii(1))
+mag0_y_scale_matlab = 1 / (mag0_cal_radii(2) / mag0_raw_radii(2))
+mag0_z_scale_matlab = 1 / (mag0_cal_radii(3) / mag0_raw_radii(3))
diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp
index e9f83775d7..045dbb032d 100644
--- a/src/modules/commander/calibration_routines.cpp
+++ b/src/modules/commander/calibration_routines.cpp
@@ -243,7 +243,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
 	const float	normal_still_thr = 0.25;		// normal still threshold
 	float		still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
 	float		accel_err_thr = 5.0f;			// set accel error threshold to 5m/s^2
-	hrt_abstime	still_time = lenient_still_position ? 1000000 : 1500000;	// still time required in us
+	hrt_abstime	still_time = lenient_still_position ? 500000 : 1300000;	// still time required in us
     
 	struct pollfd fds[1];
 	fds[0].fd = accel_sub;
@@ -324,7 +324,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
 				/* not still, reset still start time */
 				if (t_still != 0) {
 					mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
-					usleep(500000);
+					usleep(200000);
 					t_still = 0;
 				}
 			}
@@ -488,7 +488,7 @@ calibrate_return calibrate_from_orientation(int		mavlink_fd,
 		// Note that this side is complete
 		side_data_collected[orient] = true;
 		tune_neutral(true);
-		usleep(500000);
+		usleep(200000);
 	}
 	
 	if (sub_accel >= 0) {