towards a global map projection instance, WIP: need to remove local updates of the reference and add a global map update

6 files changed, 48 insertions, 44 deletions

diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 9a24ff50e..32d52aadc 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -55,33 +55,47 @@
  * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
  */
 
-__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
+struct map_projection_reference_s mp_ref;
+
+__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
 {
-	ref->lat = lat_0 / 180.0 * M_PI;
-	ref->lon = lon_0 / 180.0 * M_PI;
+	mp_ref.lat = lat_0 / 180.0 * M_PI;
+	mp_ref.lon = lon_0 / 180.0 * M_PI;
+
+	mp_ref.sin_lat = sin(mp_ref.lat);
+	mp_ref.cos_lat = cos(mp_ref.lat);
 
-	ref->sin_lat = sin(ref->lat);
-	ref->cos_lat = cos(ref->lat);
+	mp_ref.init_done = true;
 }
 
-__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
+__EXPORT bool map_projection_project(double lat, double lon, float *x, float *y)
 {
+	if (!mp_ref.init_done) {
+		return false;
+	}
+
 	double lat_rad = lat / 180.0 * M_PI;
 	double lon_rad = lon / 180.0 * M_PI;
 
 	double sin_lat = sin(lat_rad);
 	double cos_lat = cos(lat_rad);
-	double cos_d_lon = cos(lon_rad - ref->lon);
+	double cos_d_lon = cos(lon_rad - mp_ref.lon);
 
-	double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
+	double c = acos(mp_ref.sin_lat * sin_lat + mp_ref.cos_lat * cos_lat * cos_d_lon);
 	double k = (c == 0.0) ? 1.0 : (c / sin(c));
 
-	*x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
-	*y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH;
+	*x = k * (mp_ref.cos_lat * sin_lat - mp_ref.sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
+	*y = k * cos_lat * sin(lon_rad - mp_ref.lon) * CONSTANTS_RADIUS_OF_EARTH;
+
+	return true;
 }
 
-__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
+__EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon)
 {
+	if (!mp_ref.init_done) {
+		return false;
+	}
+
 	float x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
 	float y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
 	double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
@@ -92,12 +106,12 @@ __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, f
 	double lon_rad;
 
 	if (c != 0.0) {
-		lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
-		lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
+		lat_rad = asin(cos_c * mp_ref.sin_lat + (x_rad * sin_c * mp_ref.cos_lat) / c);
+		lon_rad = (mp_ref.lon + atan2(y_rad * sin_c, c * mp_ref.cos_lat * cos_c - x_rad * mp_ref.sin_lat * sin_c));
 
 	} else {
-		lat_rad = ref->lat;
-		lon_rad = ref->lon;
+		lat_rad = mp_ref.lat;
+		lon_rad = mp_ref.lon;
 	}
 
 	*lat = lat_rad * 180.0 / M_PI;
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 0a3f85d97..b48d26a66 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -71,6 +71,7 @@ struct map_projection_reference_s {
 	double lon;
 	double sin_lat;
 	double cos_lat;
+	bool init_done;
 };
 
 /**
@@ -80,7 +81,7 @@ struct map_projection_reference_s {
  * @param lat in degrees (47.1234567°, not 471234567°)
  * @param lon in degrees (8.1234567°, not 81234567°)
  */
-__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
+__EXPORT void map_projection_init(double lat_0, double lon_0);
 
 /**
  * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
@@ -88,8 +89,9 @@ __EXPORT void map_projection_init(struct map_projection_reference_s *ref, double
  * @param y east
  * @param lat in degrees (47.1234567°, not 471234567°)
  * @param lon in degrees (8.1234567°, not 81234567°)
+ * @return true if map_projection_init was called before, false else
  */
-__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
+__EXPORT bool map_projection_project(double lat, double lon, float *x, float *y);
 
 /**
  * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
@@ -98,8 +100,9 @@ __EXPORT void map_projection_project(struct map_projection_reference_s *ref, dou
  * @param y east
  * @param lat in degrees (47.1234567°, not 471234567°)
  * @param lon in degrees (8.1234567°, not 81234567°)
+ * @return true if map_projection_init was called before, false else
  */
-__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
+__EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon);
 
 /**
  * Returns the distance to the next waypoint in meters.
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;