ported more geo functions to cpp

1 files changed, 93 insertions, 0 deletions

diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp
index 6fad681c9..04094de8b 100644
--- a/src/platforms/ros/geo.cpp
+++ b/src/platforms/ros/geo.cpp
@@ -171,3 +171,96 @@ __EXPORT float _wrap_360(float bearing)
 
 	return bearing;
 }
+
+__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref)
+{
+	return ref->init_done;
+}
+
+__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref)
+{
+	return ref->timestamp;
+}
+
+__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_rad = lat_0 * M_DEG_TO_RAD;
+	ref->lon_rad = lon_0 * M_DEG_TO_RAD;
+	ref->sin_lat = sin(ref->lat_rad);
+	ref->cos_lat = cos(ref->lat_rad);
+
+	ref->timestamp = timestamp;
+	ref->init_done = true;
+
+	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
+{
+	return map_projection_init_timestamped(ref, lat_0, lon_0, px4::get_time_micros());
+}
+
+__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad)
+{
+	if (!map_projection_initialized(ref)) {
+		return -1;
+	}
+
+	*ref_lat_rad = ref->lat_rad;
+	*ref_lon_rad = ref->lon_rad;
+
+	return 0;
+}
+
+__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
+{
+	if (!map_projection_initialized(ref)) {
+		return -1;
+	}
+
+	double lat_rad = lat * M_DEG_TO_RAD;
+	double lon_rad = lon * M_DEG_TO_RAD;
+
+	double sin_lat = sin(lat_rad);
+	double cos_lat = cos(lat_rad);
+	double cos_d_lon = cos(lon_rad - ref->lon_rad);
+
+	double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
+	double k = (fabs(c) < DBL_EPSILON) ? 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_rad) * CONSTANTS_RADIUS_OF_EARTH;
+
+	return 0;
+}
+
+__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
+{
+	if (!map_projection_initialized(ref)) {
+		return -1;
+	}
+
+	double x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
+	double y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
+	double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
+	double sin_c = sin(c);
+	double cos_c = cos(c);
+
+	double lat_rad;
+	double lon_rad;
+
+	if (fabs(c) > DBL_EPSILON) {
+		lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
+		lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
+
+	} else {
+		lat_rad = ref->lat_rad;
+		lon_rad = ref->lon_rad;
+	}
+
+	*lat = lat_rad * 180.0 / M_PI;
+	*lon = lon_rad * 180.0 / M_PI;
+
+	return 0;
+}