From 3a898e54adbaac7fda28e0b9079076d9cdf8028c Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 24 Apr 2014 14:30:29 +0200
Subject: towards a global map projection instance, WIP: need to remove local
 updates of the reference and add a global map update

---
 .../fw_att_pos_estimator_main.cpp                    |  6 +-----
 src/modules/mavlink/mavlink_receiver.cpp             |  4 ++--
 src/modules/mc_pos_control/mc_pos_control_main.cpp   |  9 ++++-----
 .../position_estimator_inav_main.c                   | 20 ++++++--------------
 4 files changed, 13 insertions(+), 26 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
index 9d2d9ed5f..420a03644 100644
--- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -177,8 +177,6 @@ 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 */
 
@@ -822,8 +820,6 @@ FixedwingEstimator::task_main()
 					_ekf->baroHgt = _baro.altitude - _baro_ref;
 					_baro_gps_offset = _baro_ref - _local_pos.ref_alt;
 
-					// XXX this is not multithreading safe
-					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;
@@ -1046,7 +1042,7 @@ FixedwingEstimator::task_main()
 
 				if (_local_pos.xy_global) {
 					double est_lat, est_lon;
-					map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
+					map_projection_reproject(_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;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index adafdd139..df61c3c15 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -795,7 +795,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		if (!_hil_local_proj_inited) {
 			_hil_local_proj_inited = true;
 			_hil_local_alt0 = hil_state.alt / 1000.0f;
-			map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon);
+//			map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); //XXX fix reference update
 			hil_local_pos.ref_timestamp = timestamp;
 			hil_local_pos.ref_lat = lat;
 			hil_local_pos.ref_lon = lon;
@@ -804,7 +804,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 
 		float x;
 		float y;
-		map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
+		map_projection_project(lat, lon, &x, &y);
 		hil_local_pos.timestamp = timestamp;
 		hil_local_pos.xy_valid = true;
 		hil_local_pos.z_valid = true;
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index dc0aa172a..963349d42 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -478,17 +478,17 @@ MulticopterPositionControl::update_ref()
 
 		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);
+			map_projection_reproject(_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);
+//		map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);    //XXX fix reference update
 		_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]);
+			map_projection_project(lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
 			_pos_sp(2) = -(alt_sp - _ref_alt);
 		}
 
@@ -687,8 +687,7 @@ MulticopterPositionControl::task_main()
 					_reset_alt_sp = true;
 
 					/* project setpoint to local frame */
-					map_projection_project(&_ref_pos,
-							       _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
+					map_projection_project(_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
 							       &_pos_sp.data[0], &_pos_sp.data[1]);
 					_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
 
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 368424853..b803dfb58 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -558,12 +558,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					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);
+						map_projection_reproject(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;
 
@@ -572,9 +569,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					local_pos.ref_alt = home.alt;
 					local_pos.ref_timestamp = home.timestamp;
 
-					if (ref_inited) {
+					if (ref_inited) { //XXX fix reference update
 						/* reproject position estimate with new reference */
-						map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]);
+						map_projection_project(est_lat, est_lon, &x_est[0], &y_est[0]);
 						z_est[0] = -(est_alt - local_pos.ref_alt);
 					}
 
@@ -633,18 +630,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							local_pos.ref_lon = lon;
 							local_pos.ref_alt = alt;
 							local_pos.ref_timestamp = t;
-
-							/* initialize projection */
-							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);
 						}
 					}
 
-					if (ref_inited) {
+					if (ref_inited) { //XXX fix reference update
 						/* project GPS lat lon to plane */
 						float gps_proj[2];
-						map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1]));
+						map_projection_project(lat, lon, &(gps_proj[0]), &(gps_proj[1]));
 
 						/* reset position estimate when GPS becomes good */
 						if (reset_est) {
@@ -938,7 +930,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				global_pos.time_gps_usec = gps.time_gps_usec;
 
 				double est_lat, est_lon;
-				map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
+				map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
 
 				global_pos.lat = est_lat;
 				global_pos.lon = est_lon;
-- 
cgit v1.2.3


From fbcb248bc3a91d1c26f6f47a2a46badd2d8a58f9 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 24 Apr 2014 15:59:49 +0200
Subject: navigator: init map projection on home pos update

---
 src/modules/navigator/navigator_main.cpp | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'src/modules')

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index d4d23396a..b7cf49cfa 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -625,6 +625,7 @@ Navigator::task_main()
 	parameters_update();
 	global_position_update();
 	home_position_update();
+	map_projection_init(_home_pos.lat, _home_pos.lon);
 	navigation_capabilities_update();
 	offboard_mission_update(_vstatus.is_rotary_wing);
 	onboard_mission_update();
@@ -816,6 +817,7 @@ Navigator::task_main()
 		if (fds[2].revents & POLLIN) {
 			home_position_update();
 			// XXX check if home position really changed
+			map_projection_init(_home_pos.lat, _home_pos.lon);
 			dispatch(EVENT_HOME_POSITION_CHANGED);
 		}
 
-- 
cgit v1.2.3


From 8bb1b39399eb09ddf93c2682713d80be4f776c5d Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 24 Apr 2014 16:12:28 +0200
Subject: mavlink receiver: hil: local pos reference lat lon from first data

---
 src/modules/mavlink/mavlink_receiver.cpp | 1 -
 1 file changed, 1 deletion(-)

(limited to 'src/modules')

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index df61c3c15..40121efe4 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -795,7 +795,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		if (!_hil_local_proj_inited) {
 			_hil_local_proj_inited = true;
 			_hil_local_alt0 = hil_state.alt / 1000.0f;
-//			map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); //XXX fix reference update
 			hil_local_pos.ref_timestamp = timestamp;
 			hil_local_pos.ref_lat = lat;
 			hil_local_pos.ref_lon = lon;
-- 
cgit v1.2.3


From 9f2da53f08f5d186d95653bad9afa04e8c7276d2 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 24 Apr 2014 17:05:30 +0200
Subject: mc pos control: map projection init not needed anymore

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 --
 1 file changed, 2 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 963349d42..fa3438ea8 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -482,8 +482,6 @@ MulticopterPositionControl::update_ref()
 			alt_sp = _ref_alt - _pos_sp(2);
 		}
 
-		/* update local projection reference */
-//		map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);    //XXX fix reference update
 		_ref_alt = _local_pos.ref_alt;
 
 		if (_ref_timestamp != 0) {
-- 
cgit v1.2.3


From 618ac319e63a6597cc62df9c810d76cdc094012b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 24 Apr 2014 17:06:10 +0200
Subject: pos estimator inav: check if map projection is initialized

---
 .../position_estimator_inav/position_estimator_inav_main.c        | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index b803dfb58..d01bf488f 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -549,7 +549,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (updated) {
 				orb_copy(ORB_ID(home_position), home_position_sub, &home);
 
-				if (home.timestamp != home_timestamp) {
+				if (home.timestamp != home_timestamp && map_projection_inited()) {
 					home_timestamp = home.timestamp;
 
 					double est_lat, est_lon;
@@ -569,7 +569,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					local_pos.ref_alt = home.alt;
 					local_pos.ref_timestamp = home.timestamp;
 
-					if (ref_inited) { //XXX fix reference update
+					if (ref_inited) {
 						/* reproject position estimate with new reference */
 						map_projection_project(est_lat, est_lon, &x_est[0], &y_est[0]);
 						z_est[0] = -(est_alt - local_pos.ref_alt);
@@ -602,7 +602,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					}
 				}
 
-				if (gps_valid) {
+				if (gps_valid && map_projection_inited()) {
 					double lat = gps.lat * 1e-7;
 					double lon = gps.lon * 1e-7;
 					float alt = gps.alt * 1e-3;
@@ -633,7 +633,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						}
 					}
 
-					if (ref_inited) { //XXX fix reference update
+					if (ref_inited) {
 						/* project GPS lat lon to plane */
 						float gps_proj[2];
 						map_projection_project(lat, lon, &(gps_proj[0]), &(gps_proj[1]));
-- 
cgit v1.2.3


From 4e30784d6c506cdfdf58c623da20cffcb799e496 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 24 Apr 2014 17:16:20 +0200
Subject: rename map_projection_inited to map_projection_initialized

---
 src/lib/geo/geo.c                                                  | 6 +++---
 src/lib/geo/geo.h                                                  | 2 +-
 src/modules/position_estimator_inav/position_estimator_inav_main.c | 4 ++--
 3 files changed, 6 insertions(+), 6 deletions(-)

(limited to 'src/modules')

diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 1e3ef56ae..725a04a29 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -57,7 +57,7 @@
 
 static struct map_projection_reference_s mp_ref;
 
-__EXPORT bool map_projection_inited()
+__EXPORT bool map_projection_initialized()
 {
 	return mp_ref.init_done;
 }
@@ -81,7 +81,7 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are
 
 __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y)
 {
-	if (!map_projection_inited()) {
+	if (!map_projection_initialized()) {
 		return false;
 	}
 
@@ -103,7 +103,7 @@ __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y)
 
 __EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon)
 {
-	if (!map_projection_inited()) {
+	if (!map_projection_initialized()) {
 		return false;
 	}
 
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 913e83b6e..66e4dc75e 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -79,7 +79,7 @@ struct map_projection_reference_s {
  * Initializes the map transformation.
  * @return true if map_projection_init was called before, false else
  */
-__EXPORT bool map_projection_inited();
+__EXPORT bool map_projection_initialized();
 
 
 /**
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index d01bf488f..e88493a5e 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -549,7 +549,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (updated) {
 				orb_copy(ORB_ID(home_position), home_position_sub, &home);
 
-				if (home.timestamp != home_timestamp && map_projection_inited()) {
+				if (home.timestamp != home_timestamp && map_projection_initialized()) {
 					home_timestamp = home.timestamp;
 
 					double est_lat, est_lon;
@@ -602,7 +602,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					}
 				}
 
-				if (gps_valid && map_projection_inited()) {
+				if (gps_valid && map_projection_initialized()) {
 					double lat = gps.lat * 1e-7;
 					double lon = gps.lon * 1e-7;
 					float alt = gps.alt * 1e-3;
-- 
cgit v1.2.3


From f5d9023b1060f10c14d016492c88375c6fdc22aa Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 25 Apr 2014 09:42:28 +0200
Subject: navigator: set home_pos timestamp as map projection reference
 timestamp

---
 src/modules/navigator/navigator_main.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index b7cf49cfa..228324789 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -625,7 +625,7 @@ Navigator::task_main()
 	parameters_update();
 	global_position_update();
 	home_position_update();
-	map_projection_init(_home_pos.lat, _home_pos.lon);
+	map_projection_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp);
 	navigation_capabilities_update();
 	offboard_mission_update(_vstatus.is_rotary_wing);
 	onboard_mission_update();
@@ -817,7 +817,7 @@ Navigator::task_main()
 		if (fds[2].revents & POLLIN) {
 			home_position_update();
 			// XXX check if home position really changed
-			map_projection_init(_home_pos.lat, _home_pos.lon);
+			map_projection_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp);
 			dispatch(EVENT_HOME_POSITION_CHANGED);
 		}
 
-- 
cgit v1.2.3


From 7e1ea35571149a9d766cc7122738f206f1ed3427 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 25 Apr 2014 09:43:19 +0200
Subject: mavlink receiver: in hil use map_projection_timestamp for
 hil_local_pos.ref_timestamp

---
 src/modules/mavlink/mavlink_receiver.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src/modules')

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 40121efe4..e9b58bccf 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -795,7 +795,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		if (!_hil_local_proj_inited) {
 			_hil_local_proj_inited = true;
 			_hil_local_alt0 = hil_state.alt / 1000.0f;
-			hil_local_pos.ref_timestamp = timestamp;
+			hil_local_pos.ref_timestamp = map_projection_timestamp();
 			hil_local_pos.ref_lat = lat;
 			hil_local_pos.ref_lon = lon;
 			hil_local_pos.ref_alt = _hil_local_alt0;
-- 
cgit v1.2.3


From 5285eb2f997b590071186a8f695af4375f96e779 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 25 Apr 2014 09:44:40 +0200
Subject: fw att pos estimator: use map projection reference values for local
 pos

---
 src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
index 420a03644..45ca13a7f 100644
--- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -797,7 +797,7 @@ FixedwingEstimator::task_main()
 
 			if (hrt_elapsed_time(&start_time) > 100000) {
 
-				if (!_gps_initialized && (_ekf->GPSstatus == 3)) {
+				if (!_gps_initialized && (_ekf->GPSstatus == 3) && map_projection_initialized()) {
 					_ekf->velNED[0] = _gps.vel_n_m_s;
 					_ekf->velNED[1] = _gps.vel_e_m_s;
 					_ekf->velNED[2] = _gps.vel_d_m_s;
@@ -809,10 +809,9 @@ FixedwingEstimator::task_main()
 					_ekf->InitialiseFilter(_ekf->velNED);
 
 					// Initialize projection
-					_local_pos.ref_lat = _gps.lat;
-					_local_pos.ref_lon = _gps.lon;
+					map_projection_reference(&_local_pos.ref_lat, &_local_pos.ref_lon);
 					_local_pos.ref_alt = alt;
-					_local_pos.ref_timestamp = _gps.timestamp_position;
+					_local_pos.ref_timestamp = map_projection_timestamp();
 
 					// Store 
 					orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
-- 
cgit v1.2.3


From 08bc777208a8510da7cd91fab5059ffa8bd14150 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 25 Apr 2014 13:57:06 +0200
Subject: pos estimator inav: use map_projection_reference to set local pos
 reference lat lon

---
 src/modules/position_estimator_inav/position_estimator_inav_main.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index e88493a5e..91642e5b9 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -626,8 +626,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							z_est[0] = 0.0f;
 							y_est[2] = accel_NED[1];
 
-							local_pos.ref_lat = lat;
-							local_pos.ref_lon = lon;
+							map_projection_reference(&local_pos.ref_lat, &local_pos.ref_lon);
+
 							local_pos.ref_alt = alt;
 							local_pos.ref_timestamp = t;
 						}
-- 
cgit v1.2.3


From 241a99fc28992694f807595ce6bb48f460c9a28a Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 25 Apr 2014 14:45:21 +0200
Subject: position estimator mc: do not initialize map projection because this
 is now handled globally

---
 src/modules/position_estimator_mc/position_estimator_mc_main.c | 5 -----
 1 file changed, 5 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c
index 363961819..ee6457ade 100755
--- a/src/modules/position_estimator_mc/position_estimator_mc_main.c
+++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c
@@ -322,11 +322,6 @@ int position_estimator_mc_thread_main(int argc, char *argv[])
 		lon_current = ((double)(gps.lon)) * 1e-7d;
 		alt_current = gps.alt * 1e-3f;
 		gps_origin_altitude = alt_current;
-		/* initialize coordinates */
-		map_projection_init(lat_current, lon_current);
-		/* publish global position messages only after first GPS message */
-		printf("[pos_est_mc] initialized projection with: lat: %.10f,  lon:%.10f\n", lat_current, lon_current);
-
 	} else {
 		mavlink_log_info(mavlink_fd, "[pos_est_mc] I'm NOT using GPS - I use VICON");
 		/* onboard calculated position estimations */
-- 
cgit v1.2.3


From f7e18eaa5809580878b377dfa78ce0ece96d55a7 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 25 Apr 2014 15:08:06 +0200
Subject: uorb home position topic: add valid flag

---
 src/modules/uORB/topics/home_position.h | 1 +
 1 file changed, 1 insertion(+)

(limited to 'src/modules')

diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h
index 08d11abae..73c8dc50d 100644
--- a/src/modules/uORB/topics/home_position.h
+++ b/src/modules/uORB/topics/home_position.h
@@ -63,6 +63,7 @@ struct home_position_s
 	double lat;				/**< Latitude in degrees 			*/
 	double lon;				/**< Longitude in degrees			*/
 	float alt;				/**< Altitude in meters				*/
+	bool valid;				/**< True if home position is valid */
 };
 
 /**
-- 
cgit v1.2.3


From 06dd10cb018d48f60abd1a5e9f83fbdc67867d8c Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 25 Apr 2014 15:08:38 +0200
Subject: commander: set home position valid flag

---
 src/modules/commander/commander.cpp | 4 ++++
 1 file changed, 4 insertions(+)

(limited to 'src/modules')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 1827f252c..23fe64769 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -617,6 +617,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				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);
 
+				home->valid = true;
+
 				/* announce new home position */
 				if (*home_pub > 0) {
 					orb_publish(ORB_ID(home_position), *home_pub, home);
@@ -999,6 +1001,7 @@ int commander_thread_main(int argc, char *argv[])
 			home.lat = global_position.lat;
 			home.lon = global_position.lon;
 			home.alt = global_position.alt;
+			home.valid = true;
 
 			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);
@@ -1385,6 +1388,7 @@ int commander_thread_main(int argc, char *argv[])
 				home.lat = global_position.lat;
 				home.lon = global_position.lon;
 				home.alt = global_position.alt;
+				home.valid = true;
 
 				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);
-- 
cgit v1.2.3


From 35b0c294568ca916b344e6a9e6a8e27cefb6f8ca Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 25 Apr 2014 15:09:11 +0200
Subject: navigator: set reference for map projection only once

---
 src/modules/navigator/navigator_main.cpp | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index e43028415..2f6bf49f3 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -625,7 +625,6 @@ Navigator::task_main()
 	parameters_update();
 	global_position_update();
 	home_position_update();
-	map_projection_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp);
 	navigation_capabilities_update();
 	offboard_mission_update(_vstatus.is_rotary_wing);
 	onboard_mission_update();
@@ -781,7 +780,6 @@ Navigator::task_main()
 		if (fds[2].revents & POLLIN) {
 			home_position_update();
 			// XXX check if home position really changed
-			map_projection_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp);
 			dispatch(EVENT_HOME_POSITION_CHANGED);
 		}
 
@@ -824,6 +822,11 @@ Navigator::task_main()
 
 		_global_pos_valid = _vstatus.condition_global_position_valid;
 
+		/* set reference for map _projection if global pos is valid and home position is valid and we have not done so already */
+		if (!map_projection_initialized() && _global_pos_valid && _home_pos.valid) {
+			map_projection_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp);
+		}
+
 		/* publish position setpoint triplet if updated */
 		if (_pos_sp_triplet_updated) {
 			_pos_sp_triplet_updated = false;
-- 
cgit v1.2.3


From 4f84cdc8b871b6c8da7f05a58e96f97b65f290c9 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 29 Apr 2014 14:40:01 +0200
Subject: fw_att_pos_estimator: use new global map projection

---
 src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
index 45ca13a7f..79328d516 100644
--- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -797,7 +797,7 @@ FixedwingEstimator::task_main()
 
 			if (hrt_elapsed_time(&start_time) > 100000) {
 
-				if (!_gps_initialized && (_ekf->GPSstatus == 3) && map_projection_initialized()) {
+				if (!_gps_initialized && (_ekf->GPSstatus == 3) && map_projection_global_initialized()) {
 					_ekf->velNED[0] = _gps.vel_n_m_s;
 					_ekf->velNED[1] = _gps.vel_e_m_s;
 					_ekf->velNED[2] = _gps.vel_d_m_s;
@@ -809,9 +809,9 @@ FixedwingEstimator::task_main()
 					_ekf->InitialiseFilter(_ekf->velNED);
 
 					// Initialize projection
-					map_projection_reference(&_local_pos.ref_lat, &_local_pos.ref_lon);
+					map_projection_global_reference(&_local_pos.ref_lat, &_local_pos.ref_lon);
 					_local_pos.ref_alt = alt;
-					_local_pos.ref_timestamp = map_projection_timestamp();
+					_local_pos.ref_timestamp = map_projection_global_timestamp();
 
 					// Store 
 					orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
@@ -1041,7 +1041,7 @@ FixedwingEstimator::task_main()
 
 				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_global_reproject(_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;
-- 
cgit v1.2.3


From a7289a32664e3118bf553d23cfd91ed5c86caa84 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 29 Apr 2014 15:39:26 +0200
Subject: mavlink interface: update mapprojection usage

---
 src/lib/geo/geo.c                        | 11 ++++++++---
 src/lib/geo/geo.h                        | 13 ++++++++++++-
 src/modules/mavlink/mavlink_receiver.cpp |  4 ++--
 3 files changed, 22 insertions(+), 6 deletions(-)

(limited to 'src/modules')

diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 53e601fad..438e354df 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -81,13 +81,13 @@ __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t tim
 {
 	if (strcmp("navigator", getprogname() == 0)) {
 
-		return map_projection_init(&mp_ref, lat_0, lon_0, timestamp);
+		return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp);
 	} else {
 		return -1;
 	}
 }
 
-__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
 {
 
 	ref->lat = lat_0 / 180.0 * M_PI;
@@ -102,9 +102,14 @@ __EXPORT int map_projection_init(struct map_projection_reference_s *ref, double
 	return 0;
 }
 
+__EXPORT int 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
+{
+	map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time());
+}
+
 __EXPORT int map_projection_global_reference(double *ref_lat, double *ref_lon)
 {
-	return map_projection_reference_s(&mp_ref, ref_lat, ref_lon);
+	return map_projection_reference(&mp_ref, ref_lat, ref_lon);
 }
 
 __EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat, double *ref_lon)
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 990f79194..3d7ba050f 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -129,7 +129,18 @@ __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t tim
  * @param lat in degrees (47.1234567°, not 471234567°)
  * @param lon in degrees (8.1234567°, not 81234567°)
  */
-__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp);
+__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref,
+		double lat_0, double lon_0, uint64_t timestamp);
+
+/**
+ * Initializes the map transformation given by the argument and sets the timestamp to now.
+ *
+ * Initializes the transformation between the geographic coordinate system and
+ * the azimuthal equidistant plane
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT int 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
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index b2c5e0079..db8f00e8f 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -766,7 +766,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		if (!_hil_local_proj_inited) {
 			_hil_local_proj_inited = true;
 			_hil_local_alt0 = hil_state.alt / 1000.0f;
-			hil_local_pos.ref_timestamp = map_projection_timestamp();
+			hil_local_pos.ref_timestamp = map_projection_global_timestamp();
 			hil_local_pos.ref_lat = lat;
 			hil_local_pos.ref_lon = lon;
 			hil_local_pos.ref_alt = _hil_local_alt0;
@@ -774,7 +774,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 
 		float x;
 		float y;
-		map_projection_project(lat, lon, &x, &y);
+		map_projection_global_project(lat, lon, &x, &y);
 		hil_local_pos.timestamp = timestamp;
 		hil_local_pos.xy_valid = true;
 		hil_local_pos.z_valid = true;
-- 
cgit v1.2.3


From 5a868751b58ebb6b169beafd6258cd8874440483 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 29 Apr 2014 15:40:04 +0200
Subject: navigator: update mapprojection usage

---
 src/modules/navigator/navigator_main.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 2f6bf49f3..14c568c46 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -823,8 +823,8 @@ Navigator::task_main()
 		_global_pos_valid = _vstatus.condition_global_position_valid;
 
 		/* set reference for map _projection if global pos is valid and home position is valid and we have not done so already */
-		if (!map_projection_initialized() && _global_pos_valid && _home_pos.valid) {
-			map_projection_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp);
+		if (!map_projection_global_initialized() && _global_pos_valid && _home_pos.valid) {
+			map_projection_global_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp);
 		}
 
 		/* publish position setpoint triplet if updated */
-- 
cgit v1.2.3


From 53d23c67d72a6395cef509817df25264f05fbd85 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 29 Apr 2014 15:40:34 +0200
Subject: mc pos ctrl: revert to local map projection

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index bf7b6a19c..6b797f222 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -468,15 +468,17 @@ MulticopterPositionControl::update_ref()
 
 		if (_ref_timestamp != 0) {
 			/* calculate current position setpoint in global frame */
-			map_projection_reproject(_pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp);
+			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(lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
+			map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
 			_pos_sp(2) = -(alt_sp - _ref_alt);
 		}
 
@@ -675,7 +677,8 @@ MulticopterPositionControl::task_main()
 					_reset_alt_sp = true;
 
 					/* project setpoint to local frame */
-					map_projection_project(_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
+					map_projection_project(&_ref_pos,
+							       _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
 							       &_pos_sp.data[0], &_pos_sp.data[1]);
 					_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
 
-- 
cgit v1.2.3


From 510678bdae82a04151b6d5dcd5b02bee6f96abfd Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 29 Apr 2014 15:40:54 +0200
Subject: pos estimator inav: revert to local map projection

---
 .../position_estimator_inav_main.c                 | 24 ++++++++++++++--------
 1 file changed, 16 insertions(+), 8 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 91642e5b9..368424853 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -549,7 +549,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (updated) {
 				orb_copy(ORB_ID(home_position), home_position_sub, &home);
 
-				if (home.timestamp != home_timestamp && map_projection_initialized()) {
+				if (home.timestamp != home_timestamp) {
 					home_timestamp = home.timestamp;
 
 					double est_lat, est_lon;
@@ -558,9 +558,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					if (ref_inited) {
 						/* calculate current estimated position in global frame */
 						est_alt = local_pos.ref_alt - local_pos.z;
-						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);
 					}
 
+					/* update reference */
+					map_projection_init(&ref, home.lat, home.lon);
+
 					/* update baro offset */
 					baro_offset += home.alt - local_pos.ref_alt;
 
@@ -571,7 +574,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 					if (ref_inited) {
 						/* reproject position estimate with new reference */
-						map_projection_project(est_lat, est_lon, &x_est[0], &y_est[0]);
+						map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]);
 						z_est[0] = -(est_alt - local_pos.ref_alt);
 					}
 
@@ -602,7 +605,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					}
 				}
 
-				if (gps_valid && map_projection_initialized()) {
+				if (gps_valid) {
 					double lat = gps.lat * 1e-7;
 					double lon = gps.lon * 1e-7;
 					float alt = gps.alt * 1e-3;
@@ -626,17 +629,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							z_est[0] = 0.0f;
 							y_est[2] = accel_NED[1];
 
-							map_projection_reference(&local_pos.ref_lat, &local_pos.ref_lon);
-
+							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(&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);
 						}
 					}
 
 					if (ref_inited) {
 						/* project GPS lat lon to plane */
 						float gps_proj[2];
-						map_projection_project(lat, lon, &(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) {
@@ -930,7 +938,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				global_pos.time_gps_usec = gps.time_gps_usec;
 
 				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;
-- 
cgit v1.2.3


From 606f3cba5c932d6aba0e383e915b02b78009fd9f Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 29 Apr 2014 15:41:12 +0200
Subject: pos estimator mc: revert to local map projection

---
 src/modules/position_estimator_mc/position_estimator_mc_main.c | 5 +++++
 1 file changed, 5 insertions(+)

(limited to 'src/modules')

diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c
index ee6457ade..363961819 100755
--- a/src/modules/position_estimator_mc/position_estimator_mc_main.c
+++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c
@@ -322,6 +322,11 @@ int position_estimator_mc_thread_main(int argc, char *argv[])
 		lon_current = ((double)(gps.lon)) * 1e-7d;
 		alt_current = gps.alt * 1e-3f;
 		gps_origin_altitude = alt_current;
+		/* initialize coordinates */
+		map_projection_init(lat_current, lon_current);
+		/* publish global position messages only after first GPS message */
+		printf("[pos_est_mc] initialized projection with: lat: %.10f,  lon:%.10f\n", lat_current, lon_current);
+
 	} else {
 		mavlink_log_info(mavlink_fd, "[pos_est_mc] I'm NOT using GPS - I use VICON");
 		/* onboard calculated position estimations */
-- 
cgit v1.2.3


From 60ccbaa8bb59f4d510b74f7599ef60ef45837180 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 5 May 2014 15:37:53 +0200
Subject: init global map reference in commander and not in navigator

---
 src/modules/commander/commander.cpp      | 14 ++++++++++++--
 src/modules/navigator/navigator_main.cpp |  5 -----
 2 files changed, 12 insertions(+), 7 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 2db30b066..54834cf9b 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -87,6 +87,7 @@
 #include <systemlib/err.h>
 #include <systemlib/cpuload.h>
 #include <systemlib/rc_check.h>
+#include <geo/geo.h>
 
 #include "px4_custom_mode.h"
 #include "commander_helper.h"
@@ -366,7 +367,7 @@ static orb_advert_t status_pub;
 transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy)
 {
     transition_result_t arming_res = TRANSITION_NOT_CHANGED;
-    
+
     // Transition the armed state. By passing mavlink_fd to arming_state_transition it will
     // output appropriate error messages if the state cannot transition.
     arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
@@ -375,7 +376,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armed
     } else if (arming_res == TRANSITION_DENIED) {
         tune_negative(true);
     }
-    
+
     return arming_res;
 }
 
@@ -571,6 +572,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 				home->valid = true;
 
+				/* set reference for map _projection */
+				map_projection_global_init(home->lat, home->lon, hrt_absolute_time());
+
 				/* announce new home position */
 				if (*home_pub > 0) {
 					orb_publish(ORB_ID(home_position), *home_pub, home);
@@ -958,6 +962,9 @@ int commander_thread_main(int argc, char *argv[])
 			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);
 
+			/* set reference for map _projection */
+			map_projection_global_init(home.lat, home.lon, hrt_absolute_time());
+
 			/* announce new home position */
 			if (home_pub > 0) {
 				orb_publish(ORB_ID(home_position), home_pub, &home);
@@ -1345,6 +1352,9 @@ int commander_thread_main(int argc, char *argv[])
 				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);
 
+				/* set reference for map _projection */
+				map_projection_global_init(home.lat, home.lon, hrt_absolute_time());
+
 				/* announce new home position */
 				if (home_pub > 0) {
 					orb_publish(ORB_ID(home_position), home_pub, &home);
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 14c568c46..12fd35a0a 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -822,11 +822,6 @@ Navigator::task_main()
 
 		_global_pos_valid = _vstatus.condition_global_position_valid;
 
-		/* set reference for map _projection if global pos is valid and home position is valid and we have not done so already */
-		if (!map_projection_global_initialized() && _global_pos_valid && _home_pos.valid) {
-			map_projection_global_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp);
-		}
-
 		/* publish position setpoint triplet if updated */
 		if (_pos_sp_triplet_updated) {
 			_pos_sp_triplet_updated = false;
-- 
cgit v1.2.3


From 67e3c808d24773c7c4bd4608866de1edb617cb04 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 5 May 2014 15:40:02 +0200
Subject: remove home position valid flag

---
 src/modules/commander/commander.cpp     | 4 ----
 src/modules/uORB/topics/home_position.h | 1 -
 2 files changed, 5 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 54834cf9b..bc3bdb782 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -570,8 +570,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				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);
 
-				home->valid = true;
-
 				/* set reference for map _projection */
 				map_projection_global_init(home->lat, home->lon, hrt_absolute_time());
 
@@ -957,7 +955,6 @@ int commander_thread_main(int argc, char *argv[])
 			home.lat = global_position.lat;
 			home.lon = global_position.lon;
 			home.alt = global_position.alt;
-			home.valid = true;
 
 			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);
@@ -1347,7 +1344,6 @@ int commander_thread_main(int argc, char *argv[])
 				home.lat = global_position.lat;
 				home.lon = global_position.lon;
 				home.alt = global_position.alt;
-				home.valid = true;
 
 				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);
diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h
index 73c8dc50d..08d11abae 100644
--- a/src/modules/uORB/topics/home_position.h
+++ b/src/modules/uORB/topics/home_position.h
@@ -63,7 +63,6 @@ struct home_position_s
 	double lat;				/**< Latitude in degrees 			*/
 	double lon;				/**< Longitude in degrees			*/
 	float alt;				/**< Altitude in meters				*/
-	bool valid;				/**< True if home position is valid */
 };
 
 /**
-- 
cgit v1.2.3


From 6e0690fde1073649a4ef201671ca947cb83edc37 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 5 May 2014 16:42:52 +0200
Subject: init global map projection when gps is valid

---
 src/lib/geo/geo.c                   |  2 +-
 src/modules/commander/commander.cpp | 18 +++++++++---------
 2 files changed, 10 insertions(+), 10 deletions(-)

(limited to 'src/modules')

diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 438e354df..10a518908 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -79,7 +79,7 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference
 
 __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
 {
-	if (strcmp("navigator", getprogname() == 0)) {
+	if (strcmp("commander", getprogname() == 0)) {
 
 		return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp);
 	} else {
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index bc3bdb782..f8f215cd7 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -570,9 +570,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				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);
 
-				/* set reference for map _projection */
-				map_projection_global_init(home->lat, home->lon, hrt_absolute_time());
-
 				/* announce new home position */
 				if (*home_pub > 0) {
 					orb_publish(ORB_ID(home_position), *home_pub, home);
@@ -959,9 +956,6 @@ int commander_thread_main(int argc, char *argv[])
 			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);
 
-			/* set reference for map _projection */
-			map_projection_global_init(home.lat, home.lon, hrt_absolute_time());
-
 			/* announce new home position */
 			if (home_pub > 0) {
 				orb_publish(ORB_ID(home_position), home_pub, &home);
@@ -1130,6 +1124,15 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
 		}
 
+		/* Initialize map projection if gps is valid */
+		if (!map_projection_global_initialized()
+				&& (gps_position.eph < eph_epv_threshold)
+				&& (gps_position.epv < eph_epv_threshold)) {
+			/* set reference for map _projection */
+			map_projection_global_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, hrt_absolute_time());
+
+		}
+
 		/* start RC input check */
 		if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
 			/* handle the case where RC signal was regained */
@@ -1348,9 +1351,6 @@ int commander_thread_main(int argc, char *argv[])
 				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);
 
-				/* set reference for map _projection */
-				map_projection_global_init(home.lat, home.lon, hrt_absolute_time());
-
 				/* announce new home position */
 				if (home_pub > 0) {
 					orb_publish(ORB_ID(home_position), home_pub, &home);
-- 
cgit v1.2.3


From 7ab428976e703284037eb2f24f68ba105db9d6b8 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 5 May 2014 16:49:21 +0200
Subject: fix wrong variable name

---
 src/modules/commander/commander.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index f8f215cd7..c15651ed5 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1126,8 +1126,8 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* Initialize map projection if gps is valid */
 		if (!map_projection_global_initialized()
-				&& (gps_position.eph < eph_epv_threshold)
-				&& (gps_position.epv < eph_epv_threshold)) {
+				&& (gps_position.eph_m < eph_epv_threshold)
+				&& (gps_position.epv_m < eph_epv_threshold)) {
 			/* set reference for map _projection */
 			map_projection_global_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, hrt_absolute_time());
 
-- 
cgit v1.2.3


From 596b06ff2e13b170545b4f13da1c64e66088aedc Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 6 May 2014 11:22:18 +0200
Subject: commander: init gps eph and epv to large values, safer map projection
 initialization

---
 src/modules/commander/commander.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

(limited to 'src/modules')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index c15651ed5..71de33bcc 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -58,6 +58,7 @@
 #include <string.h>
 #include <math.h>
 #include <poll.h>
+#include <float.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
@@ -788,6 +789,8 @@ int commander_thread_main(int argc, char *argv[])
 	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	struct vehicle_gps_position_s gps_position;
 	memset(&gps_position, 0, sizeof(gps_position));
+	gps_position.eph_m = FLT_MAX;
+	gps_position.epv_m = FLT_MAX;
 
 	/* Subscribe to sensor topic */
 	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
@@ -1127,7 +1130,8 @@ int commander_thread_main(int argc, char *argv[])
 		/* Initialize map projection if gps is valid */
 		if (!map_projection_global_initialized()
 				&& (gps_position.eph_m < eph_epv_threshold)
-				&& (gps_position.epv_m < eph_epv_threshold)) {
+				&& (gps_position.epv_m < eph_epv_threshold)
+				&& hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) {
 			/* set reference for map _projection */
 			map_projection_global_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, hrt_absolute_time());
 
-- 
cgit v1.2.3


From 548c7f4aaf93bddeb05053cd4dede945fede22ef Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 6 May 2014 14:43:23 +0200
Subject: geo: introduce global/local coordinate frame converter which uses the
 map projection but also converts altitude

---
 src/lib/geo/geo.c                   | 36 ++++++++++++++++++++++++++++++++++++
 src/lib/geo/geo.h                   | 26 ++++++++++++++++++++++++++
 src/modules/commander/commander.cpp |  5 ++---
 3 files changed, 64 insertions(+), 3 deletions(-)

(limited to 'src/modules')

diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 220ea149d..c9d48491e 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -56,6 +56,7 @@
  */
 
 static struct map_projection_reference_s mp_ref = {0};
+static struct globallocal_converter_reference_s gl_ref = {0};
 
 __EXPORT bool map_projection_global_initialized()
 {
@@ -185,6 +186,41 @@ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *r
 	return 0;
 }
 
+__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp)
+{
+	if (strcmp("commander", getprogname() == 0)) {
+		gl_ref.alt = alt_0;
+		gl_ref.init_done = true;
+		return map_projection_global_init(lat_0, lon_0, timestamp);
+	} else {
+		return -1;
+	}
+}
+
+__EXPORT bool globallocalconverter_initialized()
+{
+	return gl_ref.init_done && map_projection_global_initialized();
+}
+
+__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z)
+{
+	if (!map_projection_global_initialized()) {
+		return -1;
+	}
+
+	map_projection_global_project(lat, lon, x, y);
+	*z = gl_ref.alt - alt;
+}
+
+__EXPORT int globallocalconverter_toglobal(float x, float y, float z,  double *lat, double *lon, float *alt)
+{
+	if (!map_projection_global_initialized()) {
+		return -1;
+	}
+
+	map_projection_global_reproject(x, y, lat, lon);
+	*alt = gl_ref.alt - z;
+}
 
 __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
 {
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 20fb34ca3..98dfbd14b 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -77,6 +77,11 @@ struct map_projection_reference_s {
 	uint64_t timestamp;
 };
 
+struct globallocal_converter_reference_s {
+	float alt;
+	bool init_done;
+};
+
 /**
  * Checks if global projection was initialized
  * @return true if map was initialized before, false else
@@ -190,6 +195,27 @@ __EXPORT int map_projection_global_reproject(float x, float y, double *lat, doub
  */
 __EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
 
+/**
+ * Initialize the global mapping between global position (spherical) and local position (NED).
+ */
+__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp);
+
+/**
+ * Checks if globallocalconverter was initialized
+ * @return true if map was initialized before, false else
+ */
+__EXPORT bool globallocalconverter_initialized();
+
+/**
+ * Convert from global position coordinates to local position coordinates using the global reference
+ */
+__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z);
+
+/**
+ * Convert from local position coordinates to global position coordinates using the global reference
+ */
+__EXPORT int globallocalconverter_toglobal(float x, float y, float z,  double *lat, double *lon, float *alt);
+
 /**
  * Returns the distance to the next waypoint in meters.
  *
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 71de33bcc..98887a0e4 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1132,9 +1132,8 @@ int commander_thread_main(int argc, char *argv[])
 				&& (gps_position.eph_m < eph_epv_threshold)
 				&& (gps_position.epv_m < eph_epv_threshold)
 				&& hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) {
-			/* set reference for map _projection */
-			map_projection_global_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, hrt_absolute_time());
-
+			/* set reference for global coordinates <--> local coordiantes conversion and map_projection */
+			globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
 		}
 
 		/* start RC input check */
-- 
cgit v1.2.3


From a6a2efb651d875171aaec698a836f26b548ca0d6 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 6 May 2014 16:59:09 +0200
Subject: mavlink receiver: switch back to use local and not systemwide map
 projection

---
 src/modules/mavlink/mavlink_receiver.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 1cfbde1d2..7c93c1c00 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -778,7 +778,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		if (!_hil_local_proj_inited) {
 			_hil_local_proj_inited = true;
 			_hil_local_alt0 = hil_state.alt / 1000.0f;
-			hil_local_pos.ref_timestamp = map_projection_global_timestamp();
+			map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon);
+			hil_local_pos.ref_timestamp = timestamp;
 			hil_local_pos.ref_lat = lat;
 			hil_local_pos.ref_lon = lon;
 			hil_local_pos.ref_alt = _hil_local_alt0;
@@ -786,7 +787,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 
 		float x;
 		float y;
-		map_projection_global_project(lat, lon, &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;
-- 
cgit v1.2.3


From 801d1d31983d404d5ebe8f5750359f2d8c7fdf43 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 1 Jul 2014 09:51:35 +0200
Subject: commander: Update after merge

---
 src/modules/commander/commander.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 326a3e1f3..d7425cbb4 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -826,8 +826,8 @@ int commander_thread_main(int argc, char *argv[])
 	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	struct vehicle_gps_position_s gps_position;
 	memset(&gps_position, 0, sizeof(gps_position));
-	gps_position.eph_m = FLT_MAX;
-	gps_position.epv_m = FLT_MAX;
+	gps_position.eph = FLT_MAX;
+	gps_position.epv = FLT_MAX;
 
 	/* Subscribe to sensor topic */
 	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
@@ -1261,8 +1261,8 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* Initialize map projection if gps is valid */
 		if (!map_projection_global_initialized()
-				&& (gps_position.eph_m < eph_epv_threshold)
-				&& (gps_position.epv_m < eph_epv_threshold)
+				&& (gps_position.eph < eph_epv_threshold)
+				&& (gps_position.epv < eph_epv_threshold)
 				&& hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) {
 			/* set reference for global coordinates <--> local coordiantes conversion and map_projection */
 			globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
-- 
cgit v1.2.3


From 89986cf3e5cc8a9e18d56fe971d3e089232e3beb Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 11 Aug 2014 13:46:13 +0200
Subject: epv/eph threshold variable names changed

---
 src/modules/commander/commander.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 02571f56f..6c2c03070 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1341,8 +1341,8 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* Initialize map projection if gps is valid */
 		if (!map_projection_global_initialized()
-				&& (gps_position.eph < eph_epv_threshold)
-				&& (gps_position.epv < eph_epv_threshold)
+				&& (gps_position.eph < eph_threshold)
+				&& (gps_position.epv < epv_threshold)
 				&& hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) {
 			/* set reference for global coordinates <--> local coordiantes conversion and map_projection */
 			globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
-- 
cgit v1.2.3