From 3a898e54adbaac7fda28e0b9079076d9cdf8028c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 14:30:29 +0200 Subject: [PATCH 01/41] towards a global map projection instance, WIP: need to remove local updates of the reference and add a global map update --- src/lib/geo/geo.c | 46 ++++++++++++------- src/lib/geo/geo.h | 9 ++-- .../fw_att_pos_estimator_main.cpp | 6 +-- src/modules/mavlink/mavlink_receiver.cpp | 4 +- .../mc_pos_control/mc_pos_control_main.cpp | 9 ++-- .../position_estimator_inav_main.c | 20 +++----- 6 files changed, 49 insertions(+), 45 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 9a24ff50e2..32d52aadc5 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 -{ - ref->lat = lat_0 / 180.0 * M_PI; - ref->lon = lon_0 / 180.0 * M_PI; +struct map_projection_reference_s mp_ref; - ref->sin_lat = sin(ref->lat); - ref->cos_lat = cos(ref->lat); +__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 +{ + 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); + + 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 0a3f85d970..b48d26a66f 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 9d2d9ed5f3..420a03644e 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 adafdd1395..df61c3c156 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 dc0aa172a4..963349d42e 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 368424853c..b803dfb581 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; From 6517c201f701da97acde1d7e3cc72712b9a1613c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 15:59:13 +0200 Subject: [PATCH 02/41] geo: add missing return, make map projection reference static --- src/lib/geo/geo.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 32d52aadc5..60994940f9 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -55,7 +55,7 @@ * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ -struct map_projection_reference_s mp_ref; +static 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 { @@ -116,6 +116,8 @@ __EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lo *lat = lat_rad * 180.0 / M_PI; *lon = lon_rad * 180.0 / M_PI; + + return true; } From fbcb248bc3a91d1c26f6f47a2a46badd2d8a58f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 15:59:49 +0200 Subject: [PATCH 03/41] navigator: init map projection on home pos update --- src/modules/navigator/navigator_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d4d23396a1..b7cf49cfa9 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); } From 8bb1b39399eb09ddf93c2682713d80be4f776c5d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 16:12:28 +0200 Subject: [PATCH 04/41] mavlink receiver: hil: local pos reference lat lon from first data --- src/modules/mavlink/mavlink_receiver.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index df61c3c156..40121efe4f 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; From 35d15720ef79dae4d6f6202092d4869a455fc7d2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:04:27 +0200 Subject: [PATCH 05/41] geo: add timestamp and function to get init state --- src/lib/geo/geo.c | 10 ++++++++-- src/lib/geo/geo.h | 7 +++++++ 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 60994940f9..dbbf1f80cc 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -57,6 +57,11 @@ static struct map_projection_reference_s mp_ref; +__EXPORT bool map_projection_inited() +{ + return mp_ref.init_done; +} + __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 { mp_ref.lat = lat_0 / 180.0 * M_PI; @@ -65,12 +70,13 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are mp_ref.sin_lat = sin(mp_ref.lat); mp_ref.cos_lat = cos(mp_ref.lat); + mp_ref.timestamp = hrt_absolute_time(); mp_ref.init_done = true; } __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) { - if (!mp_ref.init_done) { + if (!map_projection_inited()) { return false; } @@ -92,7 +98,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 (!mp_ref.init_done) { + if (!map_projection_inited()) { return false; } diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index b48d26a66f..b19bfe4625 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -72,8 +72,15 @@ struct map_projection_reference_s { double sin_lat; double cos_lat; bool init_done; + uint64_t timestamp; }; +/** + * Initializes the map transformation. + * @return true if map_projection_init was called before, false else + */ +__EXPORT bool map_projection_inited(); + /** * Initializes the map transformation. * From 9f2da53f08f5d186d95653bad9afa04e8c7276d2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:05:30 +0200 Subject: [PATCH 06/41] 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(-) 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 963349d42e..fa3438ea82 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) { From 618ac319e63a6597cc62df9c810d76cdc094012b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:06:10 +0200 Subject: [PATCH 07/41] pos estimator inav: check if map projection is initialized --- .../position_estimator_inav_main.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 b803dfb581..d01bf488f9 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])); From b249d04e52ae34d43d9d80638d6f1ead476f39ea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:13:06 +0200 Subject: [PATCH 08/41] geo: get function for timestamp of map projection --- src/lib/geo/geo.c | 5 +++++ src/lib/geo/geo.h | 7 +++++++ 2 files changed, 12 insertions(+) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index dbbf1f80cc..1e3ef56aef 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -62,6 +62,11 @@ __EXPORT bool map_projection_inited() return mp_ref.init_done; } +__EXPORT uint64_t map_projection_timestamp() +{ + return mp_ref.timestamp; +} + __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 { mp_ref.lat = lat_0 / 180.0 * M_PI; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index b19bfe4625..913e83b6e1 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -81,6 +81,13 @@ struct map_projection_reference_s { */ __EXPORT bool map_projection_inited(); + +/** + * Initializes the map transformation. + * @return the timestamp of the map_projection + */ +__EXPORT uint64_t map_projection_timestamp(); + /** * Initializes the map transformation. * From 4e30784d6c506cdfdf58c623da20cffcb799e496 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:16:20 +0200 Subject: [PATCH 09/41] rename map_projection_inited to map_projection_initialized --- src/lib/geo/geo.c | 6 +++--- src/lib/geo/geo.h | 2 +- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 1e3ef56aef..725a04a298 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 913e83b6e1..66e4dc75e8 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 d01bf488f9..e88493a5e2 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; From 9ebd6b348687d5563b87c5c872d432adbfad696f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 09:41:46 +0200 Subject: [PATCH 10/41] geo: interface to get reference lat lon, option to set reference timestamp on initialization --- src/lib/geo/geo.c | 16 ++++++++++++++-- src/lib/geo/geo.h | 8 +++++++- 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 725a04a298..a2117ecd01 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -67,7 +67,7 @@ __EXPORT uint64_t map_projection_timestamp() return mp_ref.timestamp; } -__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 +__EXPORT void map_projection_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 { mp_ref.lat = lat_0 / 180.0 * M_PI; mp_ref.lon = lon_0 / 180.0 * M_PI; @@ -75,10 +75,22 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are mp_ref.sin_lat = sin(mp_ref.lat); mp_ref.cos_lat = cos(mp_ref.lat); - mp_ref.timestamp = hrt_absolute_time(); + mp_ref.timestamp = timestamp; mp_ref.init_done = true; } +__EXPORT bool map_projection_reference(double *ref_lat, double *ref_lon) +{ + if (!map_projection_initialized()) { + return false; + } + + *ref_lat = mp_ref.lat; + *ref_lon = mp_ref.lon; + + return true; +} + __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) { if (!map_projection_initialized()) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 66e4dc75e8..b5e6596855 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -88,6 +88,12 @@ __EXPORT bool map_projection_initialized(); */ __EXPORT uint64_t map_projection_timestamp(); +/** + * Writes the reference values to ref_lat and ref_lon + * @return true if map_projection_init was called before, false else + */ +__EXPORT bool map_projection_reference(double *ref_lat, double *ref_lon); + /** * Initializes the map transformation. * @@ -95,7 +101,7 @@ __EXPORT uint64_t map_projection_timestamp(); * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT void map_projection_init(double lat_0, double lon_0); +__EXPORT void map_projection_init(double lat_0, double lon_0, uint64_t timestamp); /** * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane From f5d9023b1060f10c14d016492c88375c6fdc22aa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 09:42:28 +0200 Subject: [PATCH 11/41] navigator: set home_pos timestamp as map projection reference timestamp --- src/modules/navigator/navigator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b7cf49cfa9..228324789b 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); } From 7e1ea35571149a9d766cc7122738f206f1ed3427 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 09:43:19 +0200 Subject: [PATCH 12/41] 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(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 40121efe4f..e9b58bccf2 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; From 5285eb2f997b590071186a8f695af4375f96e779 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 09:44:40 +0200 Subject: [PATCH 13/41] fw att pos estimator: use map projection reference values for local pos --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) 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 420a03644e..45ca13a7fb 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); From 08bc777208a8510da7cd91fab5059ffa8bd14150 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 13:57:06 +0200 Subject: [PATCH 14/41] pos estimator inav: use map_projection_reference to set local pos reference lat lon --- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 e88493a5e2..91642e5b92 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; } From 241a99fc28992694f807595ce6bb48f460c9a28a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 14:45:21 +0200 Subject: [PATCH 15/41] position estimator mc: do not initialize map projection because this is now handled globally --- .../position_estimator_mc/position_estimator_mc_main.c | 5 ----- 1 file changed, 5 deletions(-) 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 3639618193..ee6457aded 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 */ From aab64af8844e03b04f5616c1beecf18c9f6a7d81 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 14:47:37 +0200 Subject: [PATCH 16/41] geo: map projection: safer initialization, only accept init from navigator, return int instead of bool --- src/lib/geo/geo.c | 41 ++++++++++++++++++++++++----------------- src/lib/geo/geo.h | 16 ++++++++-------- 2 files changed, 32 insertions(+), 25 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index a2117ecd01..6cbf74e466 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -55,7 +55,7 @@ * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ -static struct map_projection_reference_s mp_ref; +static struct map_projection_reference_s mp_ref = {0}; __EXPORT bool map_projection_initialized() { @@ -67,34 +67,41 @@ __EXPORT uint64_t map_projection_timestamp() return mp_ref.timestamp; } -__EXPORT void map_projection_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 +__EXPORT int map_projection_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 { - mp_ref.lat = lat_0 / 180.0 * M_PI; - mp_ref.lon = lon_0 / 180.0 * M_PI; + if (strcmp("navigator", getprogname() == 0)) { - mp_ref.sin_lat = sin(mp_ref.lat); - mp_ref.cos_lat = cos(mp_ref.lat); + mp_ref.lat = lat_0 / 180.0 * M_PI; + mp_ref.lon = lon_0 / 180.0 * M_PI; - mp_ref.timestamp = timestamp; - mp_ref.init_done = true; + mp_ref.sin_lat = sin(mp_ref.lat); + mp_ref.cos_lat = cos(mp_ref.lat); + + mp_ref.timestamp = timestamp; + mp_ref.init_done = true; + + return 0; + } else { + return -1; + } } -__EXPORT bool map_projection_reference(double *ref_lat, double *ref_lon) +__EXPORT int map_projection_reference(double *ref_lat, double *ref_lon) { if (!map_projection_initialized()) { - return false; + return -1; } *ref_lat = mp_ref.lat; *ref_lon = mp_ref.lon; - return true; + return 0; } -__EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) +__EXPORT int map_projection_project(double lat, double lon, float *x, float *y) { if (!map_projection_initialized()) { - return false; + return -1; } double lat_rad = lat / 180.0 * M_PI; @@ -110,13 +117,13 @@ __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) *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; + return 0; } -__EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon) +__EXPORT int map_projection_reproject(float x, float y, double *lat, double *lon) { if (!map_projection_initialized()) { - return false; + return -1; } float x_rad = x / CONSTANTS_RADIUS_OF_EARTH; @@ -140,7 +147,7 @@ __EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lo *lat = lat_rad * 180.0 / M_PI; *lon = lon_rad * 180.0 / M_PI; - return true; + return 0; } diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index b5e6596855..b0f8d6bc40 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -77,7 +77,7 @@ struct map_projection_reference_s { /** * Initializes the map transformation. - * @return true if map_projection_init was called before, false else + * @return true if map was initialized before, false else */ __EXPORT bool map_projection_initialized(); @@ -90,9 +90,9 @@ __EXPORT uint64_t map_projection_timestamp(); /** * Writes the reference values to ref_lat and ref_lon - * @return true if map_projection_init was called before, false else + * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT bool map_projection_reference(double *ref_lat, double *ref_lon); +__EXPORT int map_projection_reference(double *ref_lat, double *ref_lon); /** * Initializes the map transformation. @@ -101,7 +101,7 @@ __EXPORT bool map_projection_reference(double *ref_lat, double *ref_lon); * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT void map_projection_init(double lat_0, double lon_0, uint64_t timestamp); +__EXPORT int map_projection_init(double lat_0, double lon_0, uint64_t timestamp); /** * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane @@ -109,9 +109,9 @@ __EXPORT void map_projection_init(double lat_0, double lon_0, uint64_t timestamp * @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 + * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT bool map_projection_project(double lat, double lon, float *x, float *y); +__EXPORT int 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 @@ -120,9 +120,9 @@ __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) * @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 + * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon); +__EXPORT int map_projection_reproject(float x, float y, double *lat, double *lon); /** * Returns the distance to the next waypoint in meters. From f7e18eaa5809580878b377dfa78ce0ece96d55a7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 15:08:06 +0200 Subject: [PATCH 17/41] uorb home position topic: add valid flag --- src/modules/uORB/topics/home_position.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 08d11abaea..73c8dc50d1 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 */ }; /** From 06dd10cb018d48f60abd1a5e9f83fbdc67867d8c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 15:08:38 +0200 Subject: [PATCH 18/41] commander: set home position valid flag --- src/modules/commander/commander.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1827f252cc..23fe647695 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); From 35b0c294568ca916b344e6a9e6a8e27cefb6f8ca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 15:09:11 +0200 Subject: [PATCH 19/41] navigator: set reference for map projection only once --- src/modules/navigator/navigator_main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e430284156..2f6bf49f3a 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; From 3ec818ce1ed764aeb5f3e8c7371b53b87c2498c3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 14:39:36 +0200 Subject: [PATCH 20/41] rerwite projection interrface to not break the current implementation --- src/lib/geo/geo.c | 98 +++++++++++++++++++++++++++++++---------------- src/lib/geo/geo.h | 81 ++++++++++++++++++++++++++++++++------- 2 files changed, 132 insertions(+), 47 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 6cbf74e466..53e601fadb 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -57,50 +57,77 @@ static struct map_projection_reference_s mp_ref = {0}; -__EXPORT bool map_projection_initialized() +__EXPORT bool map_projection_global_initialized() { - return mp_ref.init_done; + return map_projection_initialized(&mp_ref); } -__EXPORT uint64_t map_projection_timestamp() +__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref) { - return mp_ref.timestamp; + return ref->init_done; } -__EXPORT int map_projection_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 +__EXPORT uint64_t map_projection_global_timestamp() +{ + return map_projection_timestamp(&mp_ref); +} + +__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref) +{ + return ref->timestamp; +} + +__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)) { - 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); - - mp_ref.timestamp = timestamp; - mp_ref.init_done = true; - - return 0; + return map_projection_init(&mp_ref, lat_0, lon_0, timestamp); } else { return -1; } } -__EXPORT int map_projection_reference(double *ref_lat, double *ref_lon) +__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 { - if (!map_projection_initialized()) { - return -1; - } - *ref_lat = mp_ref.lat; - *ref_lon = mp_ref.lon; + ref->lat = lat_0 / 180.0 * M_PI; + ref->lon = lon_0 / 180.0 * M_PI; + + ref->sin_lat = sin(ref->lat); + ref->cos_lat = cos(ref->lat); + + ref->timestamp = timestamp; + ref->init_done = true; return 0; } -__EXPORT int map_projection_project(double lat, double lon, float *x, float *y) +__EXPORT int map_projection_global_reference(double *ref_lat, double *ref_lon) { - if (!map_projection_initialized()) { + return map_projection_reference_s(&mp_ref, ref_lat, ref_lon); +} + +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat, double *ref_lon) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + *ref_lat = ref->lat; + *ref_lon = ref->lon; + + return 0; +} + +__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y) +{ + return map_projection_project(&mp_ref, lat, lon, x, y); + +} + +__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; } @@ -109,20 +136,25 @@ __EXPORT int map_projection_project(double lat, double lon, float *x, float *y) double sin_lat = sin(lat_rad); double cos_lat = cos(lat_rad); - double cos_d_lon = cos(lon_rad - mp_ref.lon); + double cos_d_lon = cos(lon_rad - ref->lon); - double c = acos(mp_ref.sin_lat * sin_lat + mp_ref.cos_lat * cos_lat * cos_d_lon); + double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon); double k = (c == 0.0) ? 1.0 : (c / sin(c)); - *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; + *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; return 0; } -__EXPORT int map_projection_reproject(float x, float y, double *lat, double *lon) +__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon) { - if (!map_projection_initialized()) { + map_projection_project(&mp_ref, x, y, lat, lon); +} + +__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; } @@ -136,12 +168,12 @@ __EXPORT int map_projection_reproject(float x, float y, double *lat, double *lon double lon_rad; if (c != 0.0) { - 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)); + 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)); } else { - lat_rad = mp_ref.lat; - lon_rad = mp_ref.lon; + lat_rad = ref->lat; + lon_rad = ref->lon; } *lat = lat_rad * 180.0 / M_PI; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index b0f8d6bc40..990f791948 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -76,45 +76,86 @@ struct map_projection_reference_s { }; /** - * Initializes the map transformation. + * Checks if global projection was initialized * @return true if map was initialized before, false else */ -__EXPORT bool map_projection_initialized(); - +__EXPORT bool map_projection_global_initialized(); /** - * Initializes the map transformation. + * Checks if projection given as argument was initialized + * @return true if map was initialized before, false else + */ +__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref); + +/** + * Get the timestamp of the global map projection * @return the timestamp of the map_projection */ -__EXPORT uint64_t map_projection_timestamp(); +__EXPORT uint64_t map_projection_global_timestamp(); /** - * Writes the reference values to ref_lat and ref_lon + * Get the timestamp of the map projection given by the argument + * @return the timestamp of the map_projection + */ +__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref); + +/** + * Writes the reference values of the global projection to ref_lat and ref_lon * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_reference(double *ref_lat, double *ref_lon); +__EXPORT int map_projection_global_reference(double *ref_lat, double *ref_lon); /** - * Initializes the map transformation. + * Writes the reference values of the projection given by the argument to ref_lat and ref_lon + * @return 0 if map_projection_init was called before, -1 else + */ +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat, double *ref_lon); + +/** + * Initializes the global map transformation. * - * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane + * 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(double lat_0, double lon_0, uint64_t timestamp); +__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp); /** - * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane + * Initializes the map transformation given by the argument. + * + * 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, uint64_t timestamp); + +/** + * Transforms a point in the geographic coordinate system to the local + * azimuthal equidistant plane using the global projection * @param x north * @param y east * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_project(double lat, double lon, float *x, float *y); +__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y); + + + /* Transforms a point in the geographic coordinate system to the local + * azimuthal equidistant plane using the projection given by the argument + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + * @return 0 if map_projection_init was called before, -1 else + */ +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y); /** - * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system + * Transforms a point in the local azimuthal equidistant plane to the + * geographic coordinate system using the global projection * * @param x north * @param y east @@ -122,7 +163,19 @@ __EXPORT int map_projection_project(double lat, double lon, float *x, float *y); * @param lon in degrees (8.1234567°, not 81234567°) * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_reproject(float x, float y, double *lat, double *lon); +__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon); + +/** + * Transforms a point in the local azimuthal equidistant plane to the + * geographic coordinate system using the projection given by the argument + * + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + * @return 0 if map_projection_init was called before, -1 else + */ +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); /** * Returns the distance to the next waypoint in meters. From 4f84cdc8b871b6c8da7f05a58e96f97b65f290c9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 14:40:01 +0200 Subject: [PATCH 21/41] fw_att_pos_estimator: use new global map projection --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 45ca13a7fb..79328d5161 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; From a7289a32664e3118bf553d23cfd91ed5c86caa84 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:39:26 +0200 Subject: [PATCH 22/41] 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(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 53e601fadb..438e354dff 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 990f791948..3d7ba050f9 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 b2c5e0079a..db8f00e8f7 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; From 5a868751b58ebb6b169beafd6258cd8874440483 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:40:04 +0200 Subject: [PATCH 23/41] navigator: update mapprojection usage --- src/modules/navigator/navigator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 2f6bf49f3a..14c568c46d 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 */ From 53d23c67d72a6395cef509817df25264f05fbd85 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:40:34 +0200 Subject: [PATCH 24/41] 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(-) 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 bf7b6a19cd..6b797f222b 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); From 510678bdae82a04151b6d5dcd5b02bee6f96abfd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:40:54 +0200 Subject: [PATCH 25/41] pos estimator inav: revert to local map projection --- .../position_estimator_inav_main.c | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) 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 91642e5b92..368424853c 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; From 606f3cba5c932d6aba0e383e915b02b78009fd9f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:41:12 +0200 Subject: [PATCH 26/41] pos estimator mc: revert to local map projection --- .../position_estimator_mc/position_estimator_mc_main.c | 5 +++++ 1 file changed, 5 insertions(+) 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 ee6457aded..3639618193 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 */ From 60ccbaa8bb59f4d510b74f7599ef60ef45837180 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 May 2014 15:37:53 +0200 Subject: [PATCH 27/41] 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(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2db30b0664..54834cf9bb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -87,6 +87,7 @@ #include #include #include +#include #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 14c568c46d..12fd35a0a9 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; From 67e3c808d24773c7c4bd4608866de1edb617cb04 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 May 2014 15:40:02 +0200 Subject: [PATCH 28/41] remove home position valid flag --- src/modules/commander/commander.cpp | 4 ---- src/modules/uORB/topics/home_position.h | 1 - 2 files changed, 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 54834cf9bb..bc3bdb7822 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 73c8dc50d1..08d11abaea 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 */ }; /** From 6e0690fde1073649a4ef201671ca947cb83edc37 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 May 2014 16:42:52 +0200 Subject: [PATCH 29/41] 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(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 438e354dff..10a518908d 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 bc3bdb7822..f8f215cd7f 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); From 7ab428976e703284037eb2f24f68ba105db9d6b8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 May 2014 16:49:21 +0200 Subject: [PATCH 30/41] fix wrong variable name --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f8f215cd7f..c15651ed54 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()); From f24a6187b60f5b9890d9645881b3642be420f5d6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 11:16:15 +0200 Subject: [PATCH 31/41] geo: map projection: fix variable name to highlight the unit --- src/lib/geo/geo.c | 30 ++++++++++++++---------------- src/lib/geo/geo.h | 8 ++++---- 2 files changed, 18 insertions(+), 20 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 10a518908d..ff07014aa6 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -80,7 +80,6 @@ __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("commander", getprogname() == 0)) { - return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); } else { return -1; @@ -90,11 +89,10 @@ __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t tim __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; - ref->lon = lon_0 / 180.0 * M_PI; - - ref->sin_lat = sin(ref->lat); - ref->cos_lat = cos(ref->lat); + 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; @@ -107,19 +105,19 @@ __EXPORT int map_projection_init(struct map_projection_reference_s *ref, double map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time()); } -__EXPORT int map_projection_global_reference(double *ref_lat, double *ref_lon) +__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad) { - return map_projection_reference(&mp_ref, ref_lat, ref_lon); + return map_projection_reference(&mp_ref, ref_lat_rad, ref_lon_rad); } -__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat, double *ref_lon) +__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 = ref->lat; - *ref_lon = ref->lon; + *ref_lat_rad = ref->lat_rad; + *ref_lon_rad = ref->lon_rad; return 0; } @@ -141,13 +139,13 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref 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 - ref->lon_rad); double c = acos(ref->sin_lat * sin_lat + 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; + *y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH; return 0; } @@ -174,11 +172,11 @@ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *r 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)); + 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; - lon_rad = ref->lon; + lat_rad = ref->lat_rad; + lon_rad = ref->lon_rad; } *lat = lat_rad * 180.0 / M_PI; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 1d0cd524a7..20fb34ca35 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -69,8 +69,8 @@ struct crosstrack_error_s { /* lat/lon are in radians */ struct map_projection_reference_s { - double lat; - double lon; + double lat_rad; + double lon_rad; double sin_lat; double cos_lat; bool init_done; @@ -105,13 +105,13 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference * Writes the reference values of the global projection to ref_lat and ref_lon * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_global_reference(double *ref_lat, double *ref_lon); +__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad); /** * Writes the reference values of the projection given by the argument to ref_lat and ref_lon * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat, double *ref_lon); +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad); /** * Initializes the global map transformation. From 596b06ff2e13b170545b4f13da1c64e66088aedc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 11:22:18 +0200 Subject: [PATCH 32/41] 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(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c15651ed54..71de33bccc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -58,6 +58,7 @@ #include #include #include +#include #include #include @@ -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()); From fc204a18902b5d623fff1e541a3212502295ed82 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 13:13:35 +0200 Subject: [PATCH 33/41] geo: map projection: fix stupid typo and use constants for deg to rad conversion --- src/lib/geo/geo.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index ff07014aa6..220ea149d3 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -134,8 +134,8 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref return -1; } - double lat_rad = lat / 180.0 * M_PI; - double lon_rad = lon / 180.0 * M_PI; + 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); @@ -152,7 +152,7 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref __EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon) { - map_projection_project(&mp_ref, x, y, lat, lon); + map_projection_reproject(&mp_ref, x, y, lat, lon); } __EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) From 548c7f4aaf93bddeb05053cd4dede945fede22ef Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 14:43:23 +0200 Subject: [PATCH 34/41] 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(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 220ea149d3..c9d48491ee 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 20fb34ca35..98dfbd14b9 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 71de33bccc..98887a0e44 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 */ From a6a2efb651d875171aaec698a836f26b548ca0d6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 16:59:09 +0200 Subject: [PATCH 35/41] 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(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1cfbde1d29..7c93c1c009 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; From e0042ec12cdccd157d181d5b9410fbeedfe3ce72 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 22 May 2014 13:15:29 +0200 Subject: [PATCH 36/41] geo: add functions to get global projection/transformation reference values --- src/lib/geo/geo.c | 39 +++++++++++++++++++++++++++++++++++++++ src/lib/geo/geo.h | 10 ++++++++++ 2 files changed, 49 insertions(+) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index c9d48491ee..b1e0383233 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -186,6 +186,23 @@ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *r return 0; } +__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0) +{ + if (!map_projection_global_initialized()) { + return -1; + } + + if (lat_0 != NULL) { + *lat_0 = M_RAD_TO_DEG * mp_ref.lat_rad; + } + + if (lon_0 != NULL) { + *lon_0 = M_RAD_TO_DEG * mp_ref.lon_rad; + } + + return 0; + +} __EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp) { if (strcmp("commander", getprogname() == 0)) { @@ -210,6 +227,8 @@ __EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, flo map_projection_global_project(lat, lon, x, y); *z = gl_ref.alt - alt; + + return 0; } __EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt) @@ -220,6 +239,26 @@ __EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *l map_projection_global_reproject(x, y, lat, lon); *alt = gl_ref.alt - z; + + return 0; +} + +__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0) +{ + if (!map_projection_global_initialized()) { + return -1; + } + + if (map_projection_global_getref(lat_0, lon_0)) + { + return -1; + } + + if (alt_0 != NULL) { + *alt_0 = gl_ref.alt; + } + + return 0; } __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 98dfbd14b9..4bc3cc17c3 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -195,6 +195,11 @@ __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); +/** + * Get reference position of the global map projection + */ +__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0); + /** * Initialize the global mapping between global position (spherical) and local position (NED). */ @@ -216,6 +221,11 @@ __EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, flo */ __EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt); +/** + * Get reference position of the global to local converter + */ +__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0); + /** * Returns the distance to the next waypoint in meters. * From a43e963bdb73e6fa0480d442e72e1d764418cad4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 30 May 2014 16:09:05 +0200 Subject: [PATCH 37/41] geo: fix logic of globallocal converter initialization to depend on map projection initialization --- src/lib/geo/geo.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index b1e0383233..235da5b394 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -207,8 +207,14 @@ __EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, { 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); + if (!map_projection_global_init(lat_0, lon_0, timestamp)) + { + gl_ref.init_done = true; + return 0; + } else { + gl_ref.init_done = false; + return -1; + } } else { return -1; } From 34c13df3dd6edb68921b9cdc34f2d87ac4251e12 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 09:50:48 +0200 Subject: [PATCH 38/41] geo: Fix a series of warnings, some which actually pointed to real issues --- src/lib/geo/geo.c | 23 +++++++++++------------ src/lib/geo/geo.h | 6 +++--- 2 files changed, 14 insertions(+), 15 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index e03ec65c4d..36be32158f 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -49,14 +49,18 @@ #include #include #include +#include + +#include +#include /* * Azimuthal Equidistant Projection * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ -static struct map_projection_reference_s mp_ref = {0}; -static struct globallocal_converter_reference_s gl_ref = {0}; +static struct map_projection_reference_s mp_ref = {0.0, 0.0, 0.0, 0.0, false, 0}; +static struct globallocal_converter_reference_s gl_ref = {0.0f, false}; __EXPORT bool map_projection_global_initialized() { @@ -80,7 +84,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("commander", getprogname() == 0)) { + if (strcmp("commander", getprogname()) == 0) { return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); } else { return -1; @@ -103,7 +107,7 @@ __EXPORT int map_projection_init_timestamped(struct map_projection_reference_s * __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()); + return map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time()); } __EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad) @@ -153,22 +157,17 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref __EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon) { - map_projection_reproject(&mp_ref, x, y, lat, lon); + return map_projection_reproject(&mp_ref, x, y, lat, lon); } __EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) { -<<<<<<< HEAD if (!map_projection_initialized(ref)) { return -1; } - float x_rad = x / CONSTANTS_RADIUS_OF_EARTH; - float y_rad = y / CONSTANTS_RADIUS_OF_EARTH; -======= double x_rad = x / CONSTANTS_RADIUS_OF_EARTH; double y_rad = y / CONSTANTS_RADIUS_OF_EARTH; ->>>>>>> 48f4a1e5cd6ef653b466eb68c1073fb47cbefbd7 double c = sqrtf(x_rad * x_rad + y_rad * y_rad); double sin_c = sin(c); double cos_c = cos(c); @@ -210,7 +209,7 @@ __EXPORT int map_projection_global_getref(double *lat_0, double *lon_0) } __EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp) { - if (strcmp("commander", getprogname() == 0)) { + if (strcmp("commander", getprogname()) == 0) { gl_ref.alt = alt_0; if (!map_projection_global_init(lat_0, lon_0, timestamp)) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index fbcfe1c2dc..2311e0a7c9 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -86,7 +86,7 @@ struct globallocal_converter_reference_s { * Checks if global projection was initialized * @return true if map was initialized before, false else */ -__EXPORT bool map_projection_global_initialized(); +__EXPORT bool map_projection_global_initialized(void); /** * Checks if projection given as argument was initialized @@ -98,7 +98,7 @@ __EXPORT bool map_projection_initialized(const struct map_projection_reference_s * Get the timestamp of the global map projection * @return the timestamp of the map_projection */ -__EXPORT uint64_t map_projection_global_timestamp(); +__EXPORT uint64_t map_projection_global_timestamp(void); /** * Get the timestamp of the map projection given by the argument @@ -209,7 +209,7 @@ __EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, * Checks if globallocalconverter was initialized * @return true if map was initialized before, false else */ -__EXPORT bool globallocalconverter_initialized(); +__EXPORT bool globallocalconverter_initialized(void); /** * Convert from global position coordinates to local position coordinates using the global reference From 801d1d31983d404d5ebe8f5750359f2d8c7fdf43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 09:51:35 +0200 Subject: [PATCH 39/41] commander: Update after merge --- src/modules/commander/commander.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 326a3e1f35..d7425cbb47 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()); From 89986cf3e5cc8a9e18d56fe971d3e089232e3beb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 11 Aug 2014 13:46:13 +0200 Subject: [PATCH 40/41] epv/eph threshold variable names changed --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 02571f56fb..6c2c030702 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()); From 5baf9cea0d8fcf6e0c6b3c92b26167067e9cb0fa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 11 Aug 2014 15:29:59 +0200 Subject: [PATCH 41/41] geo: fix some warnings --- src/lib/geo/geo.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index ad1ff37e30..1c8d2a2a75 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -50,6 +50,7 @@ #include #include #include +#include #include #include @@ -147,7 +148,7 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref 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 = (c == 0.0) ? 1.0 : (c / sin(c)); + 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; @@ -175,7 +176,7 @@ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *r double lat_rad; double lon_rad; - if (c != 0.0) { + 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));