Merge branch 'master' of github.com:PX4/Firmware into mtecs

src/drivers/frsky_telemetry/frsky_data.c

@@ -225,7 +225,7 @@ void frsky_send_frame2(int uart)
 	float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
 	char lat_ns = 0, lon_ew = 0;
 	int sec = 0;
-	if (global_pos.global_valid) {
+	if (global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
 		time_t time_gps = global_pos.time_gps_usec / 1000000;
 		struct tm *tm_gps = gmtime(&time_gps);
 

src/lib/geo/geo.c

@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -42,6 +39,7 @@
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <geo/geo.h>
@@ -52,124 +50,58 @@
 #include <math.h>
 #include <stdbool.h>
 
+/*
+ * Azimuthal Equidistant Projection
+ * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
+ */
 
-/* values for map projection */
+__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
-static double phi_1;
-static double sin_phi_1;
-static double cos_phi_1;
-static double lambda_0;
-static double scale;
-
-__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
 {
-	/* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
+	ref->lat = lat_0 / 180.0 * M_PI;
-	phi_1 = lat_0 / 180.0 * M_PI;
+	ref->lon = lon_0 / 180.0 * M_PI;
-	lambda_0 = lon_0 / 180.0 * M_PI;
-
-	sin_phi_1 = sin(phi_1);
-	cos_phi_1 = cos(phi_1);
-
-	/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
-
-	/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
-
-	double lat1 = phi_1;
-	double lon1 = lambda_0;
-
-	double lat2 = phi_1 + 0.5 / 180 * M_PI;
-	double lon2 = lambda_0 + 0.5 / 180 * M_PI;
-	double sin_lat_2 = sin(lat2);
-	double cos_lat_2 = cos(lat2);
-	double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * CONSTANTS_RADIUS_OF_EARTH;
-
-	/* 2) calculate distance rho on plane */
-	double k_bar = 0;
-	double c =  acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0));
-
-	if (0 != c)
-		k_bar = c / sin(c);
-
-	double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane
-	double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)));
-	double rho = sqrt(pow(x2, 2) + pow(y2, 2));
-
-	scale = d / rho;
 
+	ref->sin_lat = sin(ref->lat);
+	ref->cos_lat = cos(ref->lat);
 }
 
-__EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
+__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
 {
-	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
+	double lat_rad = lat / 180.0 * M_PI;
-	double phi = lat / 180.0 * M_PI;
+	double lon_rad = lon / 180.0 * M_PI;
-	double lambda = lon / 180.0 * M_PI;
 
-	double sin_phi = sin(phi);
+	double sin_lat = sin(lat_rad);
-	double cos_phi = cos(phi);
+	double cos_lat = cos(lat_rad);
+	double cos_d_lon = cos(lon_rad - ref->lon);
 
-	double k_bar = 0;
+	double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
-	/* using small angle approximation (formula in comment is without aproximation) */
+	double k = (c == 0.0) ? 1.0 : (c / sin(c));
-	double c =  acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c =  acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) );
 
-	if (0 != c)
+	*x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
-		k_bar = c / sin(c);
+	*y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH;
-
-	/* using small angle approximation (formula in comment is without aproximation) */
-	*y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale;
-	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; //	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale;
-
-//	printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0);
 }
 
-__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon)
+__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
 {
-	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
+	float x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
-
+	float y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
-	double x_descaled = x / scale;
+	double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
-	double y_descaled = y / scale;
-
-	double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2));
 	double sin_c = sin(c);
 	double cos_c = cos(c);
 
-	double lat_sphere = 0;
+	double lat_rad;
+	double lon_rad;
 
-	if (c != 0)
+	if (c != 0.0) {
-		lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c);
+		lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
-	else
+		lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
-		lat_sphere = asin(cos_c * sin_phi_1);
-
-//	printf("lat_sphere = %.10f\n",lat_sphere);
-
-	double lon_sphere = 0;
-
-	if (phi_1  == M_PI / 2) {
-		//using small angle approximation (formula in comment is without aproximation)
-		lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled));
-
-	} else if (phi_1 == -M_PI / 2) {
-		//using small angle approximation (formula in comment is without aproximation)
-		lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled));
 
 	} else {
-
+		lat_rad = ref->lat;
-		lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c));
+		lon_rad = ref->lon;
-		//using small angle approximation
-//    	double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c);
-//    	if(denominator != 0)
-//    	{
-//    		lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator);
-//    	}
-//    	else
-//    	{
-//    	...
-//    	}
 	}
 
-//	printf("lon_sphere = %.10f\n",lon_sphere);
+	*lat = lat_rad * 180.0 / M_PI;
-
+	*lon = lon_rad * 180.0 / M_PI;
-	*lat = lat_sphere * 180.0 / M_PI;
-	*lon = lon_sphere * 180.0 / M_PI;
-
 }
 
 
@@ -207,7 +139,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
 	return theta;
 }
 
-__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e)
+__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e)
 {
 	double lat_now_rad = lat_now * M_DEG_TO_RAD;
 	double lon_now_rad = lon_now * M_DEG_TO_RAD;
@@ -222,7 +154,7 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double
 	*v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
 }
 
-__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e)
+__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e)
 {
 	double lat_now_rad = lat_now * M_DEG_TO_RAD;
 	double lon_now_rad = lon_now * M_DEG_TO_RAD;
@@ -248,7 +180,7 @@ __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, floa
 
 // Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
 
-__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
+__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
 {
 // This function returns the distance to the nearest point on the track line.  Distance is positive if current
 // position is right of the track and negative if left of the track as seen from a point on the track line
@@ -265,7 +197,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
 	crosstrack_error->bearing = 0.0f;
 
 	// Return error if arguments are bad
-	if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_value;
+	if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) { return return_value; }
 
 	bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
 	bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
@@ -296,7 +228,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
 }
 
 
-__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
+__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
 				 float radius, float arc_start_bearing, float arc_sweep)
 {
 	// This function returns the distance to the nearest point on the track arc.  Distance is positive if current
@@ -316,29 +248,29 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
 	crosstrack_error->bearing = 0.0f;
 
 	// Return error if arguments are bad
-	if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_value;
+	if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) { return return_value; }
 
 
 	if (arc_sweep >= 0) {
 		bearing_sector_start = arc_start_bearing;
 		bearing_sector_end = arc_start_bearing + arc_sweep;
 
-		if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F;
+		if (bearing_sector_end > 2.0f * M_PI_F) { bearing_sector_end -= M_TWOPI_F; }
 
 	} else {
 		bearing_sector_end = arc_start_bearing;
 		bearing_sector_start = arc_start_bearing - arc_sweep;
 
-		if (bearing_sector_start < 0.0f) bearing_sector_start += M_TWOPI_F;
+		if (bearing_sector_start < 0.0f) { bearing_sector_start += M_TWOPI_F; }
 	}
 
 	in_sector = false;
 
 	// Case where sector does not span zero
-	if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true;
+	if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) { in_sector = true; }
 
 	// Case where sector does span zero
-	if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true;
+	if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) { in_sector = true; }
 
 	// If in the sector then calculate distance and bearing to closest point
 	if (in_sector) {
@@ -442,16 +374,20 @@ __EXPORT float _wrap_pi(float bearing)
 	int c = 0;
 	while (bearing >= M_PI_F) {
 		bearing -= M_TWOPI_F;
-		if (c++ > 3)
+
+		if (c++ > 3) {
 			return NAN;
 		}
+	}
 
 	c = 0;
 	while (bearing < -M_PI_F) {
 		bearing += M_TWOPI_F;
-		if (c++ > 3)
+
+		if (c++ > 3) {
 			return NAN;
 		}
+	}
 
 	return bearing;
 }
@@ -466,16 +402,20 @@ __EXPORT float _wrap_2pi(float bearing)
 	int c = 0;
 	while (bearing >= M_TWOPI_F) {
 		bearing -= M_TWOPI_F;
-		if (c++ > 3)
+
+		if (c++ > 3) {
 			return NAN;
 		}
+	}
 
 	c = 0;
 	while (bearing < 0.0f) {
 		bearing += M_TWOPI_F;
-		if (c++ > 3)
+
+		if (c++ > 3) {
 			return NAN;
 		}
+	}
 
 	return bearing;
 }
@@ -490,16 +430,20 @@ __EXPORT float _wrap_180(float bearing)
 	int c = 0;
 	while (bearing >= 180.0f) {
 		bearing -= 360.0f;
-		if (c++ > 3)
+
+		if (c++ > 3) {
 			return NAN;
 		}
+	}
 
 	c = 0;
 	while (bearing < -180.0f) {
 		bearing += 360.0f;
-		if (c++ > 3)
+
+		if (c++ > 3) {
 			return NAN;
 		}
+	}
 
 	return bearing;
 }
@@ -514,16 +458,20 @@ __EXPORT float _wrap_360(float bearing)
 	int c = 0;
 	while (bearing >= 360.0f) {
 		bearing -= 360.0f;
-		if (c++ > 3)
+
+		if (c++ > 3) {
 			return NAN;
 		}
+	}
 
 	c = 0;
 	while (bearing < 0.0f) {
 		bearing += 360.0f;
-		if (c++ > 3)
+
+		if (c++ > 3) {
 			return NAN;
 		}
+	}
 
 	return bearing;
 }

src/lib/geo/geo.h

@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -42,6 +39,7 @@
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
  */
 
@@ -67,6 +65,14 @@ struct crosstrack_error_s {
 	float bearing;		// Bearing in radians to closest point on line/arc
 } ;
 
+/* lat/lon are in radians */
+struct map_projection_reference_s {
+	double lat;
+	double lon;
+	double sin_lat;
+	double cos_lat;
+};
+
 /**
  * Initializes the map transformation.
  *
@@ -74,7 +80,7 @@ struct crosstrack_error_s {
  * @param lat in degrees (47.1234567°, not 471234567°)
  * @param lon in degrees (8.1234567°, not 81234567°)
  */
-__EXPORT void map_projection_init(double lat_0, double lon_0);
+__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
 
 /**
  * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
@@ -83,7 +89,7 @@ __EXPORT void map_projection_init(double lat_0, double lon_0);
  * @param lat in degrees (47.1234567°, not 471234567°)
  * @param lon in degrees (8.1234567°, not 81234567°)
  */
-__EXPORT void map_projection_project(double lat, double lon, float *x, float *y);
+__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
 
 /**
  * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
@@ -93,7 +99,7 @@ __EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
  * @param lat in degrees (47.1234567°, not 471234567°)
  * @param lon in degrees (8.1234567°, not 81234567°)
  */
-__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon);
+__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
 
 /**
  * Returns the distance to the next waypoint in meters.
@@ -115,15 +121,15 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
  */
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
-__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e);
+__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e);
 
-__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e);
+__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e);
 
 __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res);
 
-__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
+__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
 
-__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
+__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
 				 float radius, float arc_start_bearing, float arc_sweep);
 
 /*

src/modules/att_pos_estimator_ekf/KalmanNav.cpp

@@ -97,6 +97,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 {
 	using namespace math;
 
+	memset(&ref, 0, sizeof(ref));
+
 	F.zero();
 	G.zero();
 	V.zero();
@@ -247,11 +249,7 @@ void KalmanNav::update()
 		lat0 = lat;
 		lon0 = lon;
 		alt0 = alt;
-		// XXX map_projection has internal global
+		map_projection_init(&ref, lat0, lon0);
-		// states that multiple things could change, 
-		// should make map_projection take reference
-		// lat/lon and not have init
-		map_projection_init(lat0, lon0);
 		_positionInitialized = true;
 		warnx("initialized EKF state with GPS");
 		warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
@@ -314,7 +312,6 @@ void KalmanNav::updatePublications()
 	// global position publication
 	_pos.timestamp = _pubTimeStamp;
 	_pos.time_gps_usec = _gps.timestamp_position;
-	_pos.global_valid = true;
 	_pos.lat = lat * M_RAD_TO_DEG;
 	_pos.lon = lon * M_RAD_TO_DEG;
 	_pos.alt = float(alt);
@@ -327,7 +324,7 @@ void KalmanNav::updatePublications()
 	float x;
 	float y;
 	bool landed = alt < (alt0 + 0.1); // XXX improve?
-	map_projection_project(lat, lon, &x, &y); 
+	map_projection_project(&ref, lat, lon, &x, &y);
 	_localPos.timestamp = _pubTimeStamp;
 	_localPos.xy_valid = true;
 	_localPos.z_valid = true;
@@ -343,8 +340,8 @@ void KalmanNav::updatePublications()
 	_localPos.xy_global = true;
 	_localPos.z_global = true;
 	_localPos.ref_timestamp = _pubTimeStamp;
-	_localPos.ref_lat = getLatDegE7();
+	_localPos.ref_lat = lat * M_RAD_TO_DEG;
-	_localPos.ref_lon = getLonDegE7();
+	_localPos.ref_lon = lon * M_RAD_TO_DEG;
 	_localPos.ref_alt = 0;
 	_localPos.landed = landed;
 

src/modules/att_pos_estimator_ekf/KalmanNav.hpp

@@ -49,6 +49,7 @@
 #include <controllib/block/BlockParam.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/Publication.hpp>
+#include <lib/geo/geo.h>
 
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -164,6 +165,7 @@ protected:
 	// parameters
 	float alt;                   		/**< altitude, meters */
 	double lat0, lon0;                   	/**< reference latitude and longitude */
+	struct map_projection_reference_s ref;	/**< local projection reference */
 	float alt0;                   		/**<  refeerence altitude (ground height) */
 	control::BlockParamFloat _vGyro;      /**< gyro process noise */
 	control::BlockParamFloat _vAccel;     /**< accelerometer process noise  */

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -407,7 +407,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 							vel(2) = gps.vel_d_m_s;
 						}
 
-					} else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) {
+					} else if (ekf_params.acc_comp == 2 && gps.eph_m < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
 						vel_valid = true;
 						if (global_pos_updated) {
 							vel_t = global_pos.timestamp;

src/modules/commander/commander.cpp

@@ -67,6 +67,7 @@
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/subsystem_info.h>
@@ -112,12 +113,11 @@ extern struct system_load_s system_load;
 
 #define MAVLINK_OPEN_INTERVAL 50000
 
-#define STICK_ON_OFF_LIMIT 0.75f
+#define STICK_ON_OFF_LIMIT 0.9f
-#define STICK_THRUST_RANGE 1.0f
 #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
-#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
+#define POSITION_TIMEOUT 100000 /**< consider the local or global position estimate invalid after 100ms */
 #define RC_TIMEOUT 500000
 #define DIFFPRESS_TIMEOUT 2000000
 
@@ -153,6 +153,7 @@ static bool on_usb_power = false;
 
 static float takeoff_alt = 5.0f;
 static int parachute_enabled = 0;
+static float eph_epv_threshold = 5.0f;
 
 static struct vehicle_status_s status;
 static struct actuator_armed_s armed;
@@ -194,7 +195,7 @@ void usage(const char *reason);
 /**
  * React to commands that are sent e.g. from the mavlink module.
  */
-bool handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
+bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub);
 
 /**
  * Mainloop of commander.
@@ -207,7 +208,7 @@ void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool
 
 void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
 
-transition_result_t set_main_state_rc(struct vehicle_status_s *status);
+transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
 
 void set_control_mode();
 
@@ -378,7 +379,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armed
     return arming_res;
 }
 
-bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
+bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
 {
 	/* result of the command */
 	enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
@@ -536,6 +537,51 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 		break;
 
+	case VEHICLE_CMD_DO_SET_HOME: {
+			bool use_current = cmd->param1 > 0.5f;
+			if (use_current) {
+				/* use current position */
+				if (status->condition_global_position_valid) {
+					home->lat = global_pos->lat;
+					home->lon = global_pos->lon;
+					home->alt = global_pos->alt;
+
+					home->timestamp = hrt_absolute_time();
+
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+				} else {
+					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				}
+
+			} else {
+				/* use specified position */
+				home->lat = cmd->param5;
+				home->lon = cmd->param6;
+				home->alt = cmd->param7;
+
+				home->timestamp = hrt_absolute_time();
+
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+			}
+
+			if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
+				warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt);
+				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt);
+
+				/* announce new home position */
+				if (*home_pub > 0) {
+					orb_publish(ORB_ID(home_position), *home_pub, home);
+
+				} else {
+					*home_pub = orb_advertise(ORB_ID(home_position), home);
+				}
+
+				/* mark home position as set */
+				status->condition_home_position_valid = true;
+			}
+		}
+		break;
 	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
 	case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
 	case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
@@ -568,6 +614,7 @@ int commander_thread_main(int argc, char *argv[])
 	commander_initialized = false;
 
 	bool arm_tune_played = false;
+	bool was_armed = false;
 
 	/* set parameters */
 	param_t _param_sys_type = param_find("MAV_TYPE");
@@ -771,6 +818,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct subsystem_info_s info;
 	memset(&info, 0, sizeof(info));
 
+	/* Subscribe to position setpoint triplet */
+	int pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+	struct position_setpoint_triplet_s pos_sp_triplet;
+	memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
+
 	control_status_leds(&status, &armed, true);
 
 	/* now initialized */
@@ -876,7 +928,45 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* update condition_global_position_valid */
-		check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed);
+		/* hysteresis for EPH/EPV */
+		bool eph_epv_good;
+		if (status.condition_global_position_valid) {
+			if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) {
+				eph_epv_good = false;
+			}
+
+		} else {
+			if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) {
+				eph_epv_good = true;
+			}
+		}
+		check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed);
+
+		/* check if GPS fix is ok */
+
+		/* update home position */
+		if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
+			(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
+
+			home.lat = global_position.lat;
+			home.lon = global_position.lon;
+			home.alt = global_position.alt;
+
+			warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
+			mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
+
+			/* announce new home position */
+			if (home_pub > 0) {
+				orb_publish(ORB_ID(home_position), home_pub, &home);
+
+			} else {
+				home_pub = orb_advertise(ORB_ID(home_position), &home);
+			}
+
+			/* mark home position as set */
+			status.condition_home_position_valid = true;
+			tune_positive(true);
+		}
 
 		/* update local position estimate */
 		orb_check(local_position_sub, &updated);
@@ -887,7 +977,7 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* update condition_local_position_valid and condition_local_altitude_valid */
-		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed);
+		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed);
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
 
 		static bool published_condition_landed_fw = false;
@@ -962,6 +1052,13 @@ int commander_thread_main(int argc, char *argv[])
 			status_changed = true;
 		}
 
+		/* update position setpoint triplet */
+		orb_check(pos_sp_triplet_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
+		}
+
 		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
 			/* compute system load */
 			uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
@@ -1024,45 +1121,6 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
-			/* check if GPS fix is ok */
-			float hdop_threshold_m = 4.0f;
-			float vdop_threshold_m = 8.0f;
-
-			/*
-			 * If horizontal dilution of precision (hdop / eph)
-			 * and vertical diluation of precision (vdop / epv)
-			 * are below a certain threshold (e.g. 4 m), AND
-			 * home position is not yet set AND the last GPS
-			 * GPS measurement is not older than two seconds AND
-			 * the system is currently not armed, set home
-			 * position to the current position.
-			 */
-
-			if (!status.condition_home_position_valid && gps_position.fix_type >= 3 &&
-			    (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&
-			    (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed
-			    && global_position.global_valid) {
-
-				/* copy position data to uORB home message, store it locally as well */
-				home.lat = global_position.lat;
-				home.lon = global_position.lon;
-				home.alt = global_position.alt;
-
-				warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt);
-				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt);
-
-				/* announce new home position */
-				if (home_pub > 0) {
-					orb_publish(ORB_ID(home_position), home_pub, &home);
-
-				} else {
-					home_pub = orb_advertise(ORB_ID(home_position), &home);
-				}
-
-				/* mark home position as set */
-				status.condition_home_position_valid = true;
-				tune_positive(true);
-			}
 		}
 
 		/* start RC input check */
@@ -1092,7 +1150,7 @@ int commander_thread_main(int argc, char *argv[])
 			if (status.is_rotary_wing &&
 			    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
 			    (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
-			    sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+			    sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {
 
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
@@ -1110,7 +1168,7 @@ int commander_thread_main(int argc, char *argv[])
 
 			/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
 			if (status.arming_state == ARMING_STATE_STANDBY &&
-			    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+			    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {
 				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
 						print_reject_arm("NOT ARMING: Press safety switch first.");
@@ -1150,11 +1208,8 @@ int commander_thread_main(int argc, char *argv[])
 				transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
 			}
 
-			/* fill status according to mode switches */
-			check_mode_switches(&sp_man, &status);
-
 			/* evaluate the main state machine according to mode switches */
-			res = set_main_state_rc(&status);
+			res = set_main_state_rc(&status, &sp_man);
 
 			/* play tune on mode change only if armed, blink LED always */
 			if (res == TRANSITION_CHANGED) {
@@ -1165,6 +1220,33 @@ int commander_thread_main(int argc, char *argv[])
 				mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
 			}
 
+			/* set navigation state */
+			/* RETURN switch, overrides MISSION switch */
+			if (sp_man.return_switch == SWITCH_POS_ON) {
+				/* switch to RTL if not already landed after RTL and home position set */
+				status.set_nav_state = NAV_STATE_RTL;
+				status.set_nav_state_timestamp = hrt_absolute_time();
+
+			} else {
+				/* MISSION switch */
+				if (sp_man.loiter_switch == SWITCH_POS_ON) {
+					/* stick is in LOITER position */
+					status.set_nav_state = NAV_STATE_LOITER;
+					status.set_nav_state_timestamp = hrt_absolute_time();
+
+				} else if (sp_man.loiter_switch != SWITCH_POS_NONE) {
+					/* stick is in MISSION position */
+					status.set_nav_state = NAV_STATE_MISSION;
+					status.set_nav_state_timestamp = hrt_absolute_time();
+
+				} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
+						pos_sp_triplet.nav_state == NAV_STATE_RTL) {
+					/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
+					status.set_nav_state = NAV_STATE_MISSION;
+					status.set_nav_state_timestamp = hrt_absolute_time();
+				}
+			}
+
 		} else {
 			if (!status.rc_signal_lost) {
 				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
@@ -1189,10 +1271,15 @@ int commander_thread_main(int argc, char *argv[])
 
 				} else {
 					/* failsafe for manual modes */
-					transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
+					transition_result_t res = TRANSITION_DENIED;
+
+					if (!status.condition_landed) {
+						/* vehicle is not landed, try to perform RTL */
+						res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
+					}
 
 					if (res == TRANSITION_DENIED) {
-						/* RTL not allowed (no global position estimate), try LAND */
+						/* RTL not allowed (no global position estimate) or not wanted, try LAND */
 						res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
 
 						if (res == TRANSITION_DENIED) {
@@ -1212,7 +1299,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		// TODO remove this hack
 		/* flight termination in manual mode if assisted switch is on easy position */
-		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
+		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) {
 			if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
 				tune_positive(armed.armed);
 			}
@@ -1226,7 +1313,7 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
 
 			/* handle it */
-			if (handle_command(&status, &safety, &cmd, &armed))
+			if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub))
 				status_changed = true;
 		}
 
@@ -1241,8 +1328,33 @@ int commander_thread_main(int argc, char *argv[])
 		if (arming_state_changed) {
 			status_changed = true;
 			mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]);
+
+			/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
+			if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
+				(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
+
+				// TODO remove code duplication
+				home.lat = global_position.lat;
+				home.lon = global_position.lon;
+				home.alt = global_position.alt;
+
+				warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
+				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
+
+				/* announce new home position */
+				if (home_pub > 0) {
+					orb_publish(ORB_ID(home_position), home_pub, &home);
+
+				} else {
+					home_pub = orb_advertise(ORB_ID(home_position), &home);
 				}
 
+				/* mark home position as set */
+				status.condition_home_position_valid = true;
+			}
+		}
+		was_armed = armed.armed;
+
 		if (main_state_changed) {
 			status_changed = true;
 			mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
@@ -1427,76 +1539,29 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
 	leds_counter++;
 }
 
-void
-check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status)
-{
-	/* main mode switch */
-	if (!isfinite(sp_man->mode_switch)) {
-		/* default to manual if signal is invalid */
-		status->mode_switch = MODE_SWITCH_MANUAL;
-
-	} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
-		status->mode_switch = MODE_SWITCH_AUTO;
-
-	} else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) {
-		status->mode_switch = MODE_SWITCH_MANUAL;
-
-	} else {
-		status->mode_switch = MODE_SWITCH_ASSISTED;
-	}
-
-	/* return switch */
-	if (!isfinite(sp_man->return_switch)) {
-		status->return_switch = RETURN_SWITCH_NONE;
-
-	} else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) {
-		status->return_switch = RETURN_SWITCH_RETURN;
-
-	} else {
-		status->return_switch = RETURN_SWITCH_NORMAL;
-	}
-
-	/* assisted switch */
-	if (!isfinite(sp_man->assisted_switch)) {
-		status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
-
-	} else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) {
-		status->assisted_switch = ASSISTED_SWITCH_EASY;
-
-	} else {
-		status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
-	}
-
-	/* mission switch  */
-	if (!isfinite(sp_man->mission_switch)) {
-		status->mission_switch = MISSION_SWITCH_NONE;
-
-	} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
-		status->mission_switch = MISSION_SWITCH_LOITER;
-
-	} else {
-		status->mission_switch = MISSION_SWITCH_MISSION;
-	}
-}
-
 transition_result_t
-set_main_state_rc(struct vehicle_status_s *status)
+set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man)
 {
 	/* set main state according to RC switches */
 	transition_result_t res = TRANSITION_DENIED;
 
-	switch (status->mode_switch) {
+	switch (sp_man->mode_switch) {
-	case MODE_SWITCH_MANUAL:
+	case SWITCH_POS_NONE:
+		res = TRANSITION_NOT_CHANGED;
+		break;
+
+	case SWITCH_POS_OFF:		// MANUAL
 		res = main_state_transition(status, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
 
-	case MODE_SWITCH_ASSISTED:
+	case SWITCH_POS_MIDDLE:		// ASSISTED
-		if (status->assisted_switch == ASSISTED_SWITCH_EASY) {
+		if (sp_man->assisted_switch == SWITCH_POS_ON) {
 			res = main_state_transition(status, MAIN_STATE_EASY);
 
-			if (res != TRANSITION_DENIED)
+			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
+			}
 
 			// else fallback to SEATBELT
 			print_reject_mode(status, "EASY");
@@ -1504,29 +1569,33 @@ set_main_state_rc(struct vehicle_status_s *status)
 
 		res = main_state_transition(status, MAIN_STATE_SEATBELT);
 
-		if (res != TRANSITION_DENIED)
+		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this mode
+		}
 
-		if (status->assisted_switch != ASSISTED_SWITCH_EASY)	// don't print both messages
+		if (sp_man->assisted_switch != SWITCH_POS_ON) {
 			print_reject_mode(status, "SEATBELT");
+		}
 
 		// else fallback to MANUAL
 		res = main_state_transition(status, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
 
-	case MODE_SWITCH_AUTO:
+	case SWITCH_POS_ON:			// AUTO
 		res = main_state_transition(status, MAIN_STATE_AUTO);
 
-		if (res != TRANSITION_DENIED)
+		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this state
+		}
 
 		// else fallback to SEATBELT (EASY likely will not work too)
 		print_reject_mode(status, "AUTO");
 		res = main_state_transition(status, MAIN_STATE_SEATBELT);
 
-		if (res != TRANSITION_DENIED)
+		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this state
+		}
 
 		// else fallback to MANUAL
 		res = main_state_transition(status, MAIN_STATE_MANUAL);
@@ -1630,8 +1699,11 @@ set_control_mode()
 		control_mode.flag_control_auto_enabled = true;
 		control_mode.flag_control_rates_enabled = true;
 		control_mode.flag_control_attitude_enabled = true;
-		control_mode.flag_control_position_enabled = true;
+
-		control_mode.flag_control_velocity_enabled = true;
+		/* in failsafe LAND mode position may be not available */
+		control_mode.flag_control_position_enabled = status.condition_local_position_valid;
+		control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
+
 		control_mode.flag_control_altitude_enabled = true;
 		control_mode.flag_control_climb_rate_enabled = true;
 	}

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -173,6 +173,8 @@ private:
 		float pitchsp_offset_deg;			/**< Pitch Setpoint Offset in deg */
 		float rollsp_offset_rad;			/**< Roll Setpoint Offset in rad */
 		float pitchsp_offset_rad;			/**< Pitch Setpoint Offset in rad */
+		float man_roll_max;						/**< Max Roll in rad */
+		float man_pitch_max;					/**< Max Pitch in rad */
 
 	}		_parameters;			/**< local copies of interesting parameters */
 
@@ -211,6 +213,8 @@ private:
 		param_t trim_yaw;
 		param_t rollsp_offset_deg;
 		param_t pitchsp_offset_deg;
+		param_t man_roll_max;
+		param_t man_pitch_max;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -354,6 +358,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
 	_parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");
 
+	_parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
+	_parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -421,6 +428,10 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
 	_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
 	_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
+	param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max));
+	param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max));
+	_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
+	_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
 
 
 	/* pitch control parameters */
@@ -706,8 +717,8 @@ FixedwingAttitudeControl::task_main()
 					 * the intended attitude setpoint. Later, after the rate control step the
 					 * trim is added again to the control signal.
 					 */
-					roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset_rad;
+					roll_sp = (_manual.roll * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad;
-					pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset_rad;
+					pitch_sp = (-_manual.pitch * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad;
 					throttle_sp = _manual.throttle;
 					_actuators.control[4] = _manual.flaps;
 
@@ -815,7 +826,7 @@ FixedwingAttitudeControl::task_main()
 			} else {
 				/* manual/direct control */
 				_actuators.control[0] = _manual.roll;
-				_actuators.control[1] = _manual.pitch;
+				_actuators.control[1] = -_manual.pitch;
 				_actuators.control[2] = _manual.yaw;
 				_actuators.control[3] = _manual.throttle;
 				_actuators.control[4] = _manual.flaps;

src/modules/fw_att_control/fw_att_control_params.c

@@ -186,3 +186,13 @@ PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
 // @Description		An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe
 // @Range		-90.0 to 90.0
 PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);
+
+// @DisplayName		Max Manual Roll
+// @Description		Max roll for manual control in attitude stabilized mode
+// @Range		0.0 to 90.0
+PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
+
+// @DisplayName		Max Manual Pitch
+// @Description		Max pitch for manual control in attitude stabilized mode
+// @Range		0.0 to 90.0
+PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);

src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp

@@ -177,6 +177,8 @@ private:
 	struct sensor_combined_s			_sensor_combined;
 #endif
 
+	struct map_projection_reference_s	_pos_ref;
+
 	float						_baro_ref;		/**< barometer reference altitude */
 	float						_baro_gps_offset;	/**< offset between GPS and baro */
 
@@ -821,7 +823,7 @@ FixedwingEstimator::task_main()
 					_baro_gps_offset = _baro_ref - _local_pos.ref_alt;
 
 					// XXX this is not multithreading safe
-					map_projection_init(lat, lon);
+					map_projection_init(&_pos_ref, lat, lon);
 					mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
 
 					_gps_initialized = true;
@@ -1042,18 +1044,14 @@ FixedwingEstimator::task_main()
 
 				_global_pos.timestamp = _local_pos.timestamp;
 
-				_global_pos.baro_valid = true;
-				_global_pos.global_valid = true;
-
 				if (_local_pos.xy_global) {
 					double est_lat, est_lon;
-					map_projection_reproject(_local_pos.x, _local_pos.y, &est_lat, &est_lon);
+					map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
 					_global_pos.lat = est_lat;
 					_global_pos.lon = est_lon;
 					_global_pos.time_gps_usec = _gps.time_gps_usec;
 				}
 
-				/* set valid values even if position is not valid */
 				if (_local_pos.v_xy_valid) {
 					_global_pos.vel_n = _local_pos.vx;
 					_global_pos.vel_e = _local_pos.vy;
@@ -1065,16 +1063,15 @@ FixedwingEstimator::task_main()
 				/* local pos alt is negative, change sign and add alt offset */
 				_global_pos.alt = _local_pos.ref_alt + (-_local_pos.z);
 
-				if (_local_pos.z_valid) {
-					_global_pos.baro_alt = _local_pos.ref_alt - _baro_gps_offset - _local_pos.z;
-				}
-
 				if (_local_pos.v_z_valid) {
 					_global_pos.vel_d = _local_pos.vz;
 				}
 
 				_global_pos.yaw = _local_pos.yaw;
 
+				_global_pos.eph = _gps.eph_m;
+				_global_pos.epv = _gps.epv_m;
+
 				_global_pos.timestamp = _local_pos.timestamp;
 
 				/* lazily publish the global position only once available */

src/modules/mavlink/mavlink_receiver.cpp

@@ -88,8 +88,6 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
 MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_mavlink(parent),
 
-	_manual_sub(-1),
-
 	_global_pos_pub(-1),
 	_local_pos_pub(-1),
 	_attitude_pub(-1),
@@ -259,6 +257,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
 	memset(&f, 0, sizeof(f));
 
 	f.timestamp = hrt_absolute_time();
+	f.flow_timestamp = flow.time_usec;
 	f.flow_raw_x = flow.flow_x;
 	f.flow_raw_y = flow.flow_y;
 	f.flow_comp_x_m = flow.flow_comp_m_x;
@@ -429,35 +428,9 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 	mavlink_manual_control_t man;
 	mavlink_msg_manual_control_decode(msg, &man);
 
-	/* rc channels */
-	{
-		struct rc_channels_s rc;
-		memset(&rc, 0, sizeof(rc));
-
-		rc.timestamp = hrt_absolute_time();
-		rc.chan_count = 4;
-
-		rc.chan[0].scaled = man.x / 1000.0f;
-		rc.chan[1].scaled = man.y / 1000.0f;
-		rc.chan[2].scaled = man.r / 1000.0f;
-		rc.chan[3].scaled = man.z / 1000.0f;
-
-		if (_rc_pub < 0) {
-			_rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
-
-		} else {
-			orb_publish(ORB_ID(rc_channels), _rc_pub, &rc);
-		}
-	}
-
-	/* manual control */
-	{
 	struct manual_control_setpoint_s manual;
 	memset(&manual, 0, sizeof(manual));
 
-		/* get a copy first, to prevent altering values that are not sent by the mavlink command */
-		orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &manual);
-
 	manual.timestamp = hrt_absolute_time();
 	manual.roll = man.x / 1000.0f;
 	manual.pitch = man.y / 1000.0f;
@@ -470,7 +443,6 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 	} else {
 		orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
 	}
-	}
 }
 
 void
@@ -780,7 +752,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		memset(&hil_global_pos, 0, sizeof(hil_global_pos));
 
 		hil_global_pos.timestamp = timestamp;
-		hil_global_pos.global_valid = true;
 		hil_global_pos.lat = hil_state.lat;
 		hil_global_pos.lon = hil_state.lon;
 		hil_global_pos.alt = hil_state.alt / 1000.0f;
@@ -788,6 +759,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_global_pos.vel_e = hil_state.vy / 100.0f;
 		hil_global_pos.vel_d = hil_state.vz / 100.0f;
 		hil_global_pos.yaw = hil_attitude.yaw;
+		hil_global_pos.eph = 2.0f;
+		hil_global_pos.epv = 4.0f;
 
 		if (_global_pos_pub < 0) {
 			_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
@@ -799,19 +772,22 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 
 	/* local position */
 	{
+		double lat = hil_state.lat * 1e-7;
+		double lon = hil_state.lon * 1e-7;
+
 		if (!_hil_local_proj_inited) {
 			_hil_local_proj_inited = true;
 			_hil_local_alt0 = hil_state.alt / 1000.0f;
-			map_projection_init(hil_state.lat, hil_state.lon);
+			map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon);
 			hil_local_pos.ref_timestamp = timestamp;
-			hil_local_pos.ref_lat = hil_state.lat;
+			hil_local_pos.ref_lat = lat;
-			hil_local_pos.ref_lon = hil_state.lon;
+			hil_local_pos.ref_lon = lon;
 			hil_local_pos.ref_alt = _hil_local_alt0;
 		}
 
 		float x;
 		float y;
-		map_projection_project(hil_state.lat * 1e-7, hil_state.lon * 1e-7, &x, &y);
+		map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
 		hil_local_pos.timestamp = timestamp;
 		hil_local_pos.xy_valid = true;
 		hil_local_pos.z_valid = true;
@@ -899,8 +875,6 @@ MavlinkReceiver::receive_thread(void *arg)
 	sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
 	prctl(PR_SET_NAME, thread_name, getpid());
 
-	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-
 	struct pollfd fds[1];
 	fds[0].fd = uart_fd;
 	fds[0].events = POLLIN;

src/modules/mavlink/mavlink_receiver.h

@@ -138,9 +138,9 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
-	int _manual_sub;
 	int _hil_frames;
 	uint64_t _old_timestamp;
 	bool _hil_local_proj_inited;
 	float _hil_local_alt0;
+	struct map_projection_reference_s _hil_local_proj_ref;
 };

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -157,7 +157,9 @@ private:
 		param_t yaw_rate_d;
 		param_t yaw_ff;
 
-		param_t rc_scale_yaw;
+		param_t man_roll_max;
+		param_t man_pitch_max;
+		param_t man_yaw_max;
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	struct {
@@ -167,7 +169,9 @@ private:
 		math::Vector<3> rate_d;				/**< D gain for angular rate error */
 		float yaw_ff;						/**< yaw control feed-forward */
 
-		float rc_scale_yaw;
+		float man_roll_max;
+		float man_pitch_max;
+		float man_yaw_max;
 	}		_params;
 
 	/**
@@ -295,7 +299,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.yaw_rate_d		= 	param_find("MC_YAWRATE_D");
 	_params_handles.yaw_ff			= 	param_find("MC_YAW_FF");
 
-	_params_handles.rc_scale_yaw	= 	param_find("RC_SCALE_YAW");
+	_params_handles.man_roll_max	= 	param_find("MC_MAN_R_MAX");
+	_params_handles.man_pitch_max	= 	param_find("MC_MAN_P_MAX");
+	_params_handles.man_yaw_max		= 	param_find("MC_MAN_Y_MAX");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -330,7 +336,7 @@ MulticopterAttitudeControl::parameters_update()
 {
 	float v;
 
-	/* roll */
+	/* roll gains */
 	param_get(_params_handles.roll_p, &v);
 	_params.att_p(0) = v;
 	param_get(_params_handles.roll_rate_p, &v);
@@ -340,7 +346,7 @@ MulticopterAttitudeControl::parameters_update()
 	param_get(_params_handles.roll_rate_d, &v);
 	_params.rate_d(0) = v;
 
-	/* pitch */
+	/* pitch gains */
 	param_get(_params_handles.pitch_p, &v);
 	_params.att_p(1) = v;
 	param_get(_params_handles.pitch_rate_p, &v);
@@ -350,7 +356,7 @@ MulticopterAttitudeControl::parameters_update()
 	param_get(_params_handles.pitch_rate_d, &v);
 	_params.rate_d(1) = v;
 
-	/* yaw */
+	/* yaw gains */
 	param_get(_params_handles.yaw_p, &v);
 	_params.att_p(2) = v;
 	param_get(_params_handles.yaw_rate_p, &v);
@@ -362,7 +368,14 @@ MulticopterAttitudeControl::parameters_update()
 
 	param_get(_params_handles.yaw_ff, &_params.yaw_ff);
 
-	param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw);
+	/* manual control scale */
+	param_get(_params_handles.man_roll_max, &_params.man_roll_max);
+	param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
+	param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
+
+	_params.man_roll_max *= M_PI / 180.0;
+	_params.man_pitch_max *= M_PI / 180.0;
+	_params.man_yaw_max *= M_PI / 180.0;
 
 	return OK;
 }
@@ -404,7 +417,6 @@ MulticopterAttitudeControl::vehicle_manual_poll()
 	orb_check(_manual_control_sp_sub, &updated);
 
 	if (updated) {
-
 		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
 	}
 }
@@ -483,25 +495,11 @@ MulticopterAttitudeControl::control_attitude(float dt)
 			//	reset_yaw_sp = true;
 			//}
 		} else {
-			float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw;
-
-			if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
 			/* move yaw setpoint */
-				yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw;
+			_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + _manual_control_sp.yaw * _params.man_yaw_max * dt);
-
-				if (_manual_control_sp.yaw > 0.0f) {
-					yaw_sp_move_rate -= YAW_DEADZONE;
-
-				} else {
-					yaw_sp_move_rate += YAW_DEADZONE;
-				}
-
-				yaw_sp_move_rate *= _params.rc_scale_yaw;
-				_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
 			_v_att_sp.R_valid = false;
 			publish_att_sp = true;
 		}
-		}
 
 		/* reset yaw setpint to current position if needed */
 		if (_reset_yaw_sp) {
@@ -513,8 +511,8 @@ MulticopterAttitudeControl::control_attitude(float dt)
 
 		if (!_v_control_mode.flag_control_velocity_enabled) {
 			/* update attitude setpoint if not in position control mode */
-			_v_att_sp.roll_body = _manual_control_sp.roll;
+			_v_att_sp.roll_body = _manual_control_sp.roll * _params.man_roll_max;
-			_v_att_sp.pitch_body = _manual_control_sp.pitch;
+			_v_att_sp.pitch_body = -_manual_control_sp.pitch * _params.man_pitch_max;
 			_v_att_sp.R_valid = false;
 			publish_att_sp = true;
 		}

src/modules/mc_att_control/mc_att_control_params.c

@@ -173,3 +173,32 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
  * @group Multicopter Attitude Control
  */
 PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
+
+/**
+ * Max manual roll
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f);
+
+/**
+ * Max manual pitch
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f);
+
+/**
+ * Max manual yaw rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f);

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
- *   Author: @author Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,6 +39,8 @@
  * Output of velocity controller is thrust vector that splitted to thrust direction
  * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself).
  * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <nuttx/config.h>
@@ -62,9 +63,10 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
@@ -114,20 +116,21 @@ private:
 	int 	_params_sub;			/**< notification of parameter updates */
 	int 	_manual_sub;			/**< notification of manual control updates */
 	int		_arming_sub;			/**< arming status of outputs */
-	int		_global_pos_sub;			/**< vehicle local position */
+	int		_local_pos_sub;			/**< vehicle local position */
 	int		_pos_sp_triplet_sub;		/**< position setpoint triplet */
 
 	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
-	orb_advert_t	_pos_sp_triplet_pub;	/**< position setpoint triplet publication */
+	orb_advert_t	_local_pos_sp_pub;		/**< vehicle local position setpoint publication */
-	orb_advert_t	_global_vel_sp_pub;		/**< vehicle global velocity setpoint */
+	orb_advert_t	_global_vel_sp_pub;		/**< vehicle global velocity setpoint publication */
 
 	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
 	struct vehicle_attitude_setpoint_s	_att_sp;		/**< vehicle attitude setpoint */
 	struct manual_control_setpoint_s	_manual;		/**< r/c channel data */
 	struct vehicle_control_mode_s		_control_mode;	/**< vehicle control mode */
 	struct actuator_armed_s				_arming;		/**< actuator arming status */
-	struct vehicle_global_position_s		_global_pos;	/**< vehicle global position */
+	struct vehicle_local_position_s		_local_pos;		/**< vehicle local position */
 	struct position_setpoint_triplet_s		_pos_sp_triplet;	/**< vehicle global position setpoint triplet */
+	struct vehicle_local_position_setpoint_s	_local_pos_sp;		/**< vehicle local position setpoint */
 	struct vehicle_global_velocity_setpoint_s	_global_vel_sp;	/**< vehicle global velocity setpoint */
 
 	struct {
@@ -148,9 +151,6 @@ private:
 		param_t tilt_max;
 		param_t land_speed;
 		param_t land_tilt_max;
-
-		param_t rc_scale_pitch;
-		param_t rc_scale_roll;
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	struct {
@@ -160,9 +160,6 @@ private:
 		float land_speed;
 		float land_tilt_max;
 
-		float rc_scale_pitch;
-		float rc_scale_roll;
-
 		math::Vector<3> pos_p;
 		math::Vector<3> vel_p;
 		math::Vector<3> vel_i;
@@ -172,14 +169,15 @@ private:
 		math::Vector<3> sp_offs_max;
 	}		_params;
 
-	double _lat_sp;
+	struct map_projection_reference_s _ref_pos;
-	double _lon_sp;
+	float _ref_alt;
-	float _alt_sp;
+	hrt_abstime _ref_timestamp;
 
-	bool _reset_lat_lon_sp;
+	bool _reset_pos_sp;
 	bool _reset_alt_sp;
-	bool _use_global_alt;			/**< switch between global (AMSL) and barometric altitudes */
 
+	math::Vector<3> _pos;
+	math::Vector<3> _pos_sp;
 	math::Vector<3> _vel;
 	math::Vector<3> _vel_sp;
 	math::Vector<3> _vel_prev;			/**< velocity on previous step */
@@ -202,9 +200,13 @@ private:
 	static float	scale_control(float ctl, float end, float dz);
 
 	/**
-	 * Reset lat/lon to current position
+	 * Update reference for local position projection
 	 */
-	void		reset_lat_lon_sp();
+	void		update_ref();
+	/**
+	 * Reset position setpoint to current position
+	 */
+	void		reset_pos_sp();
 
 	/**
 	 * Reset altitude setpoint to current altitude
@@ -252,31 +254,32 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params_sub(-1),
 	_manual_sub(-1),
 	_arming_sub(-1),
-	_global_pos_sub(-1),
+	_local_pos_sub(-1),
 	_pos_sp_triplet_sub(-1),
 
 /* publications */
 	_att_sp_pub(-1),
-	_pos_sp_triplet_pub(-1),
+	_local_pos_sp_pub(-1),
 	_global_vel_sp_pub(-1),
 
-	_lat_sp(0.0),
+	_ref_alt(0.0f),
-	_lon_sp(0.0),
+	_ref_timestamp(0),
-	_alt_sp(0.0f),
 
-	_reset_lat_lon_sp(true),
+	_reset_pos_sp(true),
-	_reset_alt_sp(true),
+	_reset_alt_sp(true)
-	_use_global_alt(false)
 {
 	memset(&_att, 0, sizeof(_att));
 	memset(&_att_sp, 0, sizeof(_att_sp));
 	memset(&_manual, 0, sizeof(_manual));
 	memset(&_control_mode, 0, sizeof(_control_mode));
 	memset(&_arming, 0, sizeof(_arming));
-	memset(&_global_pos, 0, sizeof(_global_pos));
+	memset(&_local_pos, 0, sizeof(_local_pos));
 	memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet));
+	memset(&_local_pos_sp, 0, sizeof(_local_pos_sp));
 	memset(&_global_vel_sp, 0, sizeof(_global_vel_sp));
 
+	memset(&_ref_pos, 0, sizeof(_ref_pos));
+
 	_params.pos_p.zero();
 	_params.vel_p.zero();
 	_params.vel_i.zero();
@@ -285,6 +288,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params.vel_ff.zero();
 	_params.sp_offs_max.zero();
 
+	_pos.zero();
+	_pos_sp.zero();
 	_vel.zero();
 	_vel_sp.zero();
 	_vel_prev.zero();
@@ -306,8 +311,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params_handles.tilt_max	= param_find("MPC_TILT_MAX");
 	_params_handles.land_speed	= param_find("MPC_LAND_SPEED");
 	_params_handles.land_tilt_max	= param_find("MPC_LAND_TILT");
-	_params_handles.rc_scale_pitch	= param_find("RC_SCALE_PITCH");
-	_params_handles.rc_scale_roll	= param_find("RC_SCALE_ROLL");
 
 	/* fetch initial parameter values */
 	parameters_update(true);
@@ -345,17 +348,18 @@ MulticopterPositionControl::parameters_update(bool force)
 
 	orb_check(_params_sub, &updated);
 
-	if (updated)
+	if (updated) {
 		orb_copy(ORB_ID(parameter_update), _params_sub, &param_upd);
+	}
 
 	if (updated || force) {
 		param_get(_params_handles.thr_min, &_params.thr_min);
 		param_get(_params_handles.thr_max, &_params.thr_max);
 		param_get(_params_handles.tilt_max, &_params.tilt_max);
+		_params.tilt_max = math::radians(_params.tilt_max);
 		param_get(_params_handles.land_speed, &_params.land_speed);
 		param_get(_params_handles.land_tilt_max, &_params.land_tilt_max);
-		param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch);
+		_params.land_tilt_max = math::radians(_params.land_tilt_max);
-		param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll);
 
 		float v;
 		param_get(_params_handles.xy_p, &v);
@@ -402,33 +406,39 @@ MulticopterPositionControl::poll_subscriptions()
 
 	orb_check(_att_sub, &updated);
 
-	if (updated)
+	if (updated) {
 		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
+	}
 
 	orb_check(_att_sp_sub, &updated);
 
-	if (updated)
+	if (updated) {
 		orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
+	}
 
 	orb_check(_control_mode_sub, &updated);
 
-	if (updated)
+	if (updated) {
 		orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
+	}
 
 	orb_check(_manual_sub, &updated);
 
-	if (updated)
+	if (updated) {
 		orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
+	}
 
 	orb_check(_arming_sub, &updated);
 
-	if (updated)
+	if (updated) {
 		orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
+	}
 
-	orb_check(_global_pos_sub, &updated);
+	orb_check(_local_pos_sub, &updated);
 
-	if (updated)
+	if (updated) {
-		orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
+		orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
+	}
 }
 
 float
@@ -452,13 +462,40 @@ MulticopterPositionControl::task_main_trampoline(int argc, char *argv[])
 }
 
 void
-MulticopterPositionControl::reset_lat_lon_sp()
+MulticopterPositionControl::update_ref()
 {
-	if (_reset_lat_lon_sp) {
+	if (_local_pos.ref_timestamp != _ref_timestamp) {
-		_reset_lat_lon_sp = false;
+		double lat_sp, lon_sp;
-		_lat_sp = _global_pos.lat;
+		float alt_sp;
-		_lon_sp = _global_pos.lon;
+
-		mavlink_log_info(_mavlink_fd, "[mpc] reset lat/lon sp: %.7f, %.7f", _lat_sp, _lon_sp);
+		if (_ref_timestamp != 0) {
+			/* calculate current position setpoint in global frame */
+			map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp);
+			alt_sp = _ref_alt - _pos_sp(2);
+		}
+
+		/* update local projection reference */
+		map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);
+		_ref_alt = _local_pos.ref_alt;
+
+		if (_ref_timestamp != 0) {
+			/* reproject position setpoint to new reference */
+			map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
+			_pos_sp(2) = -(alt_sp - _ref_alt);
+		}
+
+		_ref_timestamp = _local_pos.ref_timestamp;
+	}
+}
+
+void
+MulticopterPositionControl::reset_pos_sp()
+{
+	if (_reset_pos_sp) {
+		_reset_pos_sp = false;
+		_pos_sp(0) = _pos(0);
+		_pos_sp(1) = _pos(1);
+		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
 	}
 }
 
@@ -467,25 +504,8 @@ MulticopterPositionControl::reset_alt_sp()
 {
 	if (_reset_alt_sp) {
 		_reset_alt_sp = false;
-		_alt_sp = _use_global_alt ? _global_pos.alt : _global_pos.baro_alt;
+		_pos_sp(2) = _pos(2);
-		mavlink_log_info(_mavlink_fd, "[mpc] reset alt (%s) sp: %.2f", _use_global_alt ? "AMSL" : "baro", (double)_alt_sp);
+		mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
-	}
-}
-
-void
-MulticopterPositionControl::select_alt(bool global)
-{
-	if (global != _use_global_alt) {
-		_use_global_alt = global;
-
-		if (global) {
-			/* switch from barometric to global altitude */
-			_alt_sp += _global_pos.alt - _global_pos.baro_alt;
-
-		} else {
-			/* switch from global to barometric altitude */
-			_alt_sp += _global_pos.baro_alt - _global_pos.alt;
-		}
 	}
 }
 
@@ -506,7 +526,7 @@ MulticopterPositionControl::task_main()
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
-	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
 	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
 
 	parameters_update(true);
@@ -537,8 +557,7 @@ MulticopterPositionControl::task_main()
 	/* wakeup source */
 	struct pollfd fds[1];
 
-	/* Setup of loop */
+	fds[0].fd = _local_pos_sub;
-	fds[0].fd = _global_pos_sub;
 	fds[0].events = POLLIN;
 
 	while (!_task_should_exit) {
@@ -546,8 +565,9 @@ MulticopterPositionControl::task_main()
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
 
 		/* timed out - periodic check for _task_should_exit */
-		if (pret == 0)
+		if (pret == 0) {
 			continue;
+		}
 
 		/* this is undesirable but not much we can do */
 		if (pret < 0) {
@@ -564,7 +584,7 @@ MulticopterPositionControl::task_main()
 
 		if (_control_mode.flag_armed && !was_armed) {
 			/* reset setpoints and integrals on arming */
-			_reset_lat_lon_sp = true;
+			_reset_pos_sp = true;
 			_reset_alt_sp = true;
 			reset_int_z = true;
 			reset_int_xy = true;
@@ -572,28 +592,25 @@ MulticopterPositionControl::task_main()
 
 		was_armed = _control_mode.flag_armed;
 
+		update_ref();
+
 		if (_control_mode.flag_control_altitude_enabled ||
 		    _control_mode.flag_control_position_enabled ||
 		    _control_mode.flag_control_climb_rate_enabled ||
 		    _control_mode.flag_control_velocity_enabled) {
 
-			_vel(0) = _global_pos.vel_n;
+			_pos(0) = _local_pos.x;
-			_vel(1) = _global_pos.vel_e;
+			_pos(1) = _local_pos.y;
-			_vel(2) = _global_pos.vel_d;
+			_pos(2) = _local_pos.z;
+
+			_vel(0) = _local_pos.vx;
+			_vel(1) = _local_pos.vy;
+			_vel(2) = _local_pos.vz;
 
 			sp_move_rate.zero();
 
-			float alt = _global_pos.alt;
-
 			/* select control source */
 			if (_control_mode.flag_control_manual_enabled) {
-				/* select altitude source and update setpoint */
-				select_alt(_global_pos.global_valid);
-
-				if (!_use_global_alt) {
-					alt = _global_pos.baro_alt;
-				}
-
 				/* manual control */
 				if (_control_mode.flag_control_altitude_enabled) {
 					/* reset alt setpoint to current altitude if needed */
@@ -604,12 +621,12 @@ MulticopterPositionControl::task_main()
 				}
 
 				if (_control_mode.flag_control_position_enabled) {
-					/* reset lat/lon setpoint to current position if needed */
+					/* reset position setpoint to current position if needed */
-					reset_lat_lon_sp();
+					reset_pos_sp();
 
 					/* move position setpoint with roll/pitch stick */
-					sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz);
+					sp_move_rate(0) = _manual.pitch;
-					sp_move_rate(1) = scale_control(_manual.roll / _params.rc_scale_roll, 1.0f, pos_ctl_dz);
+					sp_move_rate(1) = _manual.roll;
 				}
 
 				/* limit setpoint move rate */
@@ -625,74 +642,47 @@ MulticopterPositionControl::task_main()
 				sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
 
 				/* move position setpoint */
-				add_vector_to_global_position(_lat_sp, _lon_sp, sp_move_rate(0) * dt, sp_move_rate(1) * dt, &_lat_sp, &_lon_sp);
+				_pos_sp += sp_move_rate * dt;
-				_alt_sp -= sp_move_rate(2) * dt;
 
 				/* check if position setpoint is too far from actual position */
 				math::Vector<3> pos_sp_offs;
 				pos_sp_offs.zero();
 
 				if (_control_mode.flag_control_position_enabled) {
-					get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_sp_offs.data[0], &pos_sp_offs.data[1]);
+					pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
-					pos_sp_offs(0) /= _params.sp_offs_max(0);
+					pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
-					pos_sp_offs(1) /= _params.sp_offs_max(1);
 				}
 
 				if (_control_mode.flag_control_altitude_enabled) {
-					pos_sp_offs(2) = -(_alt_sp - alt) / _params.sp_offs_max(2);
+					pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
 				}
 
 				float pos_sp_offs_norm = pos_sp_offs.length();
 
 				if (pos_sp_offs_norm > 1.0f) {
 					pos_sp_offs /= pos_sp_offs_norm;
-					add_vector_to_global_position(_global_pos.lat, _global_pos.lon, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp);
+					_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
-					_alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2);
-				}
-
-				/* fill position setpoint triplet */
-				_pos_sp_triplet.previous.valid = true;
-				_pos_sp_triplet.current.valid = true;
-				_pos_sp_triplet.next.valid = true;
-
-				_pos_sp_triplet.nav_state = NAV_STATE_NONE;
-				_pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL;
-				_pos_sp_triplet.current.lat = _lat_sp;
-				_pos_sp_triplet.current.lon = _lon_sp;
-				_pos_sp_triplet.current.alt = _alt_sp;
-				_pos_sp_triplet.current.yaw = _att_sp.yaw_body;
-				_pos_sp_triplet.current.loiter_radius = 0.0f;
-				_pos_sp_triplet.current.loiter_direction = 1.0f;
-				_pos_sp_triplet.current.pitch_min = 0.0f;
-
-				/* publish position setpoint triplet */
-				if (_pos_sp_triplet_pub > 0) {
-					orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
-
-				} else {
-					_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
 				}
 
 			} else {
-				/* always use AMSL altitude for AUTO */
-				select_alt(true);
-
 				/* AUTO */
 				bool updated;
 				orb_check(_pos_sp_triplet_sub, &updated);
 
-				if (updated)
+				if (updated) {
 					orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
+				}
 
 				if (_pos_sp_triplet.current.valid) {
 					/* in case of interrupted mission don't go to waypoint but stay at current position */
-					_reset_lat_lon_sp = true;
+					_reset_pos_sp = true;
 					_reset_alt_sp = true;
 
-					/* update position setpoint */
+					/* project setpoint to local frame */
-					_lat_sp = _pos_sp_triplet.current.lat;
+					map_projection_project(&_ref_pos,
-					_lon_sp = _pos_sp_triplet.current.lon;
+							       _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
-					_alt_sp = _pos_sp_triplet.current.alt;
+							       &_pos_sp.data[0], &_pos_sp.data[1]);
+					_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
 
 					/* update yaw setpoint if needed */
 					if (isfinite(_pos_sp_triplet.current.yaw)) {
@@ -701,11 +691,25 @@ MulticopterPositionControl::task_main()
 
 				} else {
 					/* no waypoint, loiter, reset position setpoint if needed */
-					reset_lat_lon_sp();
+					reset_pos_sp();
 					reset_alt_sp();
 				}
 			}
 
+			/* fill local position setpoint */
+			_local_pos_sp.x = _pos_sp(0);
+			_local_pos_sp.y = _pos_sp(1);
+			_local_pos_sp.z = _pos_sp(2);
+			_local_pos_sp.yaw = _att_sp.yaw_body;
+
+			/* publish local position setpoint */
+			if (_local_pos_sp_pub > 0) {
+				orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
+
+			} else {
+				_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
+			}
+
 			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
 				/* idle state, don't run controller and set zero thrust */
 				R.identity();
@@ -729,9 +733,7 @@ MulticopterPositionControl::task_main()
 
 			} else {
 				/* run position & altitude controllers, calculate velocity setpoint */
-				math::Vector<3> pos_err;
+				math::Vector<3> pos_err = _pos_sp - _pos;
-				get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]);
-				pos_err(2) = -(_alt_sp - alt);
 
 				_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
 
@@ -741,7 +743,7 @@ MulticopterPositionControl::task_main()
 				}
 
 				if (!_control_mode.flag_control_position_enabled) {
-					_reset_lat_lon_sp = true;
+					_reset_pos_sp = true;
 					_vel_sp(0) = 0.0f;
 					_vel_sp(1) = 0.0f;
 				}
@@ -847,9 +849,10 @@ MulticopterPositionControl::task_main()
 						/* limit max tilt and min lift when landing */
 						tilt_max = _params.land_tilt_max;
 
-						if (thr_min < 0.0f)
+						if (thr_min < 0.0f) {
 							thr_min = 0.0f;
 						}
+					}
 
 					/* limit min lift */
 					if (-thrust_sp(2) < thr_min) {
@@ -939,9 +942,10 @@ MulticopterPositionControl::task_main()
 						thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
 
 						/* protection against flipping on ground when landing */
-						if (thrust_int(2) > 0.0f)
+						if (thrust_int(2) > 0.0f) {
 							thrust_int(2) = 0.0f;
 						}
+					}
 
 					/* calculate attitude setpoint from thrust vector */
 					if (_control_mode.flag_control_velocity_enabled) {
@@ -999,6 +1003,18 @@ MulticopterPositionControl::task_main()
 						_att_sp.roll_body = euler(0);
 						_att_sp.pitch_body = euler(1);
 						/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
+
+					} else if (!_control_mode.flag_control_manual_enabled) {
+						/* autonomous altitude control without position control (failsafe landing),
+						 * force level attitude, don't change yaw */
+						R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
+
+						/* copy rotation matrix to attitude setpoint topic */
+						memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+						_att_sp.R_valid = true;
+
+						_att_sp.roll_body = 0.0f;
+						_att_sp.pitch_body = 0.0f;
 					}
 
 					_att_sp.thrust = thrust_abs;
@@ -1021,7 +1037,7 @@ MulticopterPositionControl::task_main()
 		} else {
 			/* position controller disabled, reset setpoints */
 			_reset_alt_sp = true;
-			_reset_lat_lon_sp = true;
+			_reset_pos_sp = true;
 			reset_int_z = true;
 			reset_int_xy = true;
 		}
@@ -1060,18 +1076,21 @@ MulticopterPositionControl::start()
 
 int mc_pos_control_main(int argc, char *argv[])
 {
-	if (argc < 1)
+	if (argc < 1) {
 		errx(1, "usage: mc_pos_control {start|stop|status}");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (pos_control::g_control != nullptr)
+		if (pos_control::g_control != nullptr) {
 			errx(1, "already running");
+		}
 
 		pos_control::g_control = new MulticopterPositionControl;
 
-		if (pos_control::g_control == nullptr)
+		if (pos_control::g_control == nullptr) {
 			errx(1, "alloc failed");
+		}
 
 		if (OK != pos_control::g_control->start()) {
 			delete pos_control::g_control;
@@ -1083,8 +1102,9 @@ int mc_pos_control_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "stop")) {
-		if (pos_control::g_control == nullptr)
+		if (pos_control::g_control == nullptr) {
 			errx(1, "not running");
+		}
 
 		delete pos_control::g_control;
 		pos_control::g_control = nullptr;

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -100,6 +100,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
  *
  * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY).
  *
+ * @unit m/s
  * @min 0.0
  * @group Multicopter Position Control
  */
@@ -155,6 +156,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
  *
  * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY).
  *
+ * @unit m/s
  * @min 0.0
  * @group Multicopter Position Control
  */
@@ -172,31 +174,35 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
 PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
 
 /**
- * Maximum tilt
+ * Maximum tilt angle in air
  *
- * Limits maximum tilt in AUTO and EASY modes.
+ * Limits maximum tilt in AUTO and EASY modes during flight.
  *
+ * @unit deg
  * @min 0.0
- * @max 1.57
+ * @max 90.0
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f);
+
+/**
+ * Maximum tilt during landing
+ *
+ * Limits maximum tilt angle on landing.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 15.0f);
 
 /**
  * Landing descend rate
  *
+ * @unit m/s
  * @min 0.0
  * @group Multicopter Position Control
  */
 PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f);
 
-/**
- * Maximum landing tilt
- *
- * Limits maximum tilt on landing.
- *
- * @min 0.0
- * @max 1.57
- * @group Multicopter Position Control
- */
-PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f);

src/modules/navigator/navigator_main.cpp

@@ -177,7 +177,7 @@ private:
 	class 		Mission				_mission;
 
 	bool		_mission_item_valid;		/**< current mission item valid */
-	bool		_global_pos_valid;		/**< track changes of global_position.global_valid flag */
+	bool		_global_pos_valid;		/**< track changes of global_position */
 	bool		_reset_loiter_pos;		/**< if true then loiter position should be set to current position */
 	bool		_waypoint_position_reached;
 	bool		_waypoint_yaw_reached;
@@ -690,47 +690,13 @@ Navigator::task_main()
 		if (fds[6].revents & POLLIN) {
 			vehicle_status_update();
 
-			/* evaluate state machine from commander and set the navigator mode accordingly */
+			/* evaluate state requested by commander */
 			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
-				bool stick_mode = false;
+				/* publish position setpoint triplet on each status update if navigator active */
+				_pos_sp_triplet_updated = true;
 
-				if (!_vstatus.rc_signal_lost) {
-					/* RC signal available, use control switches to set mode */
-					/* RETURN switch, overrides MISSION switch */
-					if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
-						/* switch to RTL if not already landed after RTL and home position set */
-						if (!(_rtl_state == RTL_STATE_DESCEND &&
-						      (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
-						    _vstatus.condition_home_position_valid) {
-							dispatch(EVENT_RTL_REQUESTED);
-						}
-
-						stick_mode = true;
-
-					} else {
-						/* MISSION switch */
-						if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
-							request_loiter_or_ready();
-							stick_mode = true;
-
-						} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
-							request_mission_if_available();
-							stick_mode = true;
-						}
-
-						if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
-							/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
-							request_mission_if_available();
-							stick_mode = true;
-						}
-					}
-				}
-
-				if (!stick_mode) {
 				if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
 					/* commander requested new navigation mode, try to set it */
-						_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
-
 					switch (_vstatus.set_nav_state) {
 					case NAV_STATE_NONE:
 						/* nothing to do */
@@ -769,12 +735,20 @@ Navigator::task_main()
 						request_mission_if_available();
 					}
 				}
+
+				/* check if waypoint has been reached in MISSION, RTL and LAND modes */
+				if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) {
+					if (check_mission_item_reached()) {
+						on_mission_item_reached();
+					}
 				}
 
 			} else {
 				/* navigator shouldn't act */
 				dispatch(EVENT_NONE_REQUESTED);
 			}
+
+			_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
 		}
 
 		/* parameters updated */
@@ -813,17 +787,15 @@ Navigator::task_main()
 		if (fds[1].revents & POLLIN) {
 			global_position_update();
 
-			/* publish position setpoint triplet on each position update if navigator active */
 			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
+				/* publish position setpoint triplet on each position update if navigator active */
 				_pos_sp_triplet_updated = true;
 
-				if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) {
+				if (myState == NAV_STATE_LAND && !_global_pos_valid) {
 					/* got global position when landing, update setpoint */
 					start_land();
 				}
 
-				_global_pos_valid = _global_pos.global_valid;
-
 				/* check if waypoint has been reached in MISSION, RTL and LAND modes */
 				if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) {
 					if (check_mission_item_reached()) {
@@ -848,6 +820,8 @@ Navigator::task_main()
 			}
 		}
 
+		_global_pos_valid = _vstatus.condition_global_position_valid;
+
 		/* publish position setpoint triplet if updated */
 		if (_pos_sp_triplet_updated) {
 			_pos_sp_triplet_updated = false;
@@ -893,9 +867,9 @@ Navigator::start()
 void
 Navigator::status()
 {
-	warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in");
+	warnx("Global position: %svalid", _global_pos_valid ? "" : "in");
 
-	if (_global_pos.global_valid) {
+	if (_global_pos_valid) {
 		warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
 		warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
 		      (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Copyright (C) 2013, 2014 PX4 Development Team. All rights reserved.
- *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,6 +34,8 @@
 /**
  * @file position_estimator_inav_main.c
  * Model-identification based position estimator for multirotors
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <unistd.h>
@@ -57,6 +58,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/home_position.h>
 #include <uORB/topics/optical_flow.h>
 #include <mavlink/mavlink_log.h>
 #include <poll.h>
@@ -95,8 +97,9 @@ static void usage(const char *reason);
  */
 static void usage(const char *reason)
 {
-	if (reason)
+	if (reason) {
 		fprintf(stderr, "%s\n", reason);
+	}
 
 	fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n");
 	exit(1);
@@ -112,8 +115,9 @@ static void usage(const char *reason)
  */
 int position_estimator_inav_main(int argc, char *argv[])
 {
-	if (argc < 1)
+	if (argc < 1) {
 		usage("missing command");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 		if (thread_running) {
@@ -125,8 +129,9 @@ int position_estimator_inav_main(int argc, char *argv[])
 		verbose_mode = false;
 
 		if (argc > 1)
-			if (!strcmp(argv[2], "-v"))
+			if (!strcmp(argv[2], "-v")) {
 				verbose_mode = true;
+			}
 
 		thread_should_exit = false;
 		position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
@@ -163,16 +168,19 @@ int position_estimator_inav_main(int argc, char *argv[])
 	exit(1);
 }
 
-void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) {
+void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float x_est_prev[3], float y_est_prev[3], float z_est_prev[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
+{
 	FILE *f = fopen("/fs/microsd/inav.log", "a");
+
 	if (f) {
 		char *s = malloc(256);
-		unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]);
+		unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f] x_est_prev=[%.5f %.5f %.5f] y_est_prev=[%.5f %.5f %.5f] z_est_prev=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2], x_est_prev[0], x_est_prev[1], x_est_prev[2], y_est_prev[0], y_est_prev[1], y_est_prev[2], z_est_prev[0], z_est_prev[1], z_est_prev[2]);
 		fwrite(s, 1, n, f);
 		n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
 		fwrite(s, 1, n, f);
 		free(s);
 	}
+
 	fsync(fileno(f));
 	fclose(f);
 }
@@ -191,6 +199,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	float y_est[3] = { 0.0f, 0.0f, 0.0f };
 	float z_est[3] = { 0.0f, 0.0f, 0.0f };
 
+	float x_est_prev[3], y_est_prev[3], z_est_prev[3];
+	memset(x_est_prev, 0, sizeof(x_est_prev));
+	memset(y_est_prev, 0, sizeof(y_est_prev));
+	memset(z_est_prev, 0, sizeof(z_est_prev));
+
 	int baro_init_cnt = 0;
 	int baro_init_num = 200;
 	float baro_offset = 0.0f;		// baro offset for reference altitude, initialized on start, then adjusted
@@ -206,6 +219,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	bool ref_inited = false;
 	hrt_abstime ref_init_start = 0;
 	const hrt_abstime ref_init_delay = 1000000;	// wait for 1s after 3D fix
+	struct map_projection_reference_s ref;
+	memset(&ref, 0, sizeof(ref));
+	hrt_abstime home_timestamp = 0;
 
 	uint16_t accel_updates = 0;
 	uint16_t baro_updates = 0;
@@ -238,7 +254,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	float corr_flow[] = { 0.0f, 0.0f };	// N E
 	float w_flow = 0.0f;
 
+	static float min_eph_epv = 2.0f;	// min EPH/EPV, used for weight calculation
+	static float max_eph_epv = 10.0f;	// max EPH/EPV acceptable for estimation
+
 	float sonar_prev = 0.0f;
+	hrt_abstime flow_prev = 0;			// time of last flow measurement
 	hrt_abstime sonar_time = 0;			// time of last sonar measurement (not filtered)
 	hrt_abstime sonar_valid_time = 0;	// time of last sonar measurement used for correction (filtered)
 	hrt_abstime xy_src_time = 0;		// time of last available position data
@@ -257,6 +277,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	memset(&sensor, 0, sizeof(sensor));
 	struct vehicle_gps_position_s gps;
 	memset(&gps, 0, sizeof(gps));
+	struct home_position_s home;
+	memset(&home, 0, sizeof(home));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
 	struct vehicle_local_position_s local_pos;
@@ -274,10 +296,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
 	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	int home_position_sub = orb_subscribe(ORB_ID(home_position));
 
 	/* advertise */
 	orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
-	orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
+	orb_advert_t vehicle_global_position_pub = -1;
 
 	struct position_estimator_inav_params params;
 	struct position_estimator_inav_param_handles pos_inav_param_handles;
@@ -325,7 +348,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset);
 						local_pos.z_valid = true;
 						local_pos.v_z_valid = true;
-						global_pos.baro_valid = true;
 					}
 				}
 			}
@@ -425,6 +447,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (updated) {
 				orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
 
+				/* calculate time from previous update */
+				float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
+				flow_prev = flow.flow_timestamp;
+
 				if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) {
 					sonar_time = t;
 					sonar_prev = flow.ground_distance_m;
@@ -475,10 +501,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
 							fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
 
-					/* convert raw flow to angular flow */
+					/* convert raw flow to angular flow (rad/s) */
 					float flow_ang[2];
-					flow_ang[0] = flow.flow_raw_x * params.flow_k;
+					flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
-					flow_ang[1] = flow.flow_raw_y * params.flow_k;
+					flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
 					/* flow measurements vector */
 					float flow_m[3];
 					flow_m[0] = -flow_ang[0] * flow_dist;
@@ -503,8 +529,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 					/* if flow is not accurate, reduce weight for it */
 					// TODO make this more fuzzy
-					if (!flow_accurate)
+					if (!flow_accurate) {
 						w_flow *= 0.05f;
+					}
 
 					flow_valid = true;
 
@@ -516,32 +543,73 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				flow_updates++;
 			}
 
+			/* home position */
+			orb_check(home_position_sub, &updated);
+
+			if (updated) {
+				orb_copy(ORB_ID(home_position), home_position_sub, &home);
+
+				if (home.timestamp != home_timestamp) {
+					home_timestamp = home.timestamp;
+
+					double est_lat, est_lon;
+					float est_alt;
+
+					if (ref_inited) {
+						/* calculate current estimated position in global frame */
+						est_alt = local_pos.ref_alt - local_pos.z;
+						map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
+					}
+
+					/* update reference */
+					map_projection_init(&ref, home.lat, home.lon);
+
+					/* update baro offset */
+					baro_offset += home.alt - local_pos.ref_alt;
+
+					local_pos.ref_lat = home.lat;
+					local_pos.ref_lon = home.lon;
+					local_pos.ref_alt = home.alt;
+					local_pos.ref_timestamp = home.timestamp;
+
+					if (ref_inited) {
+						/* reproject position estimate with new reference */
+						map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]);
+						z_est[0] = -(est_alt - local_pos.ref_alt);
+					}
+
+					ref_inited = true;
+				}
+			}
+
 			/* vehicle GPS position */
 			orb_check(vehicle_gps_position_sub, &updated);
 
 			if (updated) {
 				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
 
-				if (gps.fix_type >= 3) {
+				bool reset_est = false;
+
 				/* hysteresis for GPS quality */
 				if (gps_valid) {
-						if (gps.eph_m > 10.0f || gps.epv_m > 20.0f) {
+					if (gps.eph_m > max_eph_epv * 1.5f || gps.epv_m > max_eph_epv * 1.5f || gps.fix_type < 3) {
 						gps_valid = false;
 						mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
 					}
 
 				} else {
-						if (gps.eph_m < 5.0f && gps.epv_m < 10.0f) {
+					if (gps.eph_m < max_eph_epv && gps.epv_m < max_eph_epv && gps.fix_type >= 3) {
 						gps_valid = true;
+						reset_est = true;
 						mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
 					}
 				}
 
-				} else {
-					gps_valid = false;
-				}
-
 				if (gps_valid) {
+					double lat = gps.lat * 1e-7;
+					double lon = gps.lon * 1e-7;
+					float alt = gps.alt * 1e-3;
+
 					/* initialize reference position if needed */
 					if (!ref_inited) {
 						if (ref_init_start == 0) {
@@ -549,18 +617,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 						} else if (t > ref_init_start + ref_init_delay) {
 							ref_inited = true;
-							/* reference GPS position */
+							/* update baro offset */
-							double lat = gps.lat * 1e-7;
+							baro_offset -= z_est[0];
-							double lon = gps.lon * 1e-7;
-							float alt = gps.alt * 1e-3;
 
-							local_pos.ref_lat = gps.lat;
+							/* set position estimate to (0, 0, 0), use GPS velocity for XY */
-							local_pos.ref_lon = gps.lon;
+							x_est[0] = 0.0f;
-							local_pos.ref_alt = alt + z_est[0];
+							x_est[1] = gps.vel_n_m_s;
+							x_est[2] = accel_NED[0];
+							y_est[0] = 0.0f;
+							y_est[1] = gps.vel_e_m_s;
+							z_est[0] = 0.0f;
+							y_est[2] = accel_NED[1];
+
+							local_pos.ref_lat = lat;
+							local_pos.ref_lon = lon;
+							local_pos.ref_alt = alt;
 							local_pos.ref_timestamp = t;
 
 							/* initialize projection */
-							map_projection_init(lat, lon);
+							map_projection_init(&ref, lat, lon);
 							warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
 							mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
 						}
@@ -569,11 +644,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					if (ref_inited) {
 						/* project GPS lat lon to plane */
 						float gps_proj[2];
-						map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
+						map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1]));
+
+						/* reset position estimate when GPS becomes good */
+						if (reset_est) {
+							x_est[0] = gps_proj[0];
+							x_est[1] = gps.vel_n_m_s;
+							x_est[2] = accel_NED[0];
+							y_est[0] = gps_proj[1];
+							y_est[1] = gps.vel_e_m_s;
+							y_est[2] = accel_NED[1];
+						}
+
 						/* calculate correction for position */
 						corr_gps[0][0] = gps_proj[0] - x_est[0];
 						corr_gps[1][0] = gps_proj[1] - y_est[0];
-						corr_gps[2][0] = local_pos.ref_alt - gps.alt * 1e-3 - z_est[0];
+						corr_gps[2][0] = local_pos.ref_alt - alt - z_est[0];
 
 						/* calculate correction for velocity */
 						if (gps.vel_ned_valid) {
@@ -587,8 +673,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							corr_gps[2][1] = 0.0f;
 						}
 
-						w_gps_xy = 2.0f / fmaxf(2.0f, gps.eph_m);
+						w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m);
-						w_gps_z = 4.0f / fmaxf(4.0f, gps.epv_m);
+						w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m);
 					}
 
 				} else {
@@ -704,23 +790,34 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		/* inertial filter prediction for altitude */
 		inertial_filter_predict(dt, z_est);
 
+		if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) {
+			write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
+			memcpy(z_est, z_est_prev, sizeof(z_est));
+		}
+
 		/* inertial filter correction for altitude */
 		inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
 		inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
 		inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc);
 
-		float x_est_prev[3], y_est_prev[3];
+		if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) {
+			write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
+			memcpy(z_est, z_est_prev, sizeof(z_est));
+			memset(corr_acc, 0, sizeof(corr_acc));
+			memset(corr_gps, 0, sizeof(corr_gps));
+			corr_baro = 0;
 
-		memcpy(x_est_prev, x_est, sizeof(x_est));
+		} else {
-		memcpy(y_est_prev, y_est, sizeof(y_est));
+			memcpy(z_est_prev, z_est, sizeof(z_est));
+		}
 
 		if (can_estimate_xy) {
 			/* inertial filter prediction for position */
 			inertial_filter_predict(dt, x_est);
 			inertial_filter_predict(dt, y_est);
 
-			if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
+			if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) {
-				write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
+				write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
 				memcpy(x_est, x_est_prev, sizeof(x_est));
 				memcpy(y_est, y_est_prev, sizeof(y_est));
 			}
@@ -744,13 +841,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				}
 			}
 
-			if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
+			if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) {
-				write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
+				write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
 				memcpy(x_est, x_est_prev, sizeof(x_est));
 				memcpy(y_est, y_est_prev, sizeof(y_est));
 				memset(corr_acc, 0, sizeof(corr_acc));
 				memset(corr_gps, 0, sizeof(corr_gps));
 				memset(corr_flow, 0, sizeof(corr_flow));
+
+			} else {
+				memcpy(x_est_prev, x_est, sizeof(x_est));
+				memcpy(y_est_prev, y_est, sizeof(y_est));
 			}
 		}
 
@@ -808,7 +909,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		if (t > pub_last + pub_interval) {
 			pub_last = t;
 			/* publish local position */
-			local_pos.xy_valid = can_estimate_xy && use_gps_xy;
+			local_pos.xy_valid = can_estimate_xy;
 			local_pos.v_xy_valid = can_estimate_xy;
 			local_pos.xy_global = local_pos.xy_valid && use_gps_xy;
 			local_pos.z_global = local_pos.z_valid && use_gps_z;
@@ -831,42 +932,37 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 			orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
 
+			if (local_pos.xy_global && local_pos.z_global) {
 				/* publish global position */
-			global_pos.global_valid = local_pos.xy_global;
+				global_pos.timestamp = t;
+				global_pos.time_gps_usec = gps.time_gps_usec;
 
-			if (local_pos.xy_global) {
 				double est_lat, est_lon;
-				map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
+				map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
+
 				global_pos.lat = est_lat;
 				global_pos.lon = est_lon;
-				global_pos.time_gps_usec = gps.time_gps_usec;
+				global_pos.alt = local_pos.ref_alt - local_pos.z;
-			}
 
-			/* set valid values even if position is not valid */
-			if (local_pos.v_xy_valid) {
 				global_pos.vel_n = local_pos.vx;
 				global_pos.vel_e = local_pos.vy;
-			}
-
-			if (local_pos.z_global) {
-				global_pos.alt = local_pos.ref_alt - local_pos.z;
-			}
-
-			if (local_pos.z_valid) {
-				global_pos.baro_alt = baro_offset - local_pos.z;
-			}
-
-			if (local_pos.v_z_valid) {
 				global_pos.vel_d = local_pos.vz;
-			}
 
 				global_pos.yaw = local_pos.yaw;
 
-			global_pos.timestamp = t;
+				// TODO implement dead-reckoning
+				global_pos.eph = gps.eph_m;
+				global_pos.epv = gps.epv_m;
 
+				if (vehicle_global_position_pub < 0) {
+					vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
+
+				} else {
 					orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
 				}
 			}
+		}
+	}
 
 	warnx("stopped");
 	mavlink_log_info(mavlink_fd, "[inav] stopped");

src/modules/position_estimator_inav/position_estimator_inav_params.c

@@ -50,7 +50,7 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f);
 PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
 PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
 PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
-PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f);
+PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f);
 PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f);
 PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f);
 PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);

src/modules/sdlog2/sdlog2.c

@@ -960,6 +960,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.msg_type = LOG_STAT_MSG;
 			log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
 			log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
+			log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe_state;
 			log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
 			log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
 			log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
@@ -1101,8 +1102,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_LPOS.vx = buf.local_pos.vx;
 			log_msg.body.log_LPOS.vy = buf.local_pos.vy;
 			log_msg.body.log_LPOS.vz = buf.local_pos.vz;
-			log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat;
+			log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat * 1e7;
-			log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon;
+			log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon * 1e7;
 			log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
 			log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
 			log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
@@ -1130,8 +1131,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n;
 			log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e;
 			log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d;
-			log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt;
+			log_msg.body.log_GPOS.eph = buf.global_pos.eph;
-			log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0);
+			log_msg.body.log_GPOS.epv = buf.global_pos.epv;
 			LOGBUFFER_WRITE_AND_COUNT(GPOS);
 		}
 
@@ -1181,6 +1182,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			/* Copy only the first 8 channels of 14 */
 			memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel));
 			log_msg.body.log_RC.channel_count = buf.rc.chan_count;
+			log_msg.body.log_RC.signal_lost = buf.rc.signal_lost;
 			LOGBUFFER_WRITE_AND_COUNT(RC);
 		}
 

src/modules/sdlog2/sdlog2_messages.h

@@ -154,6 +154,7 @@ struct log_ATTC_s {
 struct log_STAT_s {
 	uint8_t main_state;
 	uint8_t arming_state;
+	uint8_t failsafe_state;
 	float battery_remaining;
 	uint8_t battery_warning;
 	uint8_t landed;
@@ -164,6 +165,7 @@ struct log_STAT_s {
 struct log_RC_s {
 	float channel[8];
 	uint8_t channel_count;
+	uint8_t signal_lost;
 };
 
 /* --- OUT0 - ACTUATOR_0 OUTPUT --- */
@@ -209,8 +211,8 @@ struct log_GPOS_s {
 	float vel_n;
 	float vel_e;
 	float vel_d;
-	float baro_alt;
+	float eph;
-	uint8_t flags;
+	float epv;
 };
 
 /* --- GPSP - GLOBAL POSITION SETPOINT --- */
@@ -350,13 +352,13 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(LPSP, "ffff",		"X,Y,Z,Yaw"),
 	LOG_FORMAT(GPS, "QBffLLfffff",		"GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
 	LOG_FORMAT(ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
-	LOG_FORMAT(STAT, "BBfBB",		"MainState,ArmState,BatRem,BatWarn,Landed"),
+	LOG_FORMAT(STAT, "BBBfBB",		"MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"),
-	LOG_FORMAT(RC, "ffffffffB",		"Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
+	LOG_FORMAT(RC, "ffffffffBB",		"Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
 	LOG_FORMAT(OUT0, "ffffffff",		"Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "fff",			"IndSpeed,TrueSpeed,AirTemp"),
 	LOG_FORMAT(ARSP, "fff",			"RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB",		"RawX,RawY,CompX,CompY,Dist,Q,SensID"),
-	LOG_FORMAT(GPOS, "LLfffffB",		"Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
+	LOG_FORMAT(GPOS, "LLffffff",		"Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"),
 	LOG_FORMAT(GPSP, "BLLffBfbf",		"NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
 	LOG_FORMAT(ESC, "HBBBHHHHHHfH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff",			"VX,VY,VZ"),

src/modules/sensors/sensor_params.c

@@ -594,13 +594,13 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
 
 /**
- * Mission switch channel mapping.
+ * Loiter switch channel mapping.
  *
  * @min 0
  * @max 18
  * @group Radio Calibration
  */
-PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
+PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0);
 
 //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
 
@@ -647,27 +647,6 @@ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0);	/**< default function: camera roll */
 PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);
 
 
-/**
- * Roll scaling factor
- *
- * @group Radio Calibration
- */
-PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
-
-/**
- * Pitch scaling factor
- *
- * @group Radio Calibration
- */
-PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
-
-/**
- * Yaw scaling factor
- *
- * @group Radio Calibration
- */
-PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
-
 /**
  * Failsafe channel PWM threshold.
  *

src/modules/sensors/sensors.cpp

@@ -135,7 +135,7 @@
  */
 #define PCB_TEMP_ESTIMATE_DEG 5.0f
 
-#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
+#define STICK_ON_OFF_LIMIT 0.75f
 
 /**
  * Sensor app start / stop handling function
@@ -167,7 +167,15 @@ public:
 private:
 	static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS;	/**< maximum number of r/c channels we handle */
 
-	hrt_abstime	_rc_last_valid;		/**< last time we got a valid RC signal */
+	/**
+	 * Get and limit value for specified RC function. Returns NAN if not mapped.
+	 */
+	float		get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value);
+
+	/**
+	 * Get switch position for specified function.
+	 */
+	switch_pos_t	get_rc_switch_position(enum RC_CHANNELS_FUNCTION func);
 
 	/**
 	 * Gather and publish RC input data.
@@ -246,7 +254,7 @@ private:
 		int rc_map_mode_sw;
 		int rc_map_return_sw;
 		int rc_map_assisted_sw;
-		int rc_map_mission_sw;
+		int rc_map_loiter_sw;
 
 //		int rc_map_offboard_ctrl_mode_sw;
 
@@ -258,11 +266,6 @@ private:
 		int rc_map_aux4;
 		int rc_map_aux5;
 
-		float rc_scale_roll;
-		float rc_scale_pitch;
-		float rc_scale_yaw;
-		float rc_scale_flaps;
-
 		int32_t rc_fs_thr;
 
 		float battery_voltage_scaling;
@@ -294,7 +297,7 @@ private:
 		param_t rc_map_mode_sw;
 		param_t rc_map_return_sw;
 		param_t rc_map_assisted_sw;
-		param_t rc_map_mission_sw;
+		param_t rc_map_loiter_sw;
 
 //		param_t rc_map_offboard_ctrl_mode_sw;
 
@@ -306,11 +309,6 @@ private:
 		param_t rc_map_aux4;
 		param_t rc_map_aux5;
 
-		param_t rc_scale_roll;
-		param_t rc_scale_pitch;
-		param_t rc_scale_yaw;
-		param_t rc_scale_flaps;
-
 		param_t rc_fs_thr;
 
 		param_t battery_voltage_scaling;
@@ -434,8 +432,6 @@ Sensors	*g_sensors = nullptr;
 }
 
 Sensors::Sensors() :
-	_rc_last_valid(0),
-
 	_fd_adc(-1),
 	_last_adc(0),
 
@@ -470,6 +466,7 @@ Sensors::Sensors() :
 	_battery_discharged(0),
 	_battery_current_timestamp(0)
 {
+	memset(&_rc, 0, sizeof(_rc));
 
 	/* basic r/c parameters */
 	for (unsigned i = 0; i < _rc_max_chan_count; i++) {
@@ -511,7 +508,7 @@ Sensors::Sensors() :
 
 	/* optional mode switches, not mapped per default */
 	_parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
-	_parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW");
+	_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
 
 //	_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
 
@@ -521,11 +518,6 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4");
 	_parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");
 
-	_parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL");
-	_parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH");
-	_parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
-	_parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS");
-
 	/* RC failsafe */
 	_parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR");
 
@@ -662,7 +654,7 @@ Sensors::parameters_update()
 		warnx("%s", paramerr);
 	}
 
-	if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) {
+	if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) {
 		warnx("%s", paramerr);
 	}
 
@@ -679,10 +671,6 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3));
 	param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
 	param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
-	param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll));
-	param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
-	param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
-	param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
 	param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr));
 
 	/* update RC function mappings */
@@ -694,7 +682,7 @@ Sensors::parameters_update()
 	_rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
 	_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
 	_rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1;
-	_rc.function[MISSION] = _parameters.rc_map_mission_sw - 1;
+	_rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1;
 
 	_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
 
@@ -1267,6 +1255,45 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 	}
 }
 
+float
+Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value)
+{
+	if (_rc.function[func] >= 0) {
+		float value = _rc.chan[_rc.function[func]].scaled;
+		if (value < min_value) {
+			return min_value;
+
+		} else if (value > max_value) {
+			return max_value;
+
+		} else {
+			return value;
+		}
+	} else {
+		return 0.0f;
+	}
+}
+
+switch_pos_t
+Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func)
+{
+	if (_rc.function[func] >= 0) {
+		float value = _rc.chan[_rc.function[func]].scaled;
+		if (value > STICK_ON_OFF_LIMIT) {
+			return SWITCH_POS_ON;
+
+		} else if (value < -STICK_ON_OFF_LIMIT) {
+			return SWITCH_POS_OFF;
+
+		} else {
+			return SWITCH_POS_MIDDLE;
+		}
+
+	} else {
+		return SWITCH_POS_NONE;
+	}
+}
+
 void
 Sensors::rc_poll()
 {
@@ -1279,41 +1306,28 @@ Sensors::rc_poll()
 
 		orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
 
-		if (rc_input.rc_lost)
+		/* detect RC signal loss */
-			return;
+		bool signal_lost;
 
-		struct manual_control_setpoint_s manual_control;
+		/* check flags and require at least four channels to consider the signal valid */
-		struct actuator_controls_s actuator_group_3;
+		if (rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4) {
+			/* signal is lost or no enough channels */
+			signal_lost = true;
 
-		/* initialize to default values */
+		} else {
-		manual_control.roll = NAN;
+			/* signal looks good */
-		manual_control.pitch = NAN;
+			signal_lost = false;
-		manual_control.yaw = NAN;
-		manual_control.throttle = NAN;
 
-		manual_control.mode_switch = NAN;
+			/* check throttle failsafe */
-		manual_control.return_switch = NAN;
+			int8_t thr_ch = _rc.function[THROTTLE];
-		manual_control.assisted_switch = NAN;
+			if (_parameters.rc_fs_thr > 0 && thr_ch >= 0) {
-		manual_control.mission_switch = NAN;
+				/* throttle failsafe configured */
-//		manual_control.auto_offboard_input_switch = NAN;
+				if ((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) ||
-
+					(_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr)) {
-		manual_control.flaps = NAN;
+					/* throttle failsafe triggered, signal is lost by receiver */
-		manual_control.aux1 = NAN;
+					signal_lost = true;
-		manual_control.aux2 = NAN;
+				}
-		manual_control.aux3 = NAN;
-		manual_control.aux4 = NAN;
-		manual_control.aux5 = NAN;
-
-		/* require at least four channels to consider the signal valid */
-		if (rc_input.channel_count < 4) {
-			return;
 			}
-
-		/* check for failsafe */
-		if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr))
-			|| ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) {
-			/* do not publish manual control setpoints when there are none */
-			return;
 		}
 
 		unsigned channel_limit = rc_input.channel_count;
@@ -1321,10 +1335,7 @@ Sensors::rc_poll()
 		if (channel_limit > _rc_max_chan_count)
 			channel_limit = _rc_max_chan_count;
 
-		/* we are accepting this message */
+		/* read out and scale values from raw message even if signal is invalid */
-		_rc_last_valid = rc_input.timestamp_last_signal;
-
-		/* Read out values from raw message */
 		for (unsigned int i = 0; i < channel_limit; i++) {
 
 			/*
@@ -1371,122 +1382,67 @@ Sensors::rc_poll()
 		}
 
 		_rc.chan_count = rc_input.channel_count;
+		_rc.rssi = rc_input.rssi;
+		_rc.signal_lost = signal_lost;
 		_rc.timestamp = rc_input.timestamp_last_signal;
 
-		manual_control.timestamp = rc_input.timestamp_last_signal;
+		/* publish rc_channels topic even if signal is invalid, for debug */
-
-		/* roll input - rolling right is stick-wise and rotation-wise positive */
-		manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled);
-		/*
-		 * pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
-		 * so reverse sign.
-		 */
-		manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled);
-		/* yaw input - stick right is positive and positive rotation */
-		manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled);
-		/* throttle input */
-		manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
-
-		if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
-
-		if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
-
-		/* scale output */
-		if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) {
-			manual_control.roll *= _parameters.rc_scale_roll;
-		}
-
-		if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) {
-			manual_control.pitch *= _parameters.rc_scale_pitch;
-		}
-
-		if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) {
-			manual_control.yaw *= _parameters.rc_scale_yaw;
-		}
-
-		/* flaps */
-		if (_rc.function[FLAPS] >= 0) {
-
-			manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled);
-
-			if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) {
-				manual_control.flaps *= _parameters.rc_scale_flaps;
-			}
-		}
-
-		/* mode switch input */
-		if (_rc.function[MODE] >= 0) {
-			manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
-		}
-
-		/* assisted switch input */
-		if (_rc.function[ASSISTED] >= 0) {
-			manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
-		}
-
-		/* mission switch input */
-		if (_rc.function[MISSION] >= 0) {
-			manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
-		}
-
-		/* return switch input */
-		if (_rc.function[RETURN] >= 0) {
-			manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
-		}
-
-//		if (_rc.function[OFFBOARD_MODE] >= 0) {
-//			manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
-//		}
-
-		/* aux functions, only assign if valid mapping is present */
-		if (_rc.function[AUX_1] >= 0) {
-			manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled);
-		}
-
-		if (_rc.function[AUX_2] >= 0) {
-			manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled);
-		}
-
-		if (_rc.function[AUX_3] >= 0) {
-			manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled);
-		}
-
-		if (_rc.function[AUX_4] >= 0) {
-			manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled);
-		}
-
-		if (_rc.function[AUX_5] >= 0) {
-			manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled);
-		}
-
-		/* copy from mapped manual control to control group 3 */
-		actuator_group_3.control[0] = manual_control.roll;
-		actuator_group_3.control[1] = manual_control.pitch;
-		actuator_group_3.control[2] = manual_control.yaw;
-		actuator_group_3.control[3] = manual_control.throttle;
-		actuator_group_3.control[4] = manual_control.flaps;
-		actuator_group_3.control[5] = manual_control.aux1;
-		actuator_group_3.control[6] = manual_control.aux2;
-		actuator_group_3.control[7] = manual_control.aux3;
-
-		/* check if ready for publishing */
 		if (_rc_pub > 0) {
 			orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
 
 		} else {
-			/* advertise the rc topic */
 			_rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc);
 		}
 
-		/* check if ready for publishing */
+		if (!signal_lost) {
+			struct manual_control_setpoint_s manual;
+			memset(&manual, 0 , sizeof(manual));
+
+			/* fill values in manual_control_setpoint topic only if signal is valid */
+			manual.timestamp = rc_input.timestamp_last_signal;
+
+			/* limit controls */
+			manual.roll = get_rc_value(ROLL, -1.0, 1.0);
+			manual.pitch = get_rc_value(PITCH, -1.0, 1.0);
+			manual.yaw = get_rc_value(YAW, -1.0, 1.0);
+			manual.throttle = get_rc_value(THROTTLE, 0.0, 1.0);
+			manual.flaps = get_rc_value(FLAPS, -1.0, 1.0);
+			manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0);
+			manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0);
+			manual.aux3 = get_rc_value(AUX_3, -1.0, 1.0);
+			manual.aux4 = get_rc_value(AUX_4, -1.0, 1.0);
+			manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0);
+
+			/* mode switches */
+			manual.mode_switch = get_rc_switch_position(MODE);
+			manual.assisted_switch = get_rc_switch_position(ASSISTED);
+			manual.loiter_switch = get_rc_switch_position(LOITER);
+			manual.return_switch = get_rc_switch_position(RETURN);
+
+			/* publish manual_control_setpoint topic */
 			if (_manual_control_pub > 0) {
-			orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
+				orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual);
 
 			} else {
-			_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
+				_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
 			}
 
-		/* check if ready for publishing */
+			/* copy from mapped manual control to control group 3 */
+			struct actuator_controls_s actuator_group_3;
+			memset(&actuator_group_3, 0 , sizeof(actuator_group_3));
+
+			actuator_group_3.timestamp = rc_input.timestamp_last_signal;
+
+			actuator_group_3.control[0] = manual.roll;
+			actuator_group_3.control[1] = manual.pitch;
+			actuator_group_3.control[2] = manual.yaw;
+			actuator_group_3.control[3] = manual.throttle;
+			actuator_group_3.control[4] = manual.flaps;
+			actuator_group_3.control[5] = manual.aux1;
+			actuator_group_3.control[6] = manual.aux2;
+			actuator_group_3.control[7] = manual.aux3;
+
+			/* publish actuator_controls_3 topic */
 			if (_actuator_group_3_pub > 0) {
 				orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
 
@@ -1494,7 +1450,7 @@ Sensors::rc_poll()
 				_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
 			}
 		}
-
+	}
 }
 
 void

src/modules/uORB/topics/manual_control_setpoint.h

@@ -43,6 +43,16 @@
 #include <stdint.h>
 #include "../uORB.h"
 
+/**
+ * Switch position
+ */
+typedef enum {
+	SWITCH_POS_NONE = 0,	/**< switch is not mapped */
+	SWITCH_POS_ON,			/**< switch activated (value = 1) */
+	SWITCH_POS_MIDDLE,		/**< middle position (value = 0) */
+	SWITCH_POS_OFF			/**< switch not activated (value = -1) */
+} switch_pos_t;
+
 /**
  * @addtogroup topics
  * @{
@@ -51,32 +61,25 @@
 struct manual_control_setpoint_s {
 	uint64_t timestamp;
 
-	float roll;			 	/**< ailerons roll / roll rate input */
-	float pitch;				/**< elevator / pitch / pitch rate */
-	float yaw;				/**< rudder / yaw rate / yaw */
-	float throttle;				/**< throttle / collective thrust / altitude */
-
-	float mode_switch;			/**< mode 3 position switch (mandatory): manual, assisted, auto */
-	float return_switch;			/**< land 2 position switch (mandatory): land, no effect */
-	float assisted_switch;			/**< assisted 2 position switch (optional): seatbelt, simple */
-	float mission_switch;		/**< mission 2 position switch (optional): mission, loiter */
-
 	/**
-	 * Any of the channels below may not be available and be set to NaN
+	 * Any of the channels may not be available and be set to NaN
 	 * to indicate that it does not contain valid data.
 	 */
-
+	float roll;			 	/**< ailerons roll / roll rate input, -1..1 */
-	// XXX needed or parameter?
+	float pitch;			/**< elevator / pitch / pitch rate, -1..1 */
-	//float auto_offboard_input_switch;	/**< controller setpoint source (0 = onboard, 1 = offboard) */
+	float yaw;				/**< rudder / yaw rate / yaw, -1..1 */
-
+	float throttle;			/**< throttle / collective thrust / altitude, 0..1 */
 	float flaps;			/**< flap position */
-
 	float aux1;				/**< default function: camera yaw / azimuth */
 	float aux2;				/**< default function: camera pitch / tilt */
 	float aux3;				/**< default function: camera trigger */
 	float aux4;				/**< default function: camera roll */
 	float aux5;				/**< default function: payload drop */
 
+	switch_pos_t mode_switch;			/**< mode 3 position switch (mandatory): manual, assisted, auto */
+	switch_pos_t return_switch;			/**< land 2 position switch (mandatory): land, no effect */
+	switch_pos_t assisted_switch;			/**< assisted 2 position switch (optional): seatbelt, simple */
+	switch_pos_t loiter_switch;		/**< mission 2 position switch (optional): mission, loiter */
 }; /**< manual control inputs */
 
 /**

src/modules/uORB/topics/optical_flow.h

@@ -57,6 +57,7 @@ struct optical_flow_s {
 
 	uint64_t timestamp;		/**< in microseconds since system start          */
 
+	uint64_t flow_timestamp;		/**< timestamp from flow sensor */
 	int16_t flow_raw_x;		/**< flow in pixels in X direction, not rotation-compensated */
 	int16_t flow_raw_y;		/**< flow in pixels in Y direction, not rotation-compensated */
 	float flow_comp_x_m;		/**< speed over ground in meters, rotation-compensated */

src/modules/uORB/topics/rc_channels.h

@@ -65,7 +65,7 @@ enum RC_CHANNELS_FUNCTION {
 	MODE = 4,
 	RETURN = 5,
 	ASSISTED = 6,
-	MISSION = 7,
+	LOITER = 7,
 	OFFBOARD_MODE = 8,
 	FLAPS   = 9,
 	AUX_1   = 10,
@@ -94,6 +94,7 @@ struct rc_channels_s {
 	char function_name[RC_CHANNELS_FUNCTION_MAX][20];
 	int8_t function[RC_CHANNELS_FUNCTION_MAX];
 	uint8_t rssi;                       /**< Overall receive signal strength */
+	bool signal_lost;		/**< control signal lost, should be checked together with topic timeout */
 }; /**< radio control channels. */
 
 /**

src/modules/uORB/topics/vehicle_global_position.h

@@ -63,9 +63,6 @@
 struct vehicle_global_position_s {
 	uint64_t timestamp;		/**< Time of this estimate, in microseconds since system start */
 
-	bool global_valid;		/**< true if position satisfies validity criteria of estimator */
-	bool baro_valid;		/**< true if baro_alt is valid (vel_d is also valid in this case) */
-
 	uint64_t time_gps_usec; /**< GPS timestamp in microseconds					   */
 	double lat;			/**< Latitude in degrees							 	   */
 	double lon;			/**< Longitude in degrees							 	   */
@@ -74,8 +71,8 @@ struct vehicle_global_position_s {
 	float vel_e;		/**< Ground east velocity, m/s							   */
 	float vel_d;		/**< Ground downside velocity, m/s						   */
 	float yaw; 			/**< Yaw in radians -PI..+PI.							   */
-
+	float eph;
-	float baro_alt;		/**< Barometric altitude (not raw baro but fused with accelerometer) */
+	float epv;
 };
 
 /**

src/modules/uORB/topics/vehicle_local_position.h

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,6 +34,9 @@
 /**
  * @file vehicle_local_position.h
  * Definition of the local fused NED position uORB topic.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_
@@ -72,8 +74,8 @@ struct vehicle_local_position_s {
 	bool xy_global;			/**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
 	bool z_global;			/**< true if z is valid and has valid global reference (ref_alt) */
 	uint64_t ref_timestamp;	/**< Time when reference position was set */
-	int32_t ref_lat;		/**< Reference point latitude in 1E7 degrees */
+	double ref_lat;		/**< Reference point latitude in degrees */
-	int32_t ref_lon;		/**< Reference point longitude in 1E7 degrees */
+	double ref_lon;		/**< Reference point longitude in degrees */
 	float ref_alt;			/**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
 	bool landed;			/**< true if vehicle is landed */
 	/* Distance to surface */

src/modules/uORB/topics/vehicle_status.h

@@ -95,29 +95,6 @@ typedef enum {
 	FAILSAFE_STATE_MAX
 } failsafe_state_t;
 
-typedef enum {
-	MODE_SWITCH_MANUAL = 0,
-	MODE_SWITCH_ASSISTED,
-	MODE_SWITCH_AUTO
-} mode_switch_pos_t;
-
-typedef enum {
-	ASSISTED_SWITCH_SEATBELT = 0,
-	ASSISTED_SWITCH_EASY
-} assisted_switch_pos_t;
-
-typedef enum {
-	RETURN_SWITCH_NONE = 0,
-	RETURN_SWITCH_NORMAL,
-	RETURN_SWITCH_RETURN
-} return_switch_pos_t;
-
-typedef enum {
-	MISSION_SWITCH_NONE = 0,
-	MISSION_SWITCH_LOITER,
-	MISSION_SWITCH_MISSION
-} mission_switch_pos_t;
-
 enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
 	VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
@@ -189,11 +166,6 @@ struct vehicle_status_s {
 
 	bool is_rotary_wing;
 
-	mode_switch_pos_t mode_switch;
-	return_switch_pos_t return_switch;
-	assisted_switch_pos_t assisted_switch;
-	mission_switch_pos_t mission_switch;
-
 	bool condition_battery_voltage_valid;
 	bool condition_system_in_air_restore;	/**< true if we can restore in mid air */
 	bool condition_system_sensors_initialized;