diff options
-rw-r--r-- | src/lib/geo/geo.c | 11 | ||||
-rw-r--r-- | src/lib/geo/geo.h | 13 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 4 |
3 files changed, 22 insertions, 6 deletions
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; |