From 3a898e54adbaac7fda28e0b9079076d9cdf8028c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 14:30:29 +0200 Subject: 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 | 44 ++++++++++++++-------- src/lib/geo/geo.h | 9 +++-- .../fw_att_pos_estimator_main.cpp | 6 +-- src/modules/mavlink/mavlink_receiver.cpp | 4 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 ++--- .../position_estimator_inav_main.c | 20 +++------- 6 files changed, 48 insertions(+), 44 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 9a24ff50e..32d52aadc 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -55,33 +55,47 @@ * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ -__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +struct map_projection_reference_s mp_ref; + +__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - ref->lat = lat_0 / 180.0 * M_PI; - ref->lon = lon_0 / 180.0 * M_PI; + mp_ref.lat = lat_0 / 180.0 * M_PI; + mp_ref.lon = lon_0 / 180.0 * M_PI; + + mp_ref.sin_lat = sin(mp_ref.lat); + mp_ref.cos_lat = cos(mp_ref.lat); - ref->sin_lat = sin(ref->lat); - ref->cos_lat = cos(ref->lat); + mp_ref.init_done = true; } -__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +__EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) { + if (!mp_ref.init_done) { + return false; + } + double lat_rad = lat / 180.0 * M_PI; double lon_rad = lon / 180.0 * M_PI; double sin_lat = sin(lat_rad); double cos_lat = cos(lat_rad); - double cos_d_lon = cos(lon_rad - ref->lon); + double cos_d_lon = cos(lon_rad - mp_ref.lon); - double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon); + double c = acos(mp_ref.sin_lat * sin_lat + mp_ref.cos_lat * cos_lat * cos_d_lon); double k = (c == 0.0) ? 1.0 : (c / sin(c)); - *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; - *y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH; + *x = k * (mp_ref.cos_lat * sin_lat - mp_ref.sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; + *y = k * cos_lat * sin(lon_rad - mp_ref.lon) * CONSTANTS_RADIUS_OF_EARTH; + + return true; } -__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +__EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon) { + if (!mp_ref.init_done) { + return false; + } + float x_rad = x / CONSTANTS_RADIUS_OF_EARTH; float y_rad = y / CONSTANTS_RADIUS_OF_EARTH; double c = sqrtf(x_rad * x_rad + y_rad * y_rad); @@ -92,12 +106,12 @@ __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, f double lon_rad; if (c != 0.0) { - lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c); - lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); + lat_rad = asin(cos_c * mp_ref.sin_lat + (x_rad * sin_c * mp_ref.cos_lat) / c); + lon_rad = (mp_ref.lon + atan2(y_rad * sin_c, c * mp_ref.cos_lat * cos_c - x_rad * mp_ref.sin_lat * sin_c)); } else { - lat_rad = ref->lat; - lon_rad = ref->lon; + lat_rad = mp_ref.lat; + lon_rad = mp_ref.lon; } *lat = lat_rad * 180.0 / M_PI; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 0a3f85d97..b48d26a66 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -71,6 +71,7 @@ struct map_projection_reference_s { double lon; double sin_lat; double cos_lat; + bool init_done; }; /** @@ -80,7 +81,7 @@ struct map_projection_reference_s { * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0); +__EXPORT void map_projection_init(double lat_0, double lon_0); /** * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane @@ -88,8 +89,9 @@ __EXPORT void map_projection_init(struct map_projection_reference_s *ref, double * @param y east * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) + * @return true if map_projection_init was called before, false else */ -__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y); +__EXPORT bool map_projection_project(double lat, double lon, float *x, float *y); /** * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system @@ -98,8 +100,9 @@ __EXPORT void map_projection_project(struct map_projection_reference_s *ref, dou * @param y east * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) + * @return true if map_projection_init was called before, false else */ -__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); +__EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon); /** * Returns the distance to the next waypoint in meters. diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 9d2d9ed5f..420a03644 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -177,8 +177,6 @@ private: struct sensor_combined_s _sensor_combined; #endif - struct map_projection_reference_s _pos_ref; - float _baro_ref; /**< barometer reference altitude */ float _baro_gps_offset; /**< offset between GPS and baro */ @@ -822,8 +820,6 @@ FixedwingEstimator::task_main() _ekf->baroHgt = _baro.altitude - _baro_ref; _baro_gps_offset = _baro_ref - _local_pos.ref_alt; - // XXX this is not multithreading safe - map_projection_init(&_pos_ref, lat, lon); mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); _gps_initialized = true; @@ -1046,7 +1042,7 @@ FixedwingEstimator::task_main() if (_local_pos.xy_global) { double est_lat, est_lon; - map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); + map_projection_reproject(_local_pos.x, _local_pos.y, &est_lat, &est_lon); _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_gps_usec = _gps.time_gps_usec; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index adafdd139..df61c3c15 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -795,7 +795,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; - map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); +// map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); //XXX fix reference update hil_local_pos.ref_timestamp = timestamp; hil_local_pos.ref_lat = lat; hil_local_pos.ref_lon = lon; @@ -804,7 +804,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) float x; float y; - map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y); + map_projection_project(lat, lon, &x, &y); hil_local_pos.timestamp = timestamp; hil_local_pos.xy_valid = true; hil_local_pos.z_valid = true; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index dc0aa172a..963349d42 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -478,17 +478,17 @@ MulticopterPositionControl::update_ref() if (_ref_timestamp != 0) { /* calculate current position setpoint in global frame */ - map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); + map_projection_reproject(_pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); alt_sp = _ref_alt - _pos_sp(2); } /* update local projection reference */ - map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); +// map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); //XXX fix reference update _ref_alt = _local_pos.ref_alt; if (_ref_timestamp != 0) { /* reproject position setpoint to new reference */ - map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); + map_projection_project(lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); _pos_sp(2) = -(alt_sp - _ref_alt); } @@ -687,8 +687,7 @@ MulticopterPositionControl::task_main() _reset_alt_sp = true; /* project setpoint to local frame */ - map_projection_project(&_ref_pos, - _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, + map_projection_project(_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, &_pos_sp.data[0], &_pos_sp.data[1]); _pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 368424853..b803dfb58 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -558,12 +558,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ref_inited) { /* calculate current estimated position in global frame */ est_alt = local_pos.ref_alt - local_pos.z; - map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); + map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); } - /* update reference */ - map_projection_init(&ref, home.lat, home.lon); - /* update baro offset */ baro_offset += home.alt - local_pos.ref_alt; @@ -572,9 +569,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.ref_alt = home.alt; local_pos.ref_timestamp = home.timestamp; - if (ref_inited) { + if (ref_inited) { //XXX fix reference update /* reproject position estimate with new reference */ - map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]); + map_projection_project(est_lat, est_lon, &x_est[0], &y_est[0]); z_est[0] = -(est_alt - local_pos.ref_alt); } @@ -633,18 +630,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.ref_lon = lon; local_pos.ref_alt = alt; local_pos.ref_timestamp = t; - - /* initialize projection */ - map_projection_init(&ref, lat, lon); - warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); } } - if (ref_inited) { + if (ref_inited) { //XXX fix reference update /* project GPS lat lon to plane */ float gps_proj[2]; - map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); + map_projection_project(lat, lon, &(gps_proj[0]), &(gps_proj[1])); /* reset position estimate when GPS becomes good */ if (reset_est) { @@ -938,7 +930,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.time_gps_usec = gps.time_gps_usec; double est_lat, est_lon; - map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); + map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); global_pos.lat = est_lat; global_pos.lon = est_lon; -- cgit v1.2.3 From 6517c201f701da97acde1d7e3cc72712b9a1613c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 15:59:13 +0200 Subject: 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 32d52aadc..60994940f 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; } -- cgit v1.2.3 From fbcb248bc3a91d1c26f6f47a2a46badd2d8a58f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 15:59:49 +0200 Subject: 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 d4d23396a..b7cf49cfa 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); } -- cgit v1.2.3 From 8bb1b39399eb09ddf93c2682713d80be4f776c5d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 16:12:28 +0200 Subject: 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 df61c3c15..40121efe4 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; -- cgit v1.2.3 From 35d15720ef79dae4d6f6202092d4869a455fc7d2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:04:27 +0200 Subject: 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 60994940f..dbbf1f80c 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 b48d26a66..b19bfe462 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. * -- cgit v1.2.3 From 9f2da53f08f5d186d95653bad9afa04e8c7276d2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:05:30 +0200 Subject: 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 963349d42..fa3438ea8 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) { -- cgit v1.2.3 From 618ac319e63a6597cc62df9c810d76cdc094012b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:06:10 +0200 Subject: pos estimator inav: check if map projection is initialized --- .../position_estimator_inav/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 b803dfb58..d01bf488f 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])); -- cgit v1.2.3 From b249d04e52ae34d43d9d80638d6f1ead476f39ea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:13:06 +0200 Subject: 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 dbbf1f80c..1e3ef56ae 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 b19bfe462..913e83b6e 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. * -- cgit v1.2.3 From 4e30784d6c506cdfdf58c623da20cffcb799e496 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:16:20 +0200 Subject: rename map_projection_inited to map_projection_initialized --- src/lib/geo/geo.c | 6 +++--- src/lib/geo/geo.h | 2 +- src/modules/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 1e3ef56ae..725a04a29 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 913e83b6e..66e4dc75e 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 d01bf488f..e88493a5e 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; -- cgit v1.2.3 From 9ebd6b348687d5563b87c5c872d432adbfad696f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 09:41:46 +0200 Subject: 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 725a04a29..a2117ecd0 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 66e4dc75e..b5e659685 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 -- cgit v1.2.3 From f5d9023b1060f10c14d016492c88375c6fdc22aa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 09:42:28 +0200 Subject: 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 b7cf49cfa..228324789 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); } -- cgit v1.2.3 From 7e1ea35571149a9d766cc7122738f206f1ed3427 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 09:43:19 +0200 Subject: 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 40121efe4..e9b58bccf 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; -- cgit v1.2.3 From 5285eb2f997b590071186a8f695af4375f96e779 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 09:44:40 +0200 Subject: fw att pos estimator: use map projection reference values for local pos --- src/modules/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 420a03644..45ca13a7f 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); -- cgit v1.2.3 From 08bc777208a8510da7cd91fab5059ffa8bd14150 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 13:57:06 +0200 Subject: pos estimator inav: use map_projection_reference to set local pos reference lat lon --- src/modules/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 e88493a5e..91642e5b9 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; } -- cgit v1.2.3 From 241a99fc28992694f807595ce6bb48f460c9a28a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 14:45:21 +0200 Subject: position estimator mc: do not initialize map projection because this is now handled globally --- src/modules/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 363961819..ee6457ade 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 */ -- cgit v1.2.3 From aab64af8844e03b04f5616c1beecf18c9f6a7d81 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 14:47:37 +0200 Subject: 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 a2117ecd0..6cbf74e46 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 b5e659685..b0f8d6bc4 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. -- cgit v1.2.3 From f7e18eaa5809580878b377dfa78ce0ece96d55a7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 15:08:06 +0200 Subject: 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 08d11abae..73c8dc50d 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 */ }; /** -- cgit v1.2.3 From 06dd10cb018d48f60abd1a5e9f83fbdc67867d8c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 15:08:38 +0200 Subject: 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 1827f252c..23fe64769 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); -- cgit v1.2.3 From 35b0c294568ca916b344e6a9e6a8e27cefb6f8ca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 15:09:11 +0200 Subject: 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 e43028415..2f6bf49f3 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; -- cgit v1.2.3 From 3ec818ce1ed764aeb5f3e8c7371b53b87c2498c3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 14:39:36 +0200 Subject: rerwite projection interrface to not break the current implementation --- src/lib/geo/geo.c | 92 +++++++++++++++++++++++++++++++++++++------------------ src/lib/geo/geo.h | 79 +++++++++++++++++++++++++++++++++++++++-------- 2 files changed, 128 insertions(+), 43 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 6cbf74e46..53e601fad 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() { - if (strcmp("navigator", getprogname() == 0)) { - - mp_ref.lat = lat_0 / 180.0 * M_PI; - mp_ref.lon = lon_0 / 180.0 * M_PI; + return map_projection_timestamp(&mp_ref); +} - mp_ref.sin_lat = sin(mp_ref.lat); - mp_ref.cos_lat = cos(mp_ref.lat); +__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref) +{ + return ref->timestamp; +} - mp_ref.timestamp = timestamp; - mp_ref.init_done = true; +__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)) { - 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()) { + + 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_global_reference(double *ref_lat, double *ref_lon) +{ + 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 = mp_ref.lat; - *ref_lon = mp_ref.lon; + *ref_lat = ref->lat; + *ref_lon = ref->lon; return 0; } -__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) +{ + 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()) { + 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) +{ + 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()) { + 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 b0f8d6bc4..990f79194 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -76,45 +76,98 @@ 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(); +/** + * 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); /** - * Initializes the map transformation. + * 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); + +/** + * 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 + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp); /** - * Initializes the map transformation. + * Initializes the map transformation given by the argument. * - * 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_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 + * 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_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 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_reproject(float x, float y, double *lat, double *lon); /** - * 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 projection given by the argument * * @param x north * @param y east @@ -122,7 +175,7 @@ __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_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. -- cgit v1.2.3 From 4f84cdc8b871b6c8da7f05a58e96f97b65f290c9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 14:40:01 +0200 Subject: fw_att_pos_estimator: use new global map projection --- src/modules/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 45ca13a7f..79328d516 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; -- cgit v1.2.3 From a7289a32664e3118bf553d23cfd91ed5c86caa84 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:39:26 +0200 Subject: 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 53e601fad..438e354df 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -81,13 +81,13 @@ __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t tim { if (strcmp("navigator", getprogname() == 0)) { - return map_projection_init(&mp_ref, lat_0, lon_0, timestamp); + return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); } else { return -1; } } -__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { ref->lat = lat_0 / 180.0 * M_PI; @@ -102,9 +102,14 @@ __EXPORT int map_projection_init(struct map_projection_reference_s *ref, double return 0; } +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time()); +} + __EXPORT int map_projection_global_reference(double *ref_lat, double *ref_lon) { - return map_projection_reference_s(&mp_ref, ref_lat, ref_lon); + return map_projection_reference(&mp_ref, ref_lat, ref_lon); } __EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat, double *ref_lon) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 990f79194..3d7ba050f 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -129,7 +129,18 @@ __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t tim * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp); +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, + double lat_0, double lon_0, uint64_t timestamp); + +/** + * Initializes the map transformation given by the argument and sets the timestamp to now. + * + * Initializes the transformation between the geographic coordinate system and + * the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0); /** * Transforms a point in the geographic coordinate system to the local diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b2c5e0079..db8f00e8f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -766,7 +766,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; - hil_local_pos.ref_timestamp = map_projection_timestamp(); + hil_local_pos.ref_timestamp = map_projection_global_timestamp(); hil_local_pos.ref_lat = lat; hil_local_pos.ref_lon = lon; hil_local_pos.ref_alt = _hil_local_alt0; @@ -774,7 +774,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) float x; float y; - map_projection_project(lat, lon, &x, &y); + map_projection_global_project(lat, lon, &x, &y); hil_local_pos.timestamp = timestamp; hil_local_pos.xy_valid = true; hil_local_pos.z_valid = true; -- cgit v1.2.3 From 5a868751b58ebb6b169beafd6258cd8874440483 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:40:04 +0200 Subject: 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 2f6bf49f3..14c568c46 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 */ -- cgit v1.2.3 From 53d23c67d72a6395cef509817df25264f05fbd85 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:40:34 +0200 Subject: 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 bf7b6a19c..6b797f222 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); -- cgit v1.2.3 From 510678bdae82a04151b6d5dcd5b02bee6f96abfd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:40:54 +0200 Subject: 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 91642e5b9..368424853 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; -- cgit v1.2.3 From 606f3cba5c932d6aba0e383e915b02b78009fd9f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:41:12 +0200 Subject: pos estimator mc: revert to local map projection --- src/modules/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 ee6457ade..363961819 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 */ -- cgit v1.2.3 From 60ccbaa8bb59f4d510b74f7599ef60ef45837180 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 May 2014 15:37:53 +0200 Subject: 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 2db30b066..54834cf9b 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 14c568c46..12fd35a0a 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; -- cgit v1.2.3 From 67e3c808d24773c7c4bd4608866de1edb617cb04 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 May 2014 15:40:02 +0200 Subject: 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 54834cf9b..bc3bdb782 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 73c8dc50d..08d11abae 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 */ }; /** -- cgit v1.2.3 From 6e0690fde1073649a4ef201671ca947cb83edc37 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 May 2014 16:42:52 +0200 Subject: 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 438e354df..10a518908 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 bc3bdb782..f8f215cd7 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); -- cgit v1.2.3 From 7ab428976e703284037eb2f24f68ba105db9d6b8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 May 2014 16:49:21 +0200 Subject: 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 f8f215cd7..c15651ed5 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()); -- cgit v1.2.3 From f24a6187b60f5b9890d9645881b3642be420f5d6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 11:16:15 +0200 Subject: 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 10a518908..ff07014aa 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 1d0cd524a..20fb34ca3 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. -- cgit v1.2.3 From 596b06ff2e13b170545b4f13da1c64e66088aedc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 11:22:18 +0200 Subject: 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 c15651ed5..71de33bcc 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()); -- cgit v1.2.3 From fc204a18902b5d623fff1e541a3212502295ed82 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 13:13:35 +0200 Subject: 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 ff07014aa..220ea149d 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) -- cgit v1.2.3 From 548c7f4aaf93bddeb05053cd4dede945fede22ef Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 14:43:23 +0200 Subject: 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 220ea149d..c9d48491e 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 20fb34ca3..98dfbd14b 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 71de33bcc..98887a0e4 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 */ -- cgit v1.2.3 From a6a2efb651d875171aaec698a836f26b548ca0d6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 16:59:09 +0200 Subject: 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 1cfbde1d2..7c93c1c00 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; -- cgit v1.2.3 From e0042ec12cdccd157d181d5b9410fbeedfe3ce72 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 22 May 2014 13:15:29 +0200 Subject: 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 c9d48491e..b1e038323 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 98dfbd14b..4bc3cc17c 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. * -- cgit v1.2.3 From a43e963bdb73e6fa0480d442e72e1d764418cad4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 30 May 2014 16:09:05 +0200 Subject: 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 b1e038323..235da5b39 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; } -- cgit v1.2.3 From cb246a79cad96c469f29042dd33658d63cbcc743 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Jun 2014 16:47:16 +0200 Subject: Added vision position estimate topic --- src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/vision_position_estimate.h | 82 ++++++++++++++++++++++ 2 files changed, 85 insertions(+) create mode 100644 src/modules/uORB/topics/vision_position_estimate.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 90675bb2e..fc9920a41 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -199,3 +199,6 @@ ORB_DEFINE(encoders, struct encoders_s); #include "topics/estimator_status.h" ORB_DEFINE(estimator_status, struct estimator_status_report); + +#include "topics/vision_position_estimate.h" +ORB_DEFINE(vision_position_estimate, struct vision_position_estimate); diff --git a/src/modules/uORB/topics/vision_position_estimate.h b/src/modules/uORB/topics/vision_position_estimate.h new file mode 100644 index 000000000..74c96cf2e --- /dev/null +++ b/src/modules/uORB/topics/vision_position_estimate.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vision_position_estimate.h + * Vision based position estimate + */ + +#ifndef TOPIC_VISION_POSITION_ESTIMATE_H_ +#define TOPIC_VISION_POSITION_ESTIMATE_H_ + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Vision based position estimate in NED frame + */ +struct vision_position_estimate { + + unsigned id; /**< ID of the estimator, commonly the component ID of the incoming message */ + + uint64_t timestamp_boot; /**< time of this estimate, in microseconds since system start */ + uint64_t timestamp_computer; /**< timestamp provided by the companion computer, in us */ + + float x; /**< X position in meters in NED earth-fixed frame */ + float y; /**< Y position in meters in NED earth-fixed frame */ + float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */ + + float vx; /**< X velocity in meters per second in NED earth-fixed frame */ + float vy; /**< Y velocity in meters per second in NED earth-fixed frame */ + float vz; /**< Z velocity in meters per second in NED earth-fixed frame */ + + float q[4]; /**< Estimated attitude as quaternion */ + + // XXX Add covariances here + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vision_position_estimate); + +#endif /* TOPIC_VISION_POSITION_ESTIMATE_H_ */ -- cgit v1.2.3 From 7a776eacb11f30a007812449a02916c6f00d2ad0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Jun 2014 16:50:09 +0200 Subject: MAVLink app: Publish vision position estimate --- src/modules/mavlink/mavlink_receiver.cpp | 44 ++++++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 3 +++ 2 files changed, 47 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 53769e0cf..6b73a4bfa 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -103,6 +103,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _flow_pub(-1), _offboard_control_sp_pub(-1), _vicon_position_pub(-1), + _vision_position_pub(-1), _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), @@ -138,6 +139,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_vicon_position_estimate(msg); break; + case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: + handle_message_vision_position_estimate(msg); + break; + case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST: handle_message_quad_swarm_roll_pitch_yaw_thrust(msg); break; @@ -339,6 +344,45 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) +{ + mavlink_vision_position_estimate_t pos; + mavlink_msg_vision_position_estimate_decode(msg, &pos); + + struct vision_position_estimate vision_position; + memset(&vision_position, 0, sizeof(vision_position)); + + // Use the component ID to identify the vision sensor + vision_position.id = msg->compid; + + vision_position.timestamp_boot = hrt_absolute_time(); + vision_position.timestamp_computer = pos.usec; + vision_position.x = pos.x; + vision_position.y = pos.y; + vision_position.z = pos.z; + + // XXX fis this + vision_position.vx = 0.0f; + vision_position.vy = 0.0f; + vision_position.vz = 0.0f; + + math::Quaternion q; + q.from_euler(pos.roll, pos.pitch, pos.yaw); + + vision_position.q[0] = q(0); + vision_position.q[1] = q(1); + vision_position.q[2] = q(2); + vision_position.q[3] = q(3); + + if (_vision_position_pub < 0) { + _vision_position_pub = orb_advertise(ORB_ID(vision_position_estimate), &vision_position); + + } else { + orb_publish(ORB_ID(vision_position_estimate), _vision_position_pub, &vision_position); + } +} + void MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9ab84b58a..b5e6b1b1a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -109,6 +110,7 @@ private: void handle_message_optical_flow(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); + void handle_message_vision_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); @@ -135,6 +137,7 @@ private: orb_advert_t _flow_pub; orb_advert_t _offboard_control_sp_pub; orb_advert_t _vicon_position_pub; + orb_advert_t _vision_position_pub; orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; -- cgit v1.2.3 From 57a7e764068178e365b05b7baaa39041762f2e96 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Jun 2014 16:51:09 +0200 Subject: INAV: Added vision position estimate input / topic --- .../position_estimator_inav_main.c | 62 +++++++++++++++++++++- .../position_estimator_inav_params.c | 3 ++ .../position_estimator_inav_params.h | 4 ++ 3 files changed, 68 insertions(+), 1 deletion(-) 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 d7503e42d..9ab21ee38 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -78,6 +79,7 @@ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; static const hrt_abstime gps_topic_timeout = 1000000; // GPS topic timeout = 1s +static const hrt_abstime vision_topic_timeout = 1000000; // Vision topic timeout = 1s static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss @@ -270,6 +272,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool sonar_valid = false; // sonar is valid bool flow_valid = false; // flow is valid bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) + bool vision_valid = false; /* declare and safely initialize all structs */ struct actuator_controls_s actuator; @@ -288,6 +291,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&local_pos, 0, sizeof(local_pos)); struct optical_flow_s flow; memset(&flow, 0, sizeof(flow)); + struct vision_position_estimate vision; + memset(&vision, 0, sizeof(vision)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); @@ -299,6 +304,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate)); int home_position_sub = orb_subscribe(ORB_ID(home_position)); /* advertise */ @@ -587,6 +593,53 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + + /* check no vision circuit breaker is set */ + if (params.no_vision != CBRK_NO_VISION) { + /* vehicle vision position */ + orb_check(vision_position_estimate_sub, &updated); + + + + // XXX THIS IS EVIL UNTESTED CODE + // set CBRK_NO_VISION to 0 to disengage the circuit breaker + // and activate this section + + + if (updated) { + orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision); + + /* reset position estimate on first vision update */ + if (!vision_valid) { + x_est[0] = vision.x; + x_est[1] = vision.vx; + y_est[0] = vision.y; + y_est[1] = vision.vy; + z_est[0] = vision.z; + z_est[1] = vision.vz; + + vision_valid = true; + } + + /* calculate correction for position */ + corr_gps[0][0] = vision.x - x_est[0]; + corr_gps[1][0] = vision.y - y_est[0]; + corr_gps[2][0] = vision.z - z_est[0]; + + /* calculate correction for velocity */ + corr_gps[0][1] = vision.vx - x_est[1]; + corr_gps[1][1] = vision.vy - y_est[1]; + corr_gps[2][1] = vision.vz - z_est[1]; + + eph = 0.05f; + epv = 0.05f; + + w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m); + w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m); + + } + } + /* vehicle GPS position */ orb_check(vehicle_gps_position_sub, &updated); @@ -706,6 +759,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] GPS timeout"); } + /* check for timeout on vision topic */ + if (vision_valid && t > vision.timestamp_boot + vision_topic_timeout) { + vision_valid = false; + warnx("VISION timeout"); + mavlink_log_info(mavlink_fd, "[inav] VISION timeout"); + } + /* check for sonar measurement timeout */ if (sonar_valid && t > sonar_time + sonar_timeout) { corr_sonar = 0.0f; @@ -731,7 +791,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) xy_src_time = t; } - bool can_estimate_xy = eph < max_eph_epv * 1.5; + bool can_estimate_xy = (eph < max_eph_epv * 1.5) || vision_valid; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 8aa08b6f2..c7c1c070c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -55,6 +55,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); +PARAM_DEFINE_INT32(CBRK_NO_VISION, 328754); int parameters_init(struct position_estimator_inav_param_handles *h) { @@ -73,6 +74,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->land_t = param_find("INAV_LAND_T"); h->land_disp = param_find("INAV_LAND_DISP"); h->land_thr = param_find("INAV_LAND_THR"); + h->no_vision = param_find("CBRK_NO_VISION"); return OK; } @@ -94,6 +96,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->land_t, &(p->land_t)); param_get(h->land_disp, &(p->land_disp)); param_get(h->land_thr, &(p->land_thr)); + param_get(h->no_vision, &(p->no_vision)); return OK; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 08ec996a1..b7826793a 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -56,6 +56,7 @@ struct position_estimator_inav_params { float land_t; float land_disp; float land_thr; + int32_t no_vision; }; struct position_estimator_inav_param_handles { @@ -74,8 +75,11 @@ struct position_estimator_inav_param_handles { param_t land_t; param_t land_disp; param_t land_thr; + param_t no_vision; }; +#define CBRK_NO_VISION 328754 + /** * Initialize all parameter handles and values * -- cgit v1.2.3 From 78b7ba33a5f04a11ac859aecfb72ff34d75cb7ff Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Sun, 22 Jun 2014 09:53:33 +0800 Subject: Fix to allow filter correction with vision position estimate --- src/modules/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 9ab21ee38..a6a4db3eb 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -781,8 +781,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift) /* use GPS if it's valid and reference position initialized */ - bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; - bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; + bool use_gps_xy = (ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W) || vision_valid; + bool use_gps_z = (ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W) || vision_valid; /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); -- cgit v1.2.3 From 34c13df3dd6edb68921b9cdc34f2d87ac4251e12 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 09:50:48 +0200 Subject: 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 e03ec65c4..36be32158 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 fbcfe1c2d..2311e0a7c 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 -- cgit v1.2.3 From 801d1d31983d404d5ebe8f5750359f2d8c7fdf43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 09:51:35 +0200 Subject: 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 326a3e1f3..d7425cbb4 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()); -- cgit v1.2.3 From a063f58e006689bee2e28d85930cba11245b9597 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Tue, 22 Jul 2014 12:36:28 +0800 Subject: Add vision weight parameters to structure --- .../position_estimator_inav/position_estimator_inav_params.c | 6 ++++++ .../position_estimator_inav/position_estimator_inav_params.h | 4 ++++ 2 files changed, 10 insertions(+) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 0d9fbef27..36e556756 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -43,9 +43,11 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); +PARAM_DEFINE_FLOAT(INAV_W_Z_VISION_P, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_VISION_P, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); @@ -64,9 +66,11 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); + h->w_z_vision_p = param_find("INAV_W_Z_VISION_P"); h->w_z_sonar = param_find("INAV_W_Z_SONAR"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); + h->w_xy_vision_p = param_find("INAV_W_XY_VISION_P"); h->w_xy_flow = param_find("INAV_W_XY_FLOW"); h->w_xy_res_v = param_find("INAV_W_XY_RES_V"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); @@ -88,9 +92,11 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str { param_get(h->w_z_baro, &(p->w_z_baro)); param_get(h->w_z_gps_p, &(p->w_z_gps_p)); + param_get(h->w_z_vision_p, &(p->w_z_vision_p)); param_get(h->w_z_sonar, &(p->w_z_sonar)); param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); + param_get(h->w_xy_vision_p, &(p->w_xy_vision_p)); param_get(h->w_xy_flow, &(p->w_xy_flow)); param_get(h->w_xy_res_v, &(p->w_xy_res_v)); param_get(h->w_gps_flow, &(p->w_gps_flow)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 08098d6c3..a64205c1f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -44,9 +44,11 @@ struct position_estimator_inav_params { float w_z_baro; float w_z_gps_p; + float w_z_vision_p; float w_z_sonar; float w_xy_gps_p; float w_xy_gps_v; + float w_xy_vision_p; float w_xy_flow; float w_xy_res_v; float w_gps_flow; @@ -65,9 +67,11 @@ struct position_estimator_inav_params { struct position_estimator_inav_param_handles { param_t w_z_baro; param_t w_z_gps_p; + param_t w_z_vision_p; param_t w_z_sonar; param_t w_xy_gps_p; param_t w_xy_gps_v; + param_t w_xy_vision_p; param_t w_xy_flow; param_t w_xy_res_v; param_t w_gps_flow; -- cgit v1.2.3 From dd0e39972fb5a8e4b881e71d8bdb53e1ce82e380 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Tue, 22 Jul 2014 14:00:49 +0800 Subject: Add weight parameter for vision velocity --- src/modules/position_estimator_inav/position_estimator_inav_params.c | 3 +++ src/modules/position_estimator_inav/position_estimator_inav_params.h | 2 ++ 2 files changed, 5 insertions(+) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 36e556756..1867f65da 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -48,6 +48,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_VISION_P, 5.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_VISION_V, 0.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); @@ -71,6 +72,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); h->w_xy_vision_p = param_find("INAV_W_XY_VISION_P"); + h->w_xy_vision_v = param_find("INAV_W_XY_VISION_V"); h->w_xy_flow = param_find("INAV_W_XY_FLOW"); h->w_xy_res_v = param_find("INAV_W_XY_RES_V"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); @@ -97,6 +99,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); param_get(h->w_xy_vision_p, &(p->w_xy_vision_p)); + param_get(h->w_xy_vision_v, &(p->w_xy_vision_v)); param_get(h->w_xy_flow, &(p->w_xy_flow)); param_get(h->w_xy_res_v, &(p->w_xy_res_v)); param_get(h->w_gps_flow, &(p->w_gps_flow)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index a64205c1f..d0a65e42e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -49,6 +49,7 @@ struct position_estimator_inav_params { float w_xy_gps_p; float w_xy_gps_v; float w_xy_vision_p; + float w_xy_vision_v; float w_xy_flow; float w_xy_res_v; float w_gps_flow; @@ -72,6 +73,7 @@ struct position_estimator_inav_param_handles { param_t w_xy_gps_p; param_t w_xy_gps_v; param_t w_xy_vision_p; + param_t w_xy_vision_v; param_t w_xy_flow; param_t w_xy_res_v; param_t w_gps_flow; -- cgit v1.2.3 From ef74f4ad8920b59a80158ac8820ed497c9dbbe13 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Tue, 22 Jul 2014 20:48:18 +0800 Subject: Shorten vision parameter name --- .../position_estimator_inav/position_estimator_inav_params.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 1867f65da..90d8d2b50 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -43,12 +43,12 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); -PARAM_DEFINE_FLOAT(INAV_W_Z_VISION_P, 0.5f); +PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); -PARAM_DEFINE_FLOAT(INAV_W_XY_VISION_P, 5.0f); -PARAM_DEFINE_FLOAT(INAV_W_XY_VISION_V, 0.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 5.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); @@ -67,12 +67,12 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); - h->w_z_vision_p = param_find("INAV_W_Z_VISION_P"); + h->w_z_vision_p = param_find("INAV_W_Z_VIS_P"); h->w_z_sonar = param_find("INAV_W_Z_SONAR"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); - h->w_xy_vision_p = param_find("INAV_W_XY_VISION_P"); - h->w_xy_vision_v = param_find("INAV_W_XY_VISION_V"); + h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P"); + h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V"); h->w_xy_flow = param_find("INAV_W_XY_FLOW"); h->w_xy_res_v = param_find("INAV_W_XY_RES_V"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); -- cgit v1.2.3 From dfbbc6600e67618e030c366dca5b440dcd0ddcb7 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Tue, 22 Jul 2014 21:01:10 +0800 Subject: Seperate vision position estimate correction logic from gps. Add vision weight parameters. Fixes false positive of valid position indication --- .../position_estimator_inav_main.c | 103 +++++++++++++++++---- 1 file changed, 87 insertions(+), 16 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 71abcc170..398fddd69 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -235,6 +235,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float eph_flow = 1.0f; + float eph_vision = 0.5f; + float epv_vision = 0.5f; + float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev)); @@ -281,6 +284,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) }; float w_gps_xy = 1.0f; float w_gps_z = 1.0f; + + float corr_vision[3][2] = { + { 0.0f, 0.0f }, // N (pos, vel) + { 0.0f, 0.0f }, // E (pos, vel) + { 0.0f, 0.0f }, // D (pos, vel) + }; + float corr_sonar = 0.0f; float corr_sonar_filtered = 0.0f; @@ -637,27 +647,33 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) x_est[1] = vision.vx; y_est[0] = vision.y; y_est[1] = vision.vy; - z_est[0] = vision.z; - z_est[1] = vision.vz; - + /* only reset the z estimate if the z weight parameter is not zero */ + if (params.w_z_vision_p > MIN_VALID_W) + { + z_est[0] = vision.z; + z_est[1] = vision.vz; + } + vision_valid = true; + warnx("VISION estimate valid"); + mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid"); } /* calculate correction for position */ - corr_gps[0][0] = vision.x - x_est[0]; - corr_gps[1][0] = vision.y - y_est[0]; - corr_gps[2][0] = vision.z - z_est[0]; + corr_vision[0][0] = vision.x - x_est[0]; + corr_vision[1][0] = vision.y - y_est[0]; + corr_vision[2][0] = vision.z - z_est[0]; /* calculate correction for velocity */ - corr_gps[0][1] = vision.vx - x_est[1]; - corr_gps[1][1] = vision.vy - y_est[1]; - corr_gps[2][1] = vision.vz - z_est[1]; + corr_vision[0][1] = vision.vx - x_est[1]; + corr_vision[1][1] = vision.vy - y_est[1]; + corr_vision[2][1] = vision.vz - z_est[1]; - eph = 0.05f; - epv = 0.05f; + // eph_flow = 0.05f; + // epv_vision = 0.05f; - w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, eph); - w_gps_z = min_eph_epv / fmaxf(min_eph_epv, epv); + // w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, eph); + // w_gps_z = min_eph_epv / fmaxf(min_eph_epv, epv); } } @@ -810,12 +826,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* use GPS if it's valid and reference position initialized */ - bool use_gps_xy = (ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W) || vision_valid; - bool use_gps_z = (ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W) || vision_valid; + bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; + bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; + /* use VISION if it's valid and has a valid weight parameter */ + bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W; + bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W; /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); - bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || vision_valid; + bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); @@ -834,6 +853,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; float w_z_gps_p = params.w_z_gps_p * w_gps_z; + float w_xy_vision_p = params.w_xy_vision_p; + float w_xy_vision_v = params.w_xy_vision_v; + float w_z_vision_p = params.w_z_vision_p; + /* reduce GPS weight if optical flow is good */ if (use_flow && flow_accurate) { w_xy_gps_p *= params.w_gps_flow; @@ -874,6 +897,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + /* accelerometer bias correction for VISION (use buffered rotation matrix) */ + accel_bias_corr[0] = 0.0f; + accel_bias_corr[1] = 0.0f; + accel_bias_corr[2] = 0.0f; + + if (use_vision_xy) { + accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p; + accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v; + accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p; + accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v; + } + + if (use_vision_z) { + accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p; + } + + /* transform error vector from NED frame to body frame */ + for (int i = 0; i < 3; i++) { + float c = 0.0f; + + for (int j = 0; j < 3; j++) { + c += R_gps[j][i] * accel_bias_corr[j]; + } + + if (isfinite(c)) { + acc_bias[i] += c * params.w_acc_bias * dt; + } + } + /* accelerometer bias correction for flow and baro (assume that there is no delay) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; @@ -916,10 +968,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); } + if (use_vision_z) { + epv = fminf(epv, epv_vision); + inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p); + } + if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(z_est, z_est_prev, sizeof(z_est)); memset(corr_gps, 0, sizeof(corr_gps)); + memset(corr_vision, 0, sizeof(corr_vision)); corr_baro = 0; } else { @@ -957,11 +1015,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + if (use_vision_xy) { + eph = fminf(eph, eph_vision); + + inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p); + inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p); + + if (w_xy_vision_v > MIN_VALID_W) { + inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_vision_v); + inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_vision_v); + } + } + if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); memset(corr_gps, 0, sizeof(corr_gps)); + memset(corr_vision, 0, sizeof(corr_vision)); memset(corr_flow, 0, sizeof(corr_flow)); } else { -- cgit v1.2.3 From a48bede96f79c626e17584f75865ef7839744bf0 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Wed, 23 Jul 2014 14:24:51 +0800 Subject: Remove unused commented code --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 6 ------ 1 file changed, 6 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 398fddd69..77e94a27c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -669,12 +669,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_vision[1][1] = vision.vy - y_est[1]; corr_vision[2][1] = vision.vz - z_est[1]; - // eph_flow = 0.05f; - // epv_vision = 0.05f; - - // w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, eph); - // w_gps_z = min_eph_epv / fmaxf(min_eph_epv, epv); - } } -- cgit v1.2.3 From 38d3efa985be394c846e6afe294006ab074335f7 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Wed, 23 Jul 2014 15:12:26 +0800 Subject: Correct vision velocity estimate correction type --- src/modules/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 77e94a27c..d3192fec2 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -1016,8 +1016,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p); if (w_xy_vision_v > MIN_VALID_W) { - inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_vision_v); - inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_vision_v); + inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v); + inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v); } } -- cgit v1.2.3 From 25ef4bc4a0557327af1f32d81c86e8981772a844 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Thu, 24 Jul 2014 22:31:45 +0800 Subject: Use current rotation matrix for vision instead of delayed rotation --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 d3192fec2..06a81e3fe 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -912,7 +912,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float c = 0.0f; for (int j = 0; j < 3; j++) { - c += R_gps[j][i] * accel_bias_corr[j]; + c += att.R[j][i] * accel_bias_corr[j]; } if (isfinite(c)) { -- cgit v1.2.3 From ee42d5f53d81580bee56b1cdcf69ebfdefb6ba09 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Jul 2014 12:15:33 +0200 Subject: Comment fix --- src/modules/position_estimator_inav/position_estimator_inav_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index dff2768c7..cc0fb4155 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -269,7 +269,7 @@ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); /** * Disable vision input * - * Set to the appropriate key to disable input + * Set to the appropriate key (328754) to disable vision input. * * @min 0.0 * @max 1.0 -- cgit v1.2.3 From 89986cf3e5cc8a9e18d56fe971d3e089232e3beb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 11 Aug 2014 13:46:13 +0200 Subject: 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 02571f56f..6c2c03070 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()); -- cgit v1.2.3 From 5baf9cea0d8fcf6e0c6b3c92b26167067e9cb0fa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 11 Aug 2014 15:29:59 +0200 Subject: 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 ad1ff37e3..1c8d2a2a7 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)); -- cgit v1.2.3 From c8ecf59ab70780efed6ac48967caa8e2358a48ba Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 12 Aug 2014 14:23:26 -0700 Subject: List command bug fix, easier debugging - List command was generating bad data size because it was adding directory entry char to buffer before testing buffer overflow. - Added MAVLINK_FTP_DEBUG define to easily turn on/off noisier debugging output --- src/modules/mavlink/mavlink_ftp.cpp | 42 ++++++++++++++++++++++++++----------- src/modules/mavlink/mavlink_ftp.h | 20 ++++++++++-------- 2 files changed, 41 insertions(+), 21 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 17d1babff..00c8df18c 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -39,6 +39,9 @@ #include "mavlink_ftp.h" +// Uncomment the line below to get better debug output. Never commit with this left on. +//#define MAVLINK_FTP_DEBUG + MavlinkFTP *MavlinkFTP::_server; MavlinkFTP * @@ -125,7 +128,9 @@ MavlinkFTP::_worker(Request *req) warnx("ftp: bad crc"); } - //printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); +#ifdef MAVLINK_FTP_DEBUG + printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); +#endif switch (hdr->opcode) { case kCmdNone: @@ -172,7 +177,9 @@ out: // handle success vs. error if (errorCode == kErrNone) { hdr->opcode = kRspAck; - //warnx("FTP: ack\n"); +#ifdef MAVLINK_FTP_DEBUG + warnx("FTP: ack\n"); +#endif } else { warnx("FTP: nak %u", errorCode); hdr->opcode = kRspNak; @@ -217,7 +224,9 @@ MavlinkFTP::_workList(Request *req) return kErrNotDir; } - //warnx("FTP: list %s offset %d", dirPath, hdr->offset); +#ifdef MAVLINK_FTP_DEBUG + warnx("FTP: list %s offset %d", dirPath, hdr->offset); +#endif ErrorCode errorCode = kErrNone; struct dirent entry, *result = nullptr; @@ -247,11 +256,13 @@ MavlinkFTP::_workList(Request *req) uint32_t fileSize = 0; char buf[256]; + char direntType; - // store the type marker + // Determine the directory entry type switch (entry.d_type) { case DTYPE_FILE: - hdr->data[offset++] = kDirentFile; + // For files we get the file size as well + direntType = kDirentFile; snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name); struct stat st; if (stat(buf, &st) == 0) { @@ -259,29 +270,34 @@ MavlinkFTP::_workList(Request *req) } break; case DTYPE_DIRECTORY: - hdr->data[offset++] = kDirentDir; + direntType = kDirentDir; break; default: - hdr->data[offset++] = kDirentUnknown; + direntType = kDirentUnknown; break; } if (entry.d_type == DTYPE_FILE) { + // Files send filename and file length snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize); } else { + // Everything else just sends name strncpy(buf, entry.d_name, sizeof(buf)); buf[sizeof(buf)-1] = 0; } size_t nameLen = strlen(buf); - // name too big to fit? - if ((nameLen + offset + 2) > kMaxDataLength) { + // Do we have room for the name, the one char directory identifier and the null terminator? + if ((offset + nameLen + 2) > kMaxDataLength) { break; } - // copy the name, which we know will fit + // Move the data into the buffer + hdr->data[offset++] = direntType; strcpy((char *)&hdr->data[offset], buf); - //printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); +#ifdef MAVLINK_FTP_DEBUG + printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); +#endif offset += nameLen + 1; } @@ -342,7 +358,9 @@ MavlinkFTP::_workRead(Request *req) } // Seek to the specified position - //warnx("seek %d", hdr->offset); +#ifdef MAVLINK_FTP_DEBUG + warnx("seek %d", hdr->offset); +#endif if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { // Unable to see to the specified location warnx("seek fail"); diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 800c98b69..43de89de9 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -154,15 +154,17 @@ private: if (!success) { warnx("FTP TX ERR"); - } - // else { - // warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", - // _mavlink->get_system_id(), - // _mavlink->get_component_id(), - // _mavlink->get_channel(), - // len, - // msg.checksum); - // } + } +#ifdef MAVLINK_FTP_DEBUG + else { + warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", + _mavlink->get_system_id(), + _mavlink->get_component_id(), + _mavlink->get_channel(), + len, + msg.checksum); + } +#endif } uint8_t *rawData() { return &_message.data[0]; } -- cgit v1.2.3 From 2cc5c6e84f2fed508a358d2b2fc9c38085c446db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Aug 2014 09:14:43 +0200 Subject: INAV: Add braces to ensure statements are evaluated correctly --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 6 +++--- 1 file changed, 3 insertions(+), 3 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 06a81e3fe..81bbaa658 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -788,21 +788,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* check for timeout on GPS topic */ - if (gps_valid && t > gps.timestamp_position + gps_topic_timeout) { + if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) { gps_valid = false; warnx("GPS timeout"); mavlink_log_info(mavlink_fd, "[inav] GPS timeout"); } /* check for timeout on vision topic */ - if (vision_valid && t > vision.timestamp_boot + vision_topic_timeout) { + if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) { vision_valid = false; warnx("VISION timeout"); mavlink_log_info(mavlink_fd, "[inav] VISION timeout"); } /* check for sonar measurement timeout */ - if (sonar_valid && t > sonar_time + sonar_timeout) { + if (sonar_valid && (t > (sonar_time + sonar_timeout))) { corr_sonar = 0.0f; sonar_valid = false; } -- cgit v1.2.3 From ae4339ce1e3046e44387f9c9e7a66827b859a653 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Aug 2014 09:42:41 +0200 Subject: Update MAVLink version, remove unneeded INT frames --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 4cd384001..4d7487c2b 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 4cd384001b5373e1ecaa6c4cd66994855cec4742 +Subproject commit 4d7487c2bc5f5ccf87bca82970fb2c08d6d8bd50 -- cgit v1.2.3 From 54fc6aa6788a125b387926a45023844daa42ec48 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Aug 2014 10:33:46 +0200 Subject: Hotfix: Optimize shell commands for size - we do not need massive performance there --- src/modules/gpio_led/module.mk | 2 ++ src/modules/unit_test/module.mk | 1 + src/systemcmds/esc_calib/module.mk | 2 ++ src/systemcmds/mtd/module.mk | 2 ++ src/systemcmds/nshterm/module.mk | 2 ++ src/systemcmds/pwm/module.mk | 2 ++ 6 files changed, 11 insertions(+) diff --git a/src/modules/gpio_led/module.mk b/src/modules/gpio_led/module.mk index 3b8553489..70c75b10c 100644 --- a/src/modules/gpio_led/module.mk +++ b/src/modules/gpio_led/module.mk @@ -37,3 +37,5 @@ MODULE_COMMAND = gpio_led SRCS = gpio_led.c + +MAXOPTIMIZATION = -Os diff --git a/src/modules/unit_test/module.mk b/src/modules/unit_test/module.mk index f00b0f592..5000790a5 100644 --- a/src/modules/unit_test/module.mk +++ b/src/modules/unit_test/module.mk @@ -37,3 +37,4 @@ SRCS = unit_test.cpp +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/esc_calib/module.mk b/src/systemcmds/esc_calib/module.mk index 990c56768..ce87eb3e2 100644 --- a/src/systemcmds/esc_calib/module.mk +++ b/src/systemcmds/esc_calib/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = esc_calib SRCS = esc_calib.c MODULE_STACKSIZE = 4096 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk index b3fceceb5..1bc4f414e 100644 --- a/src/systemcmds/mtd/module.mk +++ b/src/systemcmds/mtd/module.mk @@ -4,3 +4,5 @@ MODULE_COMMAND = mtd SRCS = mtd.c 24xxxx_mtd.c + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index e2fa0ff80..7d2c59f91 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c MODULE_STACKSIZE = 1400 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/pwm/module.mk b/src/systemcmds/pwm/module.mk index 13a24150f..a51ac8e0c 100644 --- a/src/systemcmds/pwm/module.mk +++ b/src/systemcmds/pwm/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = pwm SRCS = pwm.c MODULE_STACKSIZE = 1800 + +MAXOPTIMIZATION = -Os -- cgit v1.2.3