Merge branch 'release_v1.0.0' of github.com:PX4/Firmware into stable

Tools/Matlab/ellipsoid_fit.m

@@ -0,0 +1,174 @@
+% Copyright (c) 2009, Yury Petrov
+% All rights reserved.
+%
+% Redistribution and use in source and binary forms, with or without
+% modification, are permitted provided that the following conditions are
+% met:
+%
+%     * Redistributions of source code must retain the above copyright
+%       notice, this list of conditions and the following disclaimer.
+%     * Redistributions in binary form must reproduce the above copyright
+%       notice, this list of conditions and the following disclaimer in
+%       the documentation and/or other materials provided with the distribution
+%
+% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+% POSSIBILITY OF SUCH DAMAGE.
+%
+
+function [ center, radii, evecs, v ] = ellipsoid_fit( X, flag, equals )
+%
+% Fit an ellispoid/sphere to a set of xyz data points:
+%
+%   [center, radii, evecs, pars ] = ellipsoid_fit( X )
+%   [center, radii, evecs, pars ] = ellipsoid_fit( [x y z] );
+%   [center, radii, evecs, pars ] = ellipsoid_fit( X, 1 );
+%   [center, radii, evecs, pars ] = ellipsoid_fit( X, 2, 'xz' );
+%   [center, radii, evecs, pars ] = ellipsoid_fit( X, 3 );
+%
+% Parameters:
+% * X, [x y z]   - Cartesian data, n x 3 matrix or three n x 1 vectors
+% * flag         - 0 fits an arbitrary ellipsoid (default),
+%                - 1 fits an ellipsoid with its axes along [x y z] axes
+%                - 2 followed by, say, 'xy' fits as 1 but also x_rad = y_rad
+%                - 3 fits a sphere
+%
+% Output:
+% * center    -  ellispoid center coordinates [xc; yc; zc]
+% * ax        -  ellipsoid radii [a; b; c]
+% * evecs     -  ellipsoid radii directions as columns of the 3x3 matrix
+% * v         -  the 9 parameters describing the ellipsoid algebraically: 
+%                Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1
+%
+% Author:
+% Yury Petrov, Northeastern University, Boston, MA
+%
+
+error( nargchk( 1, 3, nargin ) );  % check input arguments
+if nargin == 1
+    flag = 0;  % default to a free ellipsoid
+end
+if flag == 2 && nargin == 2
+    equals = 'xy';
+end
+    
+if size( X, 2 ) ~= 3
+    error( 'Input data must have three columns!' );
+else
+    x = X( :, 1 );
+    y = X( :, 2 );
+    z = X( :, 3 );
+end
+
+% need nine or more data points
+if length( x ) < 9 && flag == 0 
+   error( 'Must have at least 9 points to fit a unique ellipsoid' );
+end
+if length( x ) < 6 && flag == 1
+   error( 'Must have at least 6 points to fit a unique oriented ellipsoid' );
+end
+if length( x ) < 5 && flag == 2
+   error( 'Must have at least 5 points to fit a unique oriented ellipsoid with two axes equal' );
+end
+if length( x ) < 3 && flag == 3
+   error( 'Must have at least 4 points to fit a unique sphere' );
+end
+
+if flag == 0
+    % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1
+    D = [ x .* x, ...
+          y .* y, ...
+          z .* z, ...
+      2 * x .* y, ...
+      2 * x .* z, ...
+      2 * y .* z, ...
+      2 * x, ...
+      2 * y, ... 
+      2 * z ];  % ndatapoints x 9 ellipsoid parameters
+elseif flag == 1
+    % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1
+    D = [ x .* x, ...
+          y .* y, ...
+          z .* z, ...
+      2 * x, ...
+      2 * y, ... 
+      2 * z ];  % ndatapoints x 6 ellipsoid parameters
+elseif flag == 2
+    % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1,
+    % where A = B or B = C or A = C
+    if strcmp( equals, 'yz' ) || strcmp( equals, 'zy' )
+        D = [ y .* y + z .* z, ...
+            x .* x, ...
+            2 * x, ...
+            2 * y, ...
+            2 * z ];
+    elseif strcmp( equals, 'xz' ) || strcmp( equals, 'zx' )
+        D = [ x .* x + z .* z, ...
+            y .* y, ...
+            2 * x, ...
+            2 * y, ...
+            2 * z ];
+    else
+        D = [ x .* x + y .* y, ...
+            z .* z, ...
+            2 * x, ...
+            2 * y, ...
+            2 * z ];
+    end
+else
+    % fit sphere in the form A(x^2 + y^2 + z^2) + 2Gx + 2Hy + 2Iz = 1
+    D = [ x .* x + y .* y + z .* z, ...
+      2 * x, ...
+      2 * y, ... 
+      2 * z ];  % ndatapoints x 4 sphere parameters
+end
+
+% solve the normal system of equations
+v = ( D' * D ) \ ( D' * ones( size( x, 1 ), 1 ) );
+
+% find the ellipsoid parameters
+if flag == 0
+    % form the algebraic form of the ellipsoid
+    A = [ v(1) v(4) v(5) v(7); ...
+          v(4) v(2) v(6) v(8); ...
+          v(5) v(6) v(3) v(9); ...
+          v(7) v(8) v(9) -1 ];
+    % find the center of the ellipsoid
+    center = -A( 1:3, 1:3 ) \ [ v(7); v(8); v(9) ];
+    % form the corresponding translation matrix
+    T = eye( 4 );
+    T( 4, 1:3 ) = center';
+    % translate to the center
+    R = T * A * T';
+    % solve the eigenproblem
+    [ evecs evals ] = eig( R( 1:3, 1:3 ) / -R( 4, 4 ) );
+    radii = sqrt( 1 ./ diag( evals ) );
+else
+    if flag == 1
+        v = [ v(1) v(2) v(3) 0 0 0 v(4) v(5) v(6) ];
+    elseif flag == 2
+        if strcmp( equals, 'xz' ) || strcmp( equals, 'zx' )
+            v = [ v(1) v(2) v(1) 0 0 0 v(3) v(4) v(5) ];
+        elseif strcmp( equals, 'yz' ) || strcmp( equals, 'zy' )
+            v = [ v(2) v(1) v(1) 0 0 0 v(3) v(4) v(5) ];
+        else % xy
+            v = [ v(1) v(1) v(2) 0 0 0 v(3) v(4) v(5) ];
+        end
+    else
+        v = [ v(1) v(1) v(1) 0 0 0 v(2) v(3) v(4) ];
+    end
+    center = ( -v( 7:9 ) ./ v( 1:3 ) )';
+    gam = 1 + ( v(7)^2 / v(1) + v(8)^2 / v(2) + v(9)^2 / v(3) );
+    radii = ( sqrt( gam ./ v( 1:3 ) ) )';
+    evecs = eye( 3 );
+end
+
+

Tools/Matlab/plot_mag.m

@@ -0,0 +1,77 @@
+%
+% Tool for plotting mag data
+%
+% Reference values:
+% telem> [cal] mag #0 off: x:0.15    y:0.07     z:0.14 Ga
+%                  MATLAB: x:0.1581  y: 0.0701  z: 0.1439 Ga
+% telem> [cal] mag #0 scale: x:1.10 y:0.97 z:1.02
+%                  MATLAB: 0.5499, 0.5190, 0.4907
+%
+% telem> [cal] mag #1 off: x:-0.18    y:0.11    z:-0.09 Ga
+%                  MATLAB: x:-0.1827  y:0.1147  z:-0.0848 Ga
+% telem> [cal] mag #1 scale: x:1.00 y:1.00 z:1.00
+%                  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;
+
+plot_scale = 0.8;
+
+xmax = plot_scale;
+xmin = -xmax;
+ymax = plot_scale;
+ymin = -ymax;
+zmax = plot_scale;
+zmin = -zmax;
+
+mag0_raw = load('../../mag0_raw3.csv');
+mag1_raw = load('../../mag1_raw3.csv');
+
+mag0_cal = load('../../mag0_cal3.csv');
+mag1_cal = load('../../mag1_cal3.csv');
+
+fm0r = figure();
+
+mag0_x_scale = 0.88;
+mag0_y_scale = 0.99;
+mag0_z_scale = 0.95;
+
+plot3(mag0_raw(:,1), mag0_raw(:,2), mag0_raw(:,3), '*r');
+[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');
+[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] );
+center
+radii
+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');
+[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))

src/drivers/hmc5883/hmc5883.cpp

@@ -1124,8 +1124,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		}
 	}
 
-	/* read the sensor up to 50x, stopping when we have 10 good values */
+	/* read the sensor up to 100x, stopping when we have 30 good values */
-	for (uint8_t i = 0; i < 50 && good_count < 10; i++) {
+	for (uint8_t i = 0; i < 100 && good_count < 30; i++) {
 		struct pollfd fds;
 
 		/* wait for data to be ready */
@@ -1172,9 +1172,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 	scaling[2] = sum_excited[2] / good_count;
 
 	/* set scaling in device */
-	mscale_previous.x_scale = scaling[0];
+	mscale_previous.x_scale = 1.0f / scaling[0];
-	mscale_previous.y_scale = scaling[1];
+	mscale_previous.y_scale = 1.0f / scaling[1];
-	mscale_previous.z_scale = scaling[2];
+	mscale_previous.z_scale = 1.0f / scaling[2];
 
 	ret = OK;
 

src/modules/commander/calibration_messages.h

@@ -49,7 +49,7 @@
 // instead of visual calibration until such a time as QGC is update to the new version.
 
 // The number in the cal started message is used to indicate the version stamp for the current calibration code.
-#define CAL_QGC_STARTED_MSG			"[cal] calibration started: 1 %s"
+#define CAL_QGC_STARTED_MSG			"[cal] calibration started: 2 %s"
 #define CAL_QGC_DONE_MSG			"[cal] calibration done: %s"
 #define CAL_QGC_FAILED_MSG			"[cal] calibration failed: %s"
 #define CAL_QGC_WARNING_MSG			"[cal] calibration warning: %s"

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) {

src/modules/commander/mag_calibration.cpp

@@ -65,6 +65,8 @@ static const int ERROR = -1;
 
 static const char *sensor_name = "mag";
 static const unsigned max_mags = 3;
+static constexpr float mag_sphere_radius = 0.2f;
+static const unsigned int calibration_sides = 6;
 
 calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
 
@@ -76,7 +78,7 @@ typedef struct  {
 	unsigned int	calibration_points_perside;
 	unsigned int	calibration_interval_perside_seconds;
 	uint64_t	calibration_interval_perside_useconds;
-	unsigned int	calibration_counter_total;
+	unsigned int	calibration_counter_total[max_mags];
 	bool		side_data_collected[detect_orientation_side_count];
 	float*		x[max_mags];
 	float*		y[max_mags];
@@ -184,6 +186,24 @@ int do_mag_calibration(int mavlink_fd)
 	return result;
 }
 
+static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count)
+{
+	float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f;
+
+	for (size_t i = 0; i < count; i++) {
+		float dx = sx - x[i];
+		float dy = sy - y[i];
+		float dz = sz - z[i];
+		float dist = sqrtf(dx * dx + dy * dy + dz * dz);
+
+		if (dist < min_sample_dist) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
 static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
 {
 	calibrate_return result = calibrate_return_ok;
@@ -286,27 +306,47 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 		int poll_ret = poll(fds, fd_count, 1000);
 		
 		if (poll_ret > 0) {
+
+			int prev_count[max_mags];
+			bool rejected = false;
+
 			for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+
+				prev_count[cur_mag] = worker_data->calibration_counter_total[cur_mag];
+
 				if (worker_data->sub_mag[cur_mag] >= 0) {
 					struct mag_report mag;
 
 					orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag);
+
+					// Check if this measurement is good to go in
+					rejected = rejected || reject_sample(mag.x, mag.y, mag.z,
+						worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag],
+						worker_data->calibration_counter_total[cur_mag],
+						calibration_sides * worker_data->calibration_points_perside);
 					
-					worker_data->x[cur_mag][worker_data->calibration_counter_total] = mag.x;
+					worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.x;
-					worker_data->y[cur_mag][worker_data->calibration_counter_total] = mag.y;
+					worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.y;
-					worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z;
+					worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.z;
-					
+					worker_data->calibration_counter_total[cur_mag]++;
 				}
 			}
-			
+
-			worker_data->calibration_counter_total++;
+			// Keep calibration of all mags in lockstep
-			calibration_counter_side++;
+			if (rejected) {
-			
+				// Reset counts, since one of the mags rejected the measurement
-			// Progress indicator for side
+				for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
-			mavlink_and_console_log_info(worker_data->mavlink_fd,
+					worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag];
-						     "[cal] %s side calibration: progress <%u>",
+				}
-						     detect_orientation_str(orientation),
+			} else {
-						     (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
+				calibration_counter_side++;
+
+				// Progress indicator for side
+				mavlink_and_console_log_info(worker_data->mavlink_fd,
+							     "[cal] %s side calibration: progress <%u>",
+							     detect_orientation_str(orientation),
+							     (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
+			}
 		} else {
 			poll_errcount++;
 		}
@@ -336,8 +376,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 	
 	worker_data.mavlink_fd = mavlink_fd;
 	worker_data.done_count = 0;
-	worker_data.calibration_counter_total = 0;
+	worker_data.calibration_points_perside = 40;
-	worker_data.calibration_points_perside = 80;
 	worker_data.calibration_interval_perside_seconds = 20;
 	worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
 
@@ -345,9 +384,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;
 	worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;
 	worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;
-	worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true;
+	worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = false;
-	worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true;
+	worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = false;
-	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true;
+	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = false;
 	
 	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
 		// Initialize to no subscription
@@ -357,9 +396,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 		worker_data.x[cur_mag] = NULL;
 		worker_data.y[cur_mag] = NULL;
 		worker_data.z[cur_mag] = NULL;
+		worker_data.calibration_counter_total[cur_mag] = 0;
 	}
 
-	const unsigned int calibration_sides = 3;
 	const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
 	
 	char str[30];
@@ -438,7 +477,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 				// Mag in this slot is available and we should have values for it to calibrate
 				
 				sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
-							 worker_data.calibration_counter_total,
+							 worker_data.calibration_counter_total[cur_mag],
 							 100, 0.0f,
 							 &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
 							 &sphere_radius[cur_mag]);
@@ -450,6 +489,41 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 			}
 		}
 	}
+
+	// Print uncalibrated data points
+	if (result == calibrate_return_ok) {
+
+		printf("RAW DATA:\n--------------------\n");
+		for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
+
+			printf("RAW: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]);
+
+			for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
+				float x = worker_data.x[cur_mag][i];
+				float y = worker_data.y[cur_mag][i];
+				float z = worker_data.z[cur_mag][i];
+				printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
+			}
+
+			printf(">>>>>>>\n");
+		}
+
+		printf("CALIBRATED DATA:\n--------------------\n");
+		for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
+
+			printf("Calibrated: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]);
+
+			for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
+				float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag];
+				float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag];
+				float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag];
+				printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
+			}
+
+			printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]);
+			printf(">>>>>>>\n");
+		}
+	}
 	
 	// Data points are no longer needed
 	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -221,6 +221,10 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_parameter_handles.eas_noise = param_find("PE_EAS_NOISE");
 	_parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT");
 
+	/* indicate consumers that the current position data is not valid */
+	_gps.eph = 10000.0f;
+	_gps.epv = 10000.0f;
+
 	/* fetch initial parameter values */
 	parameters_update();
 
@@ -686,21 +690,20 @@ void AttitudePositionEstimatorEKF::task_main()
 						continue;
 					}
 
-					//Run EKF data fusion steps
+					// Run EKF data fusion steps
 					updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData);
 
-					//Publish attitude estimations
+					// Publish attitude estimations
 					publishAttitude();
 
-					//Publish Local Position estimations
+					// Publish Local Position estimations
 					publishLocalPosition();
 
-					//Publish Global Position, but only if it's any good
+					// Publish Global Position, it will have a large uncertainty
-					if (_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) {
+					// set if only altitude is known
-						publishGlobalPosition();
+					publishGlobalPosition();
-					}
 
-					//Publish wind estimates
+					// Publish wind estimates
 					if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
 						publishWindEstimate();
 					}
@@ -891,6 +894,10 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 		_global_pos.lat = est_lat;
 		_global_pos.lon = est_lon;
 		_global_pos.time_utc_usec = _gps.time_utc_usec;
+	} else {
+		_global_pos.lat = 0.0;
+		_global_pos.lon = 0.0;
+		_global_pos.time_utc_usec = 0;
 	}
 
 	if (_local_pos.v_xy_valid) {
@@ -907,6 +914,8 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 
 	if (_local_pos.v_z_valid) {
 		_global_pos.vel_d = _local_pos.vz;
+	} else {
+		_global_pos.vel_d = 0.0f;
 	}
 
 	/* terrain altitude */

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -98,10 +98,12 @@ static int	_control_task = -1;			/**< task handle for sensor task */
 #define HDG_HOLD_REACHED_DIST 		1000.0f 	// distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
 #define HDG_HOLD_SET_BACK_DIST 		100.0f 		// distance by which previous waypoint is set behind the plane
 #define HDG_HOLD_YAWRATE_THRESH 	0.1f 		// max yawrate at which plane locks yaw for heading hold mode
-#define HDG_HOLD_MAN_INPUT_THRESH 	0.01f 	// max manual roll input from user which does not change the locked heading
+#define HDG_HOLD_MAN_INPUT_THRESH 	0.01f 		// max manual roll input from user which does not change the locked heading
+#define TAKEOFF_IDLE				0.2f 		// idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0)
 
 static constexpr float THROTTLE_THRESH = 0.05f; 	///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
 static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f;	///< a throttle / pitch input above this value leads to the system switching to climbout mode
+static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f;
 
 /**
  * L1 control app start / stop handling function
@@ -173,10 +175,10 @@ private:
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	float	_hold_alt;				/**< hold altitude for altitude mode */
-	float 	_ground_alt;			/**< ground altitude at which plane was launched */
+	float	_takeoff_ground_alt;				/**< ground altitude at which plane was launched */
 	float	_hdg_hold_yaw;				/**< hold heading for velocity mode */
 	bool	_hdg_hold_enabled;			/**< heading hold enabled */
-	bool 	_yaw_lock_engaged;			/**< yaw is locked for heading hold */
+	bool	_yaw_lock_engaged;			/**< yaw is locked for heading hold */
 	struct position_setpoint_s _hdg_hold_prev_wp;	/**< position where heading hold started */
 	struct position_setpoint_s _hdg_hold_curr_wp;	/**< position to which heading hold flies */
 	hrt_abstime _control_position_last_called; /**<last call of control_position  */
@@ -502,7 +504,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
 
 	_hold_alt(0.0f),
-	_ground_alt(0.0f),
+	_takeoff_ground_alt(0.0f),
 	_hdg_hold_yaw(0.0f),
 	_hdg_hold_enabled(false),
 	_yaw_lock_engaged(false),
@@ -968,9 +970,24 @@ bool FixedwingPositionControl::update_desired_altitude(float dt)
 {
 	const float deadBand = (60.0f/1000.0f);
 	const float factor = 1.0f - deadBand;
+	// XXX this should go into a manual stick mapper
+	// class
+	static float _althold_epv = 0.0f;
 	static bool was_in_deadband = false;
 	bool climbout_mode = false;
 
+	/*
+	 * Reset the hold altitude to the current altitude if the uncertainty
+	 * changes significantly.
+	 * This is to guard against uncommanded altitude changes
+	 * when the altitude certainty increases or decreases.
+	 */
+
+	if (fabsf(_althold_epv - _global_pos.epv) > ALTHOLD_EPV_RESET_THRESH) {
+		_hold_alt = _global_pos.alt;
+		_althold_epv = _global_pos.epv;
+	}
+
 	// XXX the sign magic in this function needs to be fixed
 
 	if (_manual.x > deadBand) {
@@ -987,6 +1004,7 @@ bool FixedwingPositionControl::update_desired_altitude(float dt)
 		  * The aircraft should immediately try to fly at this altitude
 		  * as this is what the pilot expects when he moves the stick to the center */
 		_hold_alt = _global_pos.alt;
+		_althold_epv = _global_pos.epv;
 		was_in_deadband = true;
 	}
 
@@ -997,7 +1015,7 @@ bool FixedwingPositionControl::in_takeoff_situation() {
 	const hrt_abstime delta_takeoff = 10000000;
 	const float throttle_threshold = 0.1f;
 
-	if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + _parameters.climbout_diff) {
+	if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff) {
 		return true;
 	}
 
@@ -1008,7 +1026,7 @@ void FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitc
 {
 	/* demand "climbout_diff" m above ground if user switched into this mode during takeoff */
 	if (in_takeoff_situation()) {
-		*hold_altitude = _ground_alt + _parameters.climbout_diff;
+		*hold_altitude = _takeoff_ground_alt + _parameters.climbout_diff;
 		*pitch_limit_min = math::radians(10.0f);
 	} else {
 		*pitch_limit_min = _parameters.pitch_limit_min;
@@ -1050,7 +1068,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	if (!_was_in_air && !_vehicle_status.condition_landed) {
 		_was_in_air = true;
 		_time_went_in_air = hrt_absolute_time();
-		_ground_alt = _global_pos.alt;
+		_takeoff_ground_alt = _global_pos.alt;
 	}
 	/* reset flag when airplane landed */
 	if (_vehicle_status.condition_landed) {
@@ -1606,8 +1624,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		_att_sp.thrust = 0.0f;
 	} else {
 		/* Copy thrust and pitch values from tecs */
-		_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :
+		if (_vehicle_status.condition_landed &&
-				_tecs.get_throttle_demand(), throttle_max);
+			(_control_mode_current == FW_POSCTRL_MODE_POSITION || _control_mode_current == FW_POSCTRL_MODE_ALTITUDE))
+		{
+			// when we are landed in these modes we want the motor to spin
+			_att_sp.thrust = math::min(TAKEOFF_IDLE, throttle_max);
+		} else {
+			_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :
+					_tecs.get_throttle_demand(), throttle_max);
+		}
+
+
 	}
 
 	/* During a takeoff waypoint while waiting for launch the pitch sp is set