Use updated map_projection_XXX functions in apps

4 files changed, 14 insertions, 11 deletions

diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 668bac5d9..a4d5560c7 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/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
-		// states that multiple things could change, 
-		// should make map_projection take reference
-		// lat/lon and not have init
-		map_projection_init(lat0, lon0);
+		map_projection_init(&ref, 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",
@@ -327,7 +325,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;
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
index 46ee4b6c8..5021b9927 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
@@ -49,6 +49,7 @@
 #include <controllib/block/BlockParam.hpp>
 #include <controllib/uorb/UOrbSubscription.hpp>
 #include <controllib/uorb/UOrbPublication.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  */
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 1dbe56495..624740237 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -132,6 +132,7 @@ static orb_advert_t telemetry_status_pub = -1;
 static int32_t lat0 = 0;
 static int32_t lon0 = 0;
 static double alt0 = 0;
+struct map_projection_reference_s hil_ref;
 
 static void
 handle_message(mavlink_message_t *msg)
@@ -653,7 +654,7 @@ handle_message(mavlink_message_t *msg)
 				bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve?
 				double lat = hil_state.lat*1e-7;
 				double lon = hil_state.lon*1e-7;
-				map_projection_project(lat, lon, &x, &y); 
+				map_projection_project(&hil_ref, lat, lon, &x, &y);
 				hil_local_pos.timestamp = timestamp;
 				hil_local_pos.xy_valid = true;
 				hil_local_pos.z_valid = true;
@@ -679,7 +680,7 @@ handle_message(mavlink_message_t *msg)
 				lat0 = hil_state.lat;
 				lon0 = hil_state.lon;
 				alt0 = hil_state.alt / 1000.0f;
-				map_projection_init(hil_state.lat, hil_state.lon);
+				map_projection_init(&hil_ref, hil_state.lat, hil_state.lon);
 			}
 
 			/* Calculate Rotation Matrix */
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 a14354138..eddf6e94e 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -206,6 +206,8 @@ 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));
 
 	uint16_t accel_updates = 0;
 	uint16_t baro_updates = 0;
@@ -560,7 +562,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							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,7 +571,7 @@ 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, gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[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];
@@ -836,7 +838,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 			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;