From 3a898e54adbaac7fda28e0b9079076d9cdf8028c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 14:30:29 +0200 Subject: [PATCH 01/65] towards a global map projection instance, WIP: need to remove local updates of the reference and add a global map update --- src/lib/geo/geo.c | 46 ++++++++++++------- src/lib/geo/geo.h | 9 ++-- .../fw_att_pos_estimator_main.cpp | 6 +-- src/modules/mavlink/mavlink_receiver.cpp | 4 +- .../mc_pos_control/mc_pos_control_main.cpp | 9 ++-- .../position_estimator_inav_main.c | 20 +++----- 6 files changed, 49 insertions(+), 45 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 9a24ff50e2..32d52aadc5 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -55,33 +55,47 @@ * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ -__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 -{ - ref->lat = lat_0 / 180.0 * M_PI; - ref->lon = lon_0 / 180.0 * M_PI; +struct map_projection_reference_s mp_ref; - ref->sin_lat = sin(ref->lat); - ref->cos_lat = cos(ref->lat); +__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + mp_ref.lat = lat_0 / 180.0 * M_PI; + mp_ref.lon = lon_0 / 180.0 * M_PI; + + mp_ref.sin_lat = sin(mp_ref.lat); + mp_ref.cos_lat = cos(mp_ref.lat); + + mp_ref.init_done = true; } -__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +__EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) { + if (!mp_ref.init_done) { + return false; + } + double lat_rad = lat / 180.0 * M_PI; double lon_rad = lon / 180.0 * M_PI; double sin_lat = sin(lat_rad); double cos_lat = cos(lat_rad); - double cos_d_lon = cos(lon_rad - ref->lon); + double cos_d_lon = cos(lon_rad - mp_ref.lon); - double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon); + double c = acos(mp_ref.sin_lat * sin_lat + mp_ref.cos_lat * cos_lat * cos_d_lon); double k = (c == 0.0) ? 1.0 : (c / sin(c)); - *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; - *y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH; + *x = k * (mp_ref.cos_lat * sin_lat - mp_ref.sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; + *y = k * cos_lat * sin(lon_rad - mp_ref.lon) * CONSTANTS_RADIUS_OF_EARTH; + + return true; } -__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +__EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon) { + if (!mp_ref.init_done) { + return false; + } + float x_rad = x / CONSTANTS_RADIUS_OF_EARTH; float y_rad = y / CONSTANTS_RADIUS_OF_EARTH; double c = sqrtf(x_rad * x_rad + y_rad * y_rad); @@ -92,12 +106,12 @@ __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, f double lon_rad; if (c != 0.0) { - lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c); - lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); + lat_rad = asin(cos_c * mp_ref.sin_lat + (x_rad * sin_c * mp_ref.cos_lat) / c); + lon_rad = (mp_ref.lon + atan2(y_rad * sin_c, c * mp_ref.cos_lat * cos_c - x_rad * mp_ref.sin_lat * sin_c)); } else { - lat_rad = ref->lat; - lon_rad = ref->lon; + lat_rad = mp_ref.lat; + lon_rad = mp_ref.lon; } *lat = lat_rad * 180.0 / M_PI; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 0a3f85d970..b48d26a66f 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -71,6 +71,7 @@ struct map_projection_reference_s { double lon; double sin_lat; double cos_lat; + bool init_done; }; /** @@ -80,7 +81,7 @@ struct map_projection_reference_s { * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0); +__EXPORT void map_projection_init(double lat_0, double lon_0); /** * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane @@ -88,8 +89,9 @@ __EXPORT void map_projection_init(struct map_projection_reference_s *ref, double * @param y east * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) + * @return true if map_projection_init was called before, false else */ -__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y); +__EXPORT bool map_projection_project(double lat, double lon, float *x, float *y); /** * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system @@ -98,8 +100,9 @@ __EXPORT void map_projection_project(struct map_projection_reference_s *ref, dou * @param y east * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) + * @return true if map_projection_init was called before, false else */ -__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); +__EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon); /** * Returns the distance to the next waypoint in meters. diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 9d2d9ed5f3..420a03644e 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -177,8 +177,6 @@ private: struct sensor_combined_s _sensor_combined; #endif - struct map_projection_reference_s _pos_ref; - float _baro_ref; /**< barometer reference altitude */ float _baro_gps_offset; /**< offset between GPS and baro */ @@ -822,8 +820,6 @@ FixedwingEstimator::task_main() _ekf->baroHgt = _baro.altitude - _baro_ref; _baro_gps_offset = _baro_ref - _local_pos.ref_alt; - // XXX this is not multithreading safe - map_projection_init(&_pos_ref, lat, lon); mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); _gps_initialized = true; @@ -1046,7 +1042,7 @@ FixedwingEstimator::task_main() if (_local_pos.xy_global) { double est_lat, est_lon; - map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); + map_projection_reproject(_local_pos.x, _local_pos.y, &est_lat, &est_lon); _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_gps_usec = _gps.time_gps_usec; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index adafdd1395..df61c3c156 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -795,7 +795,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; - map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); +// map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); //XXX fix reference update hil_local_pos.ref_timestamp = timestamp; hil_local_pos.ref_lat = lat; hil_local_pos.ref_lon = lon; @@ -804,7 +804,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) float x; float y; - map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y); + map_projection_project(lat, lon, &x, &y); hil_local_pos.timestamp = timestamp; hil_local_pos.xy_valid = true; hil_local_pos.z_valid = true; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index dc0aa172a4..963349d42e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -478,17 +478,17 @@ MulticopterPositionControl::update_ref() if (_ref_timestamp != 0) { /* calculate current position setpoint in global frame */ - map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); + map_projection_reproject(_pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); alt_sp = _ref_alt - _pos_sp(2); } /* update local projection reference */ - map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); +// map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); //XXX fix reference update _ref_alt = _local_pos.ref_alt; if (_ref_timestamp != 0) { /* reproject position setpoint to new reference */ - map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); + map_projection_project(lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); _pos_sp(2) = -(alt_sp - _ref_alt); } @@ -687,8 +687,7 @@ MulticopterPositionControl::task_main() _reset_alt_sp = true; /* project setpoint to local frame */ - map_projection_project(&_ref_pos, - _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, + map_projection_project(_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, &_pos_sp.data[0], &_pos_sp.data[1]); _pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 368424853c..b803dfb581 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -558,12 +558,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ref_inited) { /* calculate current estimated position in global frame */ est_alt = local_pos.ref_alt - local_pos.z; - map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); + map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); } - /* update reference */ - map_projection_init(&ref, home.lat, home.lon); - /* update baro offset */ baro_offset += home.alt - local_pos.ref_alt; @@ -572,9 +569,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.ref_alt = home.alt; local_pos.ref_timestamp = home.timestamp; - if (ref_inited) { + if (ref_inited) { //XXX fix reference update /* reproject position estimate with new reference */ - map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]); + map_projection_project(est_lat, est_lon, &x_est[0], &y_est[0]); z_est[0] = -(est_alt - local_pos.ref_alt); } @@ -633,18 +630,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.ref_lon = lon; local_pos.ref_alt = alt; local_pos.ref_timestamp = t; - - /* initialize projection */ - map_projection_init(&ref, lat, lon); - warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); } } - if (ref_inited) { + if (ref_inited) { //XXX fix reference update /* project GPS lat lon to plane */ float gps_proj[2]; - map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); + map_projection_project(lat, lon, &(gps_proj[0]), &(gps_proj[1])); /* reset position estimate when GPS becomes good */ if (reset_est) { @@ -938,7 +930,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.time_gps_usec = gps.time_gps_usec; double est_lat, est_lon; - map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); + map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); global_pos.lat = est_lat; global_pos.lon = est_lon; From 6517c201f701da97acde1d7e3cc72712b9a1613c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 15:59:13 +0200 Subject: [PATCH 02/65] geo: add missing return, make map projection reference static --- src/lib/geo/geo.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 32d52aadc5..60994940f9 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -55,7 +55,7 @@ * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ -struct map_projection_reference_s mp_ref; +static struct map_projection_reference_s mp_ref; __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { @@ -116,6 +116,8 @@ __EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lo *lat = lat_rad * 180.0 / M_PI; *lon = lon_rad * 180.0 / M_PI; + + return true; } From fbcb248bc3a91d1c26f6f47a2a46badd2d8a58f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 15:59:49 +0200 Subject: [PATCH 03/65] navigator: init map projection on home pos update --- src/modules/navigator/navigator_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d4d23396a1..b7cf49cfa9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -625,6 +625,7 @@ Navigator::task_main() parameters_update(); global_position_update(); home_position_update(); + map_projection_init(_home_pos.lat, _home_pos.lon); navigation_capabilities_update(); offboard_mission_update(_vstatus.is_rotary_wing); onboard_mission_update(); @@ -816,6 +817,7 @@ Navigator::task_main() if (fds[2].revents & POLLIN) { home_position_update(); // XXX check if home position really changed + map_projection_init(_home_pos.lat, _home_pos.lon); dispatch(EVENT_HOME_POSITION_CHANGED); } From 8bb1b39399eb09ddf93c2682713d80be4f776c5d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 16:12:28 +0200 Subject: [PATCH 04/65] mavlink receiver: hil: local pos reference lat lon from first data --- src/modules/mavlink/mavlink_receiver.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index df61c3c156..40121efe4f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -795,7 +795,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; -// map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); //XXX fix reference update hil_local_pos.ref_timestamp = timestamp; hil_local_pos.ref_lat = lat; hil_local_pos.ref_lon = lon; From 35d15720ef79dae4d6f6202092d4869a455fc7d2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:04:27 +0200 Subject: [PATCH 05/65] geo: add timestamp and function to get init state --- src/lib/geo/geo.c | 10 ++++++++-- src/lib/geo/geo.h | 7 +++++++ 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 60994940f9..dbbf1f80cc 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -57,6 +57,11 @@ static struct map_projection_reference_s mp_ref; +__EXPORT bool map_projection_inited() +{ + return mp_ref.init_done; +} + __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { mp_ref.lat = lat_0 / 180.0 * M_PI; @@ -65,12 +70,13 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are mp_ref.sin_lat = sin(mp_ref.lat); mp_ref.cos_lat = cos(mp_ref.lat); + mp_ref.timestamp = hrt_absolute_time(); mp_ref.init_done = true; } __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) { - if (!mp_ref.init_done) { + if (!map_projection_inited()) { return false; } @@ -92,7 +98,7 @@ __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) __EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon) { - if (!mp_ref.init_done) { + if (!map_projection_inited()) { return false; } diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index b48d26a66f..b19bfe4625 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -72,8 +72,15 @@ struct map_projection_reference_s { double sin_lat; double cos_lat; bool init_done; + uint64_t timestamp; }; +/** + * Initializes the map transformation. + * @return true if map_projection_init was called before, false else + */ +__EXPORT bool map_projection_inited(); + /** * Initializes the map transformation. * From 9f2da53f08f5d186d95653bad9afa04e8c7276d2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:05:30 +0200 Subject: [PATCH 06/65] mc pos control: map projection init not needed anymore --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 963349d42e..fa3438ea82 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -482,8 +482,6 @@ MulticopterPositionControl::update_ref() alt_sp = _ref_alt - _pos_sp(2); } - /* update local projection reference */ -// map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); //XXX fix reference update _ref_alt = _local_pos.ref_alt; if (_ref_timestamp != 0) { From 618ac319e63a6597cc62df9c810d76cdc094012b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:06:10 +0200 Subject: [PATCH 07/65] pos estimator inav: check if map projection is initialized --- .../position_estimator_inav_main.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index b803dfb581..d01bf488f9 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -549,7 +549,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(home_position), home_position_sub, &home); - if (home.timestamp != home_timestamp) { + if (home.timestamp != home_timestamp && map_projection_inited()) { home_timestamp = home.timestamp; double est_lat, est_lon; @@ -569,7 +569,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.ref_alt = home.alt; local_pos.ref_timestamp = home.timestamp; - if (ref_inited) { //XXX fix reference update + if (ref_inited) { /* reproject position estimate with new reference */ map_projection_project(est_lat, est_lon, &x_est[0], &y_est[0]); z_est[0] = -(est_alt - local_pos.ref_alt); @@ -602,7 +602,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (gps_valid) { + if (gps_valid && map_projection_inited()) { double lat = gps.lat * 1e-7; double lon = gps.lon * 1e-7; float alt = gps.alt * 1e-3; @@ -633,7 +633,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (ref_inited) { //XXX fix reference update + if (ref_inited) { /* project GPS lat lon to plane */ float gps_proj[2]; map_projection_project(lat, lon, &(gps_proj[0]), &(gps_proj[1])); From b249d04e52ae34d43d9d80638d6f1ead476f39ea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:13:06 +0200 Subject: [PATCH 08/65] geo: get function for timestamp of map projection --- src/lib/geo/geo.c | 5 +++++ src/lib/geo/geo.h | 7 +++++++ 2 files changed, 12 insertions(+) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index dbbf1f80cc..1e3ef56aef 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -62,6 +62,11 @@ __EXPORT bool map_projection_inited() return mp_ref.init_done; } +__EXPORT uint64_t map_projection_timestamp() +{ + return mp_ref.timestamp; +} + __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { mp_ref.lat = lat_0 / 180.0 * M_PI; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index b19bfe4625..913e83b6e1 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -81,6 +81,13 @@ struct map_projection_reference_s { */ __EXPORT bool map_projection_inited(); + +/** + * Initializes the map transformation. + * @return the timestamp of the map_projection + */ +__EXPORT uint64_t map_projection_timestamp(); + /** * Initializes the map transformation. * From 4e30784d6c506cdfdf58c623da20cffcb799e496 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:16:20 +0200 Subject: [PATCH 09/65] rename map_projection_inited to map_projection_initialized --- src/lib/geo/geo.c | 6 +++--- src/lib/geo/geo.h | 2 +- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 1e3ef56aef..725a04a298 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -57,7 +57,7 @@ static struct map_projection_reference_s mp_ref; -__EXPORT bool map_projection_inited() +__EXPORT bool map_projection_initialized() { return mp_ref.init_done; } @@ -81,7 +81,7 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) { - if (!map_projection_inited()) { + if (!map_projection_initialized()) { return false; } @@ -103,7 +103,7 @@ __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) __EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon) { - if (!map_projection_inited()) { + if (!map_projection_initialized()) { return false; } diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 913e83b6e1..66e4dc75e8 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -79,7 +79,7 @@ struct map_projection_reference_s { * Initializes the map transformation. * @return true if map_projection_init was called before, false else */ -__EXPORT bool map_projection_inited(); +__EXPORT bool map_projection_initialized(); /** diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index d01bf488f9..e88493a5e2 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -549,7 +549,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(home_position), home_position_sub, &home); - if (home.timestamp != home_timestamp && map_projection_inited()) { + if (home.timestamp != home_timestamp && map_projection_initialized()) { home_timestamp = home.timestamp; double est_lat, est_lon; @@ -602,7 +602,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (gps_valid && map_projection_inited()) { + if (gps_valid && map_projection_initialized()) { double lat = gps.lat * 1e-7; double lon = gps.lon * 1e-7; float alt = gps.alt * 1e-3; From 9ebd6b348687d5563b87c5c872d432adbfad696f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 09:41:46 +0200 Subject: [PATCH 10/65] geo: interface to get reference lat lon, option to set reference timestamp on initialization --- src/lib/geo/geo.c | 16 ++++++++++++++-- src/lib/geo/geo.h | 8 +++++++- 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 725a04a298..a2117ecd01 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -67,7 +67,7 @@ __EXPORT uint64_t map_projection_timestamp() return mp_ref.timestamp; } -__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT void map_projection_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { mp_ref.lat = lat_0 / 180.0 * M_PI; mp_ref.lon = lon_0 / 180.0 * M_PI; @@ -75,10 +75,22 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are mp_ref.sin_lat = sin(mp_ref.lat); mp_ref.cos_lat = cos(mp_ref.lat); - mp_ref.timestamp = hrt_absolute_time(); + mp_ref.timestamp = timestamp; mp_ref.init_done = true; } +__EXPORT bool map_projection_reference(double *ref_lat, double *ref_lon) +{ + if (!map_projection_initialized()) { + return false; + } + + *ref_lat = mp_ref.lat; + *ref_lon = mp_ref.lon; + + return true; +} + __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) { if (!map_projection_initialized()) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 66e4dc75e8..b5e6596855 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -88,6 +88,12 @@ __EXPORT bool map_projection_initialized(); */ __EXPORT uint64_t map_projection_timestamp(); +/** + * Writes the reference values to ref_lat and ref_lon + * @return true if map_projection_init was called before, false else + */ +__EXPORT bool map_projection_reference(double *ref_lat, double *ref_lon); + /** * Initializes the map transformation. * @@ -95,7 +101,7 @@ __EXPORT uint64_t map_projection_timestamp(); * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT void map_projection_init(double lat_0, double lon_0); +__EXPORT void map_projection_init(double lat_0, double lon_0, uint64_t timestamp); /** * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane From f5d9023b1060f10c14d016492c88375c6fdc22aa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 09:42:28 +0200 Subject: [PATCH 11/65] navigator: set home_pos timestamp as map projection reference timestamp --- src/modules/navigator/navigator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b7cf49cfa9..228324789b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -625,7 +625,7 @@ Navigator::task_main() parameters_update(); global_position_update(); home_position_update(); - map_projection_init(_home_pos.lat, _home_pos.lon); + map_projection_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp); navigation_capabilities_update(); offboard_mission_update(_vstatus.is_rotary_wing); onboard_mission_update(); @@ -817,7 +817,7 @@ Navigator::task_main() if (fds[2].revents & POLLIN) { home_position_update(); // XXX check if home position really changed - map_projection_init(_home_pos.lat, _home_pos.lon); + map_projection_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp); dispatch(EVENT_HOME_POSITION_CHANGED); } From 7e1ea35571149a9d766cc7122738f206f1ed3427 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 09:43:19 +0200 Subject: [PATCH 12/65] mavlink receiver: in hil use map_projection_timestamp for hil_local_pos.ref_timestamp --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 40121efe4f..e9b58bccf2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -795,7 +795,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; - hil_local_pos.ref_timestamp = timestamp; + hil_local_pos.ref_timestamp = map_projection_timestamp(); hil_local_pos.ref_lat = lat; hil_local_pos.ref_lon = lon; hil_local_pos.ref_alt = _hil_local_alt0; From 5285eb2f997b590071186a8f695af4375f96e779 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 09:44:40 +0200 Subject: [PATCH 13/65] fw att pos estimator: use map projection reference values for local pos --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 420a03644e..45ca13a7fb 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -797,7 +797,7 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&start_time) > 100000) { - if (!_gps_initialized && (_ekf->GPSstatus == 3)) { + if (!_gps_initialized && (_ekf->GPSstatus == 3) && map_projection_initialized()) { _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; @@ -809,10 +809,9 @@ FixedwingEstimator::task_main() _ekf->InitialiseFilter(_ekf->velNED); // Initialize projection - _local_pos.ref_lat = _gps.lat; - _local_pos.ref_lon = _gps.lon; + map_projection_reference(&_local_pos.ref_lat, &_local_pos.ref_lon); _local_pos.ref_alt = alt; - _local_pos.ref_timestamp = _gps.timestamp_position; + _local_pos.ref_timestamp = map_projection_timestamp(); // Store orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); From 08bc777208a8510da7cd91fab5059ffa8bd14150 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 13:57:06 +0200 Subject: [PATCH 14/65] pos estimator inav: use map_projection_reference to set local pos reference lat lon --- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index e88493a5e2..91642e5b92 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -626,8 +626,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) z_est[0] = 0.0f; y_est[2] = accel_NED[1]; - local_pos.ref_lat = lat; - local_pos.ref_lon = lon; + map_projection_reference(&local_pos.ref_lat, &local_pos.ref_lon); + local_pos.ref_alt = alt; local_pos.ref_timestamp = t; } From 241a99fc28992694f807595ce6bb48f460c9a28a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 14:45:21 +0200 Subject: [PATCH 15/65] position estimator mc: do not initialize map projection because this is now handled globally --- .../position_estimator_mc/position_estimator_mc_main.c | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c index 3639618193..ee6457aded 100755 --- a/src/modules/position_estimator_mc/position_estimator_mc_main.c +++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c @@ -322,11 +322,6 @@ int position_estimator_mc_thread_main(int argc, char *argv[]) lon_current = ((double)(gps.lon)) * 1e-7d; alt_current = gps.alt * 1e-3f; gps_origin_altitude = alt_current; - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - /* publish global position messages only after first GPS message */ - printf("[pos_est_mc] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); - } else { mavlink_log_info(mavlink_fd, "[pos_est_mc] I'm NOT using GPS - I use VICON"); /* onboard calculated position estimations */ From aab64af8844e03b04f5616c1beecf18c9f6a7d81 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 14:47:37 +0200 Subject: [PATCH 16/65] geo: map projection: safer initialization, only accept init from navigator, return int instead of bool --- src/lib/geo/geo.c | 41 ++++++++++++++++++++++++----------------- src/lib/geo/geo.h | 16 ++++++++-------- 2 files changed, 32 insertions(+), 25 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index a2117ecd01..6cbf74e466 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -55,7 +55,7 @@ * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ -static struct map_projection_reference_s mp_ref; +static struct map_projection_reference_s mp_ref = {0}; __EXPORT bool map_projection_initialized() { @@ -67,34 +67,41 @@ __EXPORT uint64_t map_projection_timestamp() return mp_ref.timestamp; } -__EXPORT void map_projection_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - mp_ref.lat = lat_0 / 180.0 * M_PI; - mp_ref.lon = lon_0 / 180.0 * M_PI; + if (strcmp("navigator", getprogname() == 0)) { - mp_ref.sin_lat = sin(mp_ref.lat); - mp_ref.cos_lat = cos(mp_ref.lat); + mp_ref.lat = lat_0 / 180.0 * M_PI; + mp_ref.lon = lon_0 / 180.0 * M_PI; - mp_ref.timestamp = timestamp; - mp_ref.init_done = true; + mp_ref.sin_lat = sin(mp_ref.lat); + mp_ref.cos_lat = cos(mp_ref.lat); + + mp_ref.timestamp = timestamp; + mp_ref.init_done = true; + + return 0; + } else { + return -1; + } } -__EXPORT bool map_projection_reference(double *ref_lat, double *ref_lon) +__EXPORT int map_projection_reference(double *ref_lat, double *ref_lon) { if (!map_projection_initialized()) { - return false; + return -1; } *ref_lat = mp_ref.lat; *ref_lon = mp_ref.lon; - return true; + return 0; } -__EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) +__EXPORT int map_projection_project(double lat, double lon, float *x, float *y) { if (!map_projection_initialized()) { - return false; + return -1; } double lat_rad = lat / 180.0 * M_PI; @@ -110,13 +117,13 @@ __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) *x = k * (mp_ref.cos_lat * sin_lat - mp_ref.sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; *y = k * cos_lat * sin(lon_rad - mp_ref.lon) * CONSTANTS_RADIUS_OF_EARTH; - return true; + return 0; } -__EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon) +__EXPORT int map_projection_reproject(float x, float y, double *lat, double *lon) { if (!map_projection_initialized()) { - return false; + return -1; } float x_rad = x / CONSTANTS_RADIUS_OF_EARTH; @@ -140,7 +147,7 @@ __EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lo *lat = lat_rad * 180.0 / M_PI; *lon = lon_rad * 180.0 / M_PI; - return true; + return 0; } diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index b5e6596855..b0f8d6bc40 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -77,7 +77,7 @@ struct map_projection_reference_s { /** * Initializes the map transformation. - * @return true if map_projection_init was called before, false else + * @return true if map was initialized before, false else */ __EXPORT bool map_projection_initialized(); @@ -90,9 +90,9 @@ __EXPORT uint64_t map_projection_timestamp(); /** * Writes the reference values to ref_lat and ref_lon - * @return true if map_projection_init was called before, false else + * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT bool map_projection_reference(double *ref_lat, double *ref_lon); +__EXPORT int map_projection_reference(double *ref_lat, double *ref_lon); /** * Initializes the map transformation. @@ -101,7 +101,7 @@ __EXPORT bool map_projection_reference(double *ref_lat, double *ref_lon); * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT void map_projection_init(double lat_0, double lon_0, uint64_t timestamp); +__EXPORT int map_projection_init(double lat_0, double lon_0, uint64_t timestamp); /** * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane @@ -109,9 +109,9 @@ __EXPORT void map_projection_init(double lat_0, double lon_0, uint64_t timestamp * @param y east * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) - * @return true if map_projection_init was called before, false else + * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT bool map_projection_project(double lat, double lon, float *x, float *y); +__EXPORT int map_projection_project(double lat, double lon, float *x, float *y); /** * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system @@ -120,9 +120,9 @@ __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) * @param y east * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) - * @return true if map_projection_init was called before, false else + * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon); +__EXPORT int map_projection_reproject(float x, float y, double *lat, double *lon); /** * Returns the distance to the next waypoint in meters. From f7e18eaa5809580878b377dfa78ce0ece96d55a7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 15:08:06 +0200 Subject: [PATCH 17/65] uorb home position topic: add valid flag --- src/modules/uORB/topics/home_position.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 08d11abaea..73c8dc50d1 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -63,6 +63,7 @@ struct home_position_s double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float alt; /**< Altitude in meters */ + bool valid; /**< True if home position is valid */ }; /** From 06dd10cb018d48f60abd1a5e9f83fbdc67867d8c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 15:08:38 +0200 Subject: [PATCH 18/65] commander: set home position valid flag --- src/modules/commander/commander.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1827f252cc..23fe647695 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -617,6 +617,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); + home->valid = true; + /* announce new home position */ if (*home_pub > 0) { orb_publish(ORB_ID(home_position), *home_pub, home); @@ -999,6 +1001,7 @@ int commander_thread_main(int argc, char *argv[]) home.lat = global_position.lat; home.lon = global_position.lon; home.alt = global_position.alt; + home.valid = true; warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); @@ -1385,6 +1388,7 @@ int commander_thread_main(int argc, char *argv[]) home.lat = global_position.lat; home.lon = global_position.lon; home.alt = global_position.alt; + home.valid = true; warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); From 35b0c294568ca916b344e6a9e6a8e27cefb6f8ca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 15:09:11 +0200 Subject: [PATCH 19/65] navigator: set reference for map projection only once --- src/modules/navigator/navigator_main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e430284156..2f6bf49f3a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -625,7 +625,6 @@ Navigator::task_main() parameters_update(); global_position_update(); home_position_update(); - map_projection_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp); navigation_capabilities_update(); offboard_mission_update(_vstatus.is_rotary_wing); onboard_mission_update(); @@ -781,7 +780,6 @@ Navigator::task_main() if (fds[2].revents & POLLIN) { home_position_update(); // XXX check if home position really changed - map_projection_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp); dispatch(EVENT_HOME_POSITION_CHANGED); } @@ -824,6 +822,11 @@ Navigator::task_main() _global_pos_valid = _vstatus.condition_global_position_valid; + /* set reference for map _projection if global pos is valid and home position is valid and we have not done so already */ + if (!map_projection_initialized() && _global_pos_valid && _home_pos.valid) { + map_projection_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp); + } + /* publish position setpoint triplet if updated */ if (_pos_sp_triplet_updated) { _pos_sp_triplet_updated = false; From 3ec818ce1ed764aeb5f3e8c7371b53b87c2498c3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 14:39:36 +0200 Subject: [PATCH 20/65] rerwite projection interrface to not break the current implementation --- src/lib/geo/geo.c | 98 +++++++++++++++++++++++++++++++---------------- src/lib/geo/geo.h | 81 ++++++++++++++++++++++++++++++++------- 2 files changed, 132 insertions(+), 47 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 6cbf74e466..53e601fadb 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -57,50 +57,77 @@ static struct map_projection_reference_s mp_ref = {0}; -__EXPORT bool map_projection_initialized() +__EXPORT bool map_projection_global_initialized() { - return mp_ref.init_done; + return map_projection_initialized(&mp_ref); } -__EXPORT uint64_t map_projection_timestamp() +__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref) { - return mp_ref.timestamp; + return ref->init_done; } -__EXPORT int map_projection_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT uint64_t map_projection_global_timestamp() +{ + return map_projection_timestamp(&mp_ref); +} + +__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref) +{ + return ref->timestamp; +} + +__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { if (strcmp("navigator", getprogname() == 0)) { - mp_ref.lat = lat_0 / 180.0 * M_PI; - mp_ref.lon = lon_0 / 180.0 * M_PI; - - mp_ref.sin_lat = sin(mp_ref.lat); - mp_ref.cos_lat = cos(mp_ref.lat); - - mp_ref.timestamp = timestamp; - mp_ref.init_done = true; - - return 0; + return map_projection_init(&mp_ref, lat_0, lon_0, timestamp); } else { return -1; } } -__EXPORT int map_projection_reference(double *ref_lat, double *ref_lon) +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - if (!map_projection_initialized()) { - return -1; - } - *ref_lat = mp_ref.lat; - *ref_lon = mp_ref.lon; + ref->lat = lat_0 / 180.0 * M_PI; + ref->lon = lon_0 / 180.0 * M_PI; + + ref->sin_lat = sin(ref->lat); + ref->cos_lat = cos(ref->lat); + + ref->timestamp = timestamp; + ref->init_done = true; return 0; } -__EXPORT int map_projection_project(double lat, double lon, float *x, float *y) +__EXPORT int map_projection_global_reference(double *ref_lat, double *ref_lon) { - if (!map_projection_initialized()) { + return map_projection_reference_s(&mp_ref, ref_lat, ref_lon); +} + +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat, double *ref_lon) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + *ref_lat = ref->lat; + *ref_lon = ref->lon; + + return 0; +} + +__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y) +{ + return map_projection_project(&mp_ref, lat, lon, x, y); + +} + +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +{ + if (!map_projection_initialized(ref)) { return -1; } @@ -109,20 +136,25 @@ __EXPORT int map_projection_project(double lat, double lon, float *x, float *y) double sin_lat = sin(lat_rad); double cos_lat = cos(lat_rad); - double cos_d_lon = cos(lon_rad - mp_ref.lon); + double cos_d_lon = cos(lon_rad - ref->lon); - double c = acos(mp_ref.sin_lat * sin_lat + mp_ref.cos_lat * cos_lat * cos_d_lon); + double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon); double k = (c == 0.0) ? 1.0 : (c / sin(c)); - *x = k * (mp_ref.cos_lat * sin_lat - mp_ref.sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; - *y = k * cos_lat * sin(lon_rad - mp_ref.lon) * CONSTANTS_RADIUS_OF_EARTH; + *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; + *y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH; return 0; } -__EXPORT int map_projection_reproject(float x, float y, double *lat, double *lon) +__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon) { - if (!map_projection_initialized()) { + map_projection_project(&mp_ref, x, y, lat, lon); +} + +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +{ + if (!map_projection_initialized(ref)) { return -1; } @@ -136,12 +168,12 @@ __EXPORT int map_projection_reproject(float x, float y, double *lat, double *lon double lon_rad; if (c != 0.0) { - lat_rad = asin(cos_c * mp_ref.sin_lat + (x_rad * sin_c * mp_ref.cos_lat) / c); - lon_rad = (mp_ref.lon + atan2(y_rad * sin_c, c * mp_ref.cos_lat * cos_c - x_rad * mp_ref.sin_lat * sin_c)); + lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c); + lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); } else { - lat_rad = mp_ref.lat; - lon_rad = mp_ref.lon; + lat_rad = ref->lat; + lon_rad = ref->lon; } *lat = lat_rad * 180.0 / M_PI; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index b0f8d6bc40..990f791948 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -76,45 +76,86 @@ struct map_projection_reference_s { }; /** - * Initializes the map transformation. + * Checks if global projection was initialized * @return true if map was initialized before, false else */ -__EXPORT bool map_projection_initialized(); - +__EXPORT bool map_projection_global_initialized(); /** - * Initializes the map transformation. + * Checks if projection given as argument was initialized + * @return true if map was initialized before, false else + */ +__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref); + +/** + * Get the timestamp of the global map projection * @return the timestamp of the map_projection */ -__EXPORT uint64_t map_projection_timestamp(); +__EXPORT uint64_t map_projection_global_timestamp(); /** - * Writes the reference values to ref_lat and ref_lon + * Get the timestamp of the map projection given by the argument + * @return the timestamp of the map_projection + */ +__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref); + +/** + * Writes the reference values of the global projection to ref_lat and ref_lon * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_reference(double *ref_lat, double *ref_lon); +__EXPORT int map_projection_global_reference(double *ref_lat, double *ref_lon); /** - * Initializes the map transformation. + * Writes the reference values of the projection given by the argument to ref_lat and ref_lon + * @return 0 if map_projection_init was called before, -1 else + */ +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat, double *ref_lon); + +/** + * Initializes the global map transformation. * - * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane + * Initializes the transformation between the geographic coordinate system and + * the azimuthal equidistant plane * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT int map_projection_init(double lat_0, double lon_0, uint64_t timestamp); +__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp); /** - * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane + * Initializes the map transformation given by the argument. + * + * Initializes the transformation between the geographic coordinate system and + * the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp); + +/** + * Transforms a point in the geographic coordinate system to the local + * azimuthal equidistant plane using the global projection * @param x north * @param y east * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_project(double lat, double lon, float *x, float *y); +__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y); + + + /* Transforms a point in the geographic coordinate system to the local + * azimuthal equidistant plane using the projection given by the argument + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + * @return 0 if map_projection_init was called before, -1 else + */ +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y); /** - * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system + * Transforms a point in the local azimuthal equidistant plane to the + * geographic coordinate system using the global projection * * @param x north * @param y east @@ -122,7 +163,19 @@ __EXPORT int map_projection_project(double lat, double lon, float *x, float *y); * @param lon in degrees (8.1234567°, not 81234567°) * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_reproject(float x, float y, double *lat, double *lon); +__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon); + +/** + * Transforms a point in the local azimuthal equidistant plane to the + * geographic coordinate system using the projection given by the argument + * + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + * @return 0 if map_projection_init was called before, -1 else + */ +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); /** * Returns the distance to the next waypoint in meters. From 4f84cdc8b871b6c8da7f05a58e96f97b65f290c9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 14:40:01 +0200 Subject: [PATCH 21/65] fw_att_pos_estimator: use new global map projection --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 45ca13a7fb..79328d5161 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -797,7 +797,7 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&start_time) > 100000) { - if (!_gps_initialized && (_ekf->GPSstatus == 3) && map_projection_initialized()) { + if (!_gps_initialized && (_ekf->GPSstatus == 3) && map_projection_global_initialized()) { _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; @@ -809,9 +809,9 @@ FixedwingEstimator::task_main() _ekf->InitialiseFilter(_ekf->velNED); // Initialize projection - map_projection_reference(&_local_pos.ref_lat, &_local_pos.ref_lon); + map_projection_global_reference(&_local_pos.ref_lat, &_local_pos.ref_lon); _local_pos.ref_alt = alt; - _local_pos.ref_timestamp = map_projection_timestamp(); + _local_pos.ref_timestamp = map_projection_global_timestamp(); // Store orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); @@ -1041,7 +1041,7 @@ FixedwingEstimator::task_main() if (_local_pos.xy_global) { double est_lat, est_lon; - map_projection_reproject(_local_pos.x, _local_pos.y, &est_lat, &est_lon); + map_projection_global_reproject(_local_pos.x, _local_pos.y, &est_lat, &est_lon); _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_gps_usec = _gps.time_gps_usec; From a7289a32664e3118bf553d23cfd91ed5c86caa84 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:39:26 +0200 Subject: [PATCH 22/65] mavlink interface: update mapprojection usage --- src/lib/geo/geo.c | 11 ++++++++--- src/lib/geo/geo.h | 13 ++++++++++++- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- 3 files changed, 22 insertions(+), 6 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 53e601fadb..438e354dff 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -81,13 +81,13 @@ __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t tim { if (strcmp("navigator", getprogname() == 0)) { - return map_projection_init(&mp_ref, lat_0, lon_0, timestamp); + return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); } else { return -1; } } -__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { ref->lat = lat_0 / 180.0 * M_PI; @@ -102,9 +102,14 @@ __EXPORT int map_projection_init(struct map_projection_reference_s *ref, double return 0; } +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time()); +} + __EXPORT int map_projection_global_reference(double *ref_lat, double *ref_lon) { - return map_projection_reference_s(&mp_ref, ref_lat, ref_lon); + return map_projection_reference(&mp_ref, ref_lat, ref_lon); } __EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat, double *ref_lon) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 990f791948..3d7ba050f9 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -129,7 +129,18 @@ __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t tim * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp); +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, + double lat_0, double lon_0, uint64_t timestamp); + +/** + * Initializes the map transformation given by the argument and sets the timestamp to now. + * + * Initializes the transformation between the geographic coordinate system and + * the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0); /** * Transforms a point in the geographic coordinate system to the local diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b2c5e0079a..db8f00e8f7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -766,7 +766,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; - hil_local_pos.ref_timestamp = map_projection_timestamp(); + hil_local_pos.ref_timestamp = map_projection_global_timestamp(); hil_local_pos.ref_lat = lat; hil_local_pos.ref_lon = lon; hil_local_pos.ref_alt = _hil_local_alt0; @@ -774,7 +774,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) float x; float y; - map_projection_project(lat, lon, &x, &y); + map_projection_global_project(lat, lon, &x, &y); hil_local_pos.timestamp = timestamp; hil_local_pos.xy_valid = true; hil_local_pos.z_valid = true; From 5a868751b58ebb6b169beafd6258cd8874440483 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:40:04 +0200 Subject: [PATCH 23/65] navigator: update mapprojection usage --- src/modules/navigator/navigator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 2f6bf49f3a..14c568c46d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -823,8 +823,8 @@ Navigator::task_main() _global_pos_valid = _vstatus.condition_global_position_valid; /* set reference for map _projection if global pos is valid and home position is valid and we have not done so already */ - if (!map_projection_initialized() && _global_pos_valid && _home_pos.valid) { - map_projection_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp); + if (!map_projection_global_initialized() && _global_pos_valid && _home_pos.valid) { + map_projection_global_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp); } /* publish position setpoint triplet if updated */ From 53d23c67d72a6395cef509817df25264f05fbd85 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:40:34 +0200 Subject: [PATCH 24/65] mc pos ctrl: revert to local map projection --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index bf7b6a19cd..6b797f222b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -468,15 +468,17 @@ MulticopterPositionControl::update_ref() if (_ref_timestamp != 0) { /* calculate current position setpoint in global frame */ - map_projection_reproject(_pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); + map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); alt_sp = _ref_alt - _pos_sp(2); } + /* update local projection reference */ + map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); _ref_alt = _local_pos.ref_alt; if (_ref_timestamp != 0) { /* reproject position setpoint to new reference */ - map_projection_project(lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); + map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); _pos_sp(2) = -(alt_sp - _ref_alt); } @@ -675,7 +677,8 @@ MulticopterPositionControl::task_main() _reset_alt_sp = true; /* project setpoint to local frame */ - map_projection_project(_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, + map_projection_project(&_ref_pos, + _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, &_pos_sp.data[0], &_pos_sp.data[1]); _pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); From 510678bdae82a04151b6d5dcd5b02bee6f96abfd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:40:54 +0200 Subject: [PATCH 25/65] pos estimator inav: revert to local map projection --- .../position_estimator_inav_main.c | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 91642e5b92..368424853c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -549,7 +549,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(home_position), home_position_sub, &home); - if (home.timestamp != home_timestamp && map_projection_initialized()) { + if (home.timestamp != home_timestamp) { home_timestamp = home.timestamp; double est_lat, est_lon; @@ -558,9 +558,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ref_inited) { /* calculate current estimated position in global frame */ est_alt = local_pos.ref_alt - local_pos.z; - map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); + map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); } + /* update reference */ + map_projection_init(&ref, home.lat, home.lon); + /* update baro offset */ baro_offset += home.alt - local_pos.ref_alt; @@ -571,7 +574,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ref_inited) { /* reproject position estimate with new reference */ - map_projection_project(est_lat, est_lon, &x_est[0], &y_est[0]); + map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]); z_est[0] = -(est_alt - local_pos.ref_alt); } @@ -602,7 +605,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (gps_valid && map_projection_initialized()) { + if (gps_valid) { double lat = gps.lat * 1e-7; double lon = gps.lon * 1e-7; float alt = gps.alt * 1e-3; @@ -626,17 +629,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) z_est[0] = 0.0f; y_est[2] = accel_NED[1]; - map_projection_reference(&local_pos.ref_lat, &local_pos.ref_lon); - + local_pos.ref_lat = lat; + local_pos.ref_lon = lon; local_pos.ref_alt = alt; local_pos.ref_timestamp = t; + + /* initialize projection */ + map_projection_init(&ref, lat, lon); + warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); } } if (ref_inited) { /* project GPS lat lon to plane */ float gps_proj[2]; - map_projection_project(lat, lon, &(gps_proj[0]), &(gps_proj[1])); + map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); /* reset position estimate when GPS becomes good */ if (reset_est) { @@ -930,7 +938,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.time_gps_usec = gps.time_gps_usec; double est_lat, est_lon; - map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); + map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); global_pos.lat = est_lat; global_pos.lon = est_lon; From 606f3cba5c932d6aba0e383e915b02b78009fd9f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:41:12 +0200 Subject: [PATCH 26/65] pos estimator mc: revert to local map projection --- .../position_estimator_mc/position_estimator_mc_main.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c index ee6457aded..3639618193 100755 --- a/src/modules/position_estimator_mc/position_estimator_mc_main.c +++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c @@ -322,6 +322,11 @@ int position_estimator_mc_thread_main(int argc, char *argv[]) lon_current = ((double)(gps.lon)) * 1e-7d; alt_current = gps.alt * 1e-3f; gps_origin_altitude = alt_current; + /* initialize coordinates */ + map_projection_init(lat_current, lon_current); + /* publish global position messages only after first GPS message */ + printf("[pos_est_mc] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); + } else { mavlink_log_info(mavlink_fd, "[pos_est_mc] I'm NOT using GPS - I use VICON"); /* onboard calculated position estimations */ From 60ccbaa8bb59f4d510b74f7599ef60ef45837180 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 May 2014 15:37:53 +0200 Subject: [PATCH 27/65] init global map reference in commander and not in navigator --- src/modules/commander/commander.cpp | 14 ++++++++++++-- src/modules/navigator/navigator_main.cpp | 5 ----- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2db30b0664..54834cf9bb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -87,6 +87,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" @@ -366,7 +367,7 @@ static orb_advert_t status_pub; transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; - + // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); @@ -375,7 +376,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armed } else if (arming_res == TRANSITION_DENIED) { tune_negative(true); } - + return arming_res; } @@ -571,6 +572,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe home->valid = true; + /* set reference for map _projection */ + map_projection_global_init(home->lat, home->lon, hrt_absolute_time()); + /* announce new home position */ if (*home_pub > 0) { orb_publish(ORB_ID(home_position), *home_pub, home); @@ -958,6 +962,9 @@ int commander_thread_main(int argc, char *argv[]) warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + /* set reference for map _projection */ + map_projection_global_init(home.lat, home.lon, hrt_absolute_time()); + /* announce new home position */ if (home_pub > 0) { orb_publish(ORB_ID(home_position), home_pub, &home); @@ -1345,6 +1352,9 @@ int commander_thread_main(int argc, char *argv[]) warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + /* set reference for map _projection */ + map_projection_global_init(home.lat, home.lon, hrt_absolute_time()); + /* announce new home position */ if (home_pub > 0) { orb_publish(ORB_ID(home_position), home_pub, &home); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 14c568c46d..12fd35a0a9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -822,11 +822,6 @@ Navigator::task_main() _global_pos_valid = _vstatus.condition_global_position_valid; - /* set reference for map _projection if global pos is valid and home position is valid and we have not done so already */ - if (!map_projection_global_initialized() && _global_pos_valid && _home_pos.valid) { - map_projection_global_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp); - } - /* publish position setpoint triplet if updated */ if (_pos_sp_triplet_updated) { _pos_sp_triplet_updated = false; From 67e3c808d24773c7c4bd4608866de1edb617cb04 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 May 2014 15:40:02 +0200 Subject: [PATCH 28/65] remove home position valid flag --- src/modules/commander/commander.cpp | 4 ---- src/modules/uORB/topics/home_position.h | 1 - 2 files changed, 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 54834cf9bb..bc3bdb7822 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -570,8 +570,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); - home->valid = true; - /* set reference for map _projection */ map_projection_global_init(home->lat, home->lon, hrt_absolute_time()); @@ -957,7 +955,6 @@ int commander_thread_main(int argc, char *argv[]) home.lat = global_position.lat; home.lon = global_position.lon; home.alt = global_position.alt; - home.valid = true; warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); @@ -1347,7 +1344,6 @@ int commander_thread_main(int argc, char *argv[]) home.lat = global_position.lat; home.lon = global_position.lon; home.alt = global_position.alt; - home.valid = true; warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 73c8dc50d1..08d11abaea 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -63,7 +63,6 @@ struct home_position_s double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float alt; /**< Altitude in meters */ - bool valid; /**< True if home position is valid */ }; /** From 6e0690fde1073649a4ef201671ca947cb83edc37 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 May 2014 16:42:52 +0200 Subject: [PATCH 29/65] init global map projection when gps is valid --- src/lib/geo/geo.c | 2 +- src/modules/commander/commander.cpp | 18 +++++++++--------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 438e354dff..10a518908d 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -79,7 +79,7 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - if (strcmp("navigator", getprogname() == 0)) { + if (strcmp("commander", getprogname() == 0)) { return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); } else { diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bc3bdb7822..f8f215cd7f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -570,9 +570,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); - /* set reference for map _projection */ - map_projection_global_init(home->lat, home->lon, hrt_absolute_time()); - /* announce new home position */ if (*home_pub > 0) { orb_publish(ORB_ID(home_position), *home_pub, home); @@ -959,9 +956,6 @@ int commander_thread_main(int argc, char *argv[]) warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); - /* set reference for map _projection */ - map_projection_global_init(home.lat, home.lon, hrt_absolute_time()); - /* announce new home position */ if (home_pub > 0) { orb_publish(ORB_ID(home_position), home_pub, &home); @@ -1130,6 +1124,15 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); } + /* Initialize map projection if gps is valid */ + if (!map_projection_global_initialized() + && (gps_position.eph < eph_epv_threshold) + && (gps_position.epv < eph_epv_threshold)) { + /* set reference for map _projection */ + map_projection_global_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, hrt_absolute_time()); + + } + /* start RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ @@ -1348,9 +1351,6 @@ int commander_thread_main(int argc, char *argv[]) warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); - /* set reference for map _projection */ - map_projection_global_init(home.lat, home.lon, hrt_absolute_time()); - /* announce new home position */ if (home_pub > 0) { orb_publish(ORB_ID(home_position), home_pub, &home); From 7ab428976e703284037eb2f24f68ba105db9d6b8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 May 2014 16:49:21 +0200 Subject: [PATCH 30/65] fix wrong variable name --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f8f215cd7f..c15651ed54 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1126,8 +1126,8 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize map projection if gps is valid */ if (!map_projection_global_initialized() - && (gps_position.eph < eph_epv_threshold) - && (gps_position.epv < eph_epv_threshold)) { + && (gps_position.eph_m < eph_epv_threshold) + && (gps_position.epv_m < eph_epv_threshold)) { /* set reference for map _projection */ map_projection_global_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, hrt_absolute_time()); From f24a6187b60f5b9890d9645881b3642be420f5d6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 11:16:15 +0200 Subject: [PATCH 31/65] geo: map projection: fix variable name to highlight the unit --- src/lib/geo/geo.c | 30 ++++++++++++++---------------- src/lib/geo/geo.h | 8 ++++---- 2 files changed, 18 insertions(+), 20 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 10a518908d..ff07014aa6 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -80,7 +80,6 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { if (strcmp("commander", getprogname() == 0)) { - return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); } else { return -1; @@ -90,11 +89,10 @@ __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t tim __EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - ref->lat = lat_0 / 180.0 * M_PI; - ref->lon = lon_0 / 180.0 * M_PI; - - ref->sin_lat = sin(ref->lat); - ref->cos_lat = cos(ref->lat); + ref->lat_rad = lat_0 * M_DEG_TO_RAD; + ref->lon_rad = lon_0 * M_DEG_TO_RAD; + ref->sin_lat = sin(ref->lat_rad); + ref->cos_lat = cos(ref->lat_rad); ref->timestamp = timestamp; ref->init_done = true; @@ -107,19 +105,19 @@ __EXPORT int map_projection_init(struct map_projection_reference_s *ref, double map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time()); } -__EXPORT int map_projection_global_reference(double *ref_lat, double *ref_lon) +__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad) { - return map_projection_reference(&mp_ref, ref_lat, ref_lon); + return map_projection_reference(&mp_ref, ref_lat_rad, ref_lon_rad); } -__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat, double *ref_lon) +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad) { if (!map_projection_initialized(ref)) { return -1; } - *ref_lat = ref->lat; - *ref_lon = ref->lon; + *ref_lat_rad = ref->lat_rad; + *ref_lon_rad = ref->lon_rad; return 0; } @@ -141,13 +139,13 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref double sin_lat = sin(lat_rad); double cos_lat = cos(lat_rad); - double cos_d_lon = cos(lon_rad - ref->lon); + double cos_d_lon = cos(lon_rad - ref->lon_rad); double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon); double k = (c == 0.0) ? 1.0 : (c / sin(c)); *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; - *y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH; + *y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH; return 0; } @@ -174,11 +172,11 @@ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *r if (c != 0.0) { lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c); - lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); + lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); } else { - lat_rad = ref->lat; - lon_rad = ref->lon; + lat_rad = ref->lat_rad; + lon_rad = ref->lon_rad; } *lat = lat_rad * 180.0 / M_PI; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 1d0cd524a7..20fb34ca35 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -69,8 +69,8 @@ struct crosstrack_error_s { /* lat/lon are in radians */ struct map_projection_reference_s { - double lat; - double lon; + double lat_rad; + double lon_rad; double sin_lat; double cos_lat; bool init_done; @@ -105,13 +105,13 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference * Writes the reference values of the global projection to ref_lat and ref_lon * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_global_reference(double *ref_lat, double *ref_lon); +__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad); /** * Writes the reference values of the projection given by the argument to ref_lat and ref_lon * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat, double *ref_lon); +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad); /** * Initializes the global map transformation. From 596b06ff2e13b170545b4f13da1c64e66088aedc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 11:22:18 +0200 Subject: [PATCH 32/65] commander: init gps eph and epv to large values, safer map projection initialization --- src/modules/commander/commander.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c15651ed54..71de33bccc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -58,6 +58,7 @@ #include #include #include +#include #include #include @@ -788,6 +789,8 @@ int commander_thread_main(int argc, char *argv[]) int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); struct vehicle_gps_position_s gps_position; memset(&gps_position, 0, sizeof(gps_position)); + gps_position.eph_m = FLT_MAX; + gps_position.epv_m = FLT_MAX; /* Subscribe to sensor topic */ int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -1127,7 +1130,8 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize map projection if gps is valid */ if (!map_projection_global_initialized() && (gps_position.eph_m < eph_epv_threshold) - && (gps_position.epv_m < eph_epv_threshold)) { + && (gps_position.epv_m < eph_epv_threshold) + && hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) { /* set reference for map _projection */ map_projection_global_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, hrt_absolute_time()); From fc204a18902b5d623fff1e541a3212502295ed82 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 13:13:35 +0200 Subject: [PATCH 33/65] geo: map projection: fix stupid typo and use constants for deg to rad conversion --- src/lib/geo/geo.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index ff07014aa6..220ea149d3 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -134,8 +134,8 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref return -1; } - double lat_rad = lat / 180.0 * M_PI; - double lon_rad = lon / 180.0 * M_PI; + double lat_rad = lat * M_DEG_TO_RAD; + double lon_rad = lon * M_DEG_TO_RAD; double sin_lat = sin(lat_rad); double cos_lat = cos(lat_rad); @@ -152,7 +152,7 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref __EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon) { - map_projection_project(&mp_ref, x, y, lat, lon); + map_projection_reproject(&mp_ref, x, y, lat, lon); } __EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) From 548c7f4aaf93bddeb05053cd4dede945fede22ef Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 14:43:23 +0200 Subject: [PATCH 34/65] geo: introduce global/local coordinate frame converter which uses the map projection but also converts altitude --- src/lib/geo/geo.c | 36 +++++++++++++++++++++++++++++ src/lib/geo/geo.h | 26 +++++++++++++++++++++ src/modules/commander/commander.cpp | 5 ++-- 3 files changed, 64 insertions(+), 3 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 220ea149d3..c9d48491ee 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -56,6 +56,7 @@ */ static struct map_projection_reference_s mp_ref = {0}; +static struct globallocal_converter_reference_s gl_ref = {0}; __EXPORT bool map_projection_global_initialized() { @@ -185,6 +186,41 @@ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *r return 0; } +__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp) +{ + if (strcmp("commander", getprogname() == 0)) { + gl_ref.alt = alt_0; + gl_ref.init_done = true; + return map_projection_global_init(lat_0, lon_0, timestamp); + } else { + return -1; + } +} + +__EXPORT bool globallocalconverter_initialized() +{ + return gl_ref.init_done && map_projection_global_initialized(); +} + +__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z) +{ + if (!map_projection_global_initialized()) { + return -1; + } + + map_projection_global_project(lat, lon, x, y); + *z = gl_ref.alt - alt; +} + +__EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt) +{ + if (!map_projection_global_initialized()) { + return -1; + } + + map_projection_global_reproject(x, y, lat, lon); + *alt = gl_ref.alt - z; +} __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 20fb34ca35..98dfbd14b9 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -77,6 +77,11 @@ struct map_projection_reference_s { uint64_t timestamp; }; +struct globallocal_converter_reference_s { + float alt; + bool init_done; +}; + /** * Checks if global projection was initialized * @return true if map was initialized before, false else @@ -190,6 +195,27 @@ __EXPORT int map_projection_global_reproject(float x, float y, double *lat, doub */ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); +/** + * Initialize the global mapping between global position (spherical) and local position (NED). + */ +__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp); + +/** + * Checks if globallocalconverter was initialized + * @return true if map was initialized before, false else + */ +__EXPORT bool globallocalconverter_initialized(); + +/** + * Convert from global position coordinates to local position coordinates using the global reference + */ +__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z); + +/** + * Convert from local position coordinates to global position coordinates using the global reference + */ +__EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt); + /** * Returns the distance to the next waypoint in meters. * diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 71de33bccc..98887a0e44 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1132,9 +1132,8 @@ int commander_thread_main(int argc, char *argv[]) && (gps_position.eph_m < eph_epv_threshold) && (gps_position.epv_m < eph_epv_threshold) && hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) { - /* set reference for map _projection */ - map_projection_global_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, hrt_absolute_time()); - + /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ + globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); } /* start RC input check */ From a6a2efb651d875171aaec698a836f26b548ca0d6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 16:59:09 +0200 Subject: [PATCH 35/65] mavlink receiver: switch back to use local and not systemwide map projection --- src/modules/mavlink/mavlink_receiver.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1cfbde1d29..7c93c1c009 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -778,7 +778,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; - hil_local_pos.ref_timestamp = map_projection_global_timestamp(); + map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); + hil_local_pos.ref_timestamp = timestamp; hil_local_pos.ref_lat = lat; hil_local_pos.ref_lon = lon; hil_local_pos.ref_alt = _hil_local_alt0; @@ -786,7 +787,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) float x; float y; - map_projection_global_project(lat, lon, &x, &y); + map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y); hil_local_pos.timestamp = timestamp; hil_local_pos.xy_valid = true; hil_local_pos.z_valid = true; From e0042ec12cdccd157d181d5b9410fbeedfe3ce72 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 22 May 2014 13:15:29 +0200 Subject: [PATCH 36/65] geo: add functions to get global projection/transformation reference values --- src/lib/geo/geo.c | 39 +++++++++++++++++++++++++++++++++++++++ src/lib/geo/geo.h | 10 ++++++++++ 2 files changed, 49 insertions(+) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index c9d48491ee..b1e0383233 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -186,6 +186,23 @@ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *r return 0; } +__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0) +{ + if (!map_projection_global_initialized()) { + return -1; + } + + if (lat_0 != NULL) { + *lat_0 = M_RAD_TO_DEG * mp_ref.lat_rad; + } + + if (lon_0 != NULL) { + *lon_0 = M_RAD_TO_DEG * mp_ref.lon_rad; + } + + return 0; + +} __EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp) { if (strcmp("commander", getprogname() == 0)) { @@ -210,6 +227,8 @@ __EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, flo map_projection_global_project(lat, lon, x, y); *z = gl_ref.alt - alt; + + return 0; } __EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt) @@ -220,6 +239,26 @@ __EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *l map_projection_global_reproject(x, y, lat, lon); *alt = gl_ref.alt - z; + + return 0; +} + +__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0) +{ + if (!map_projection_global_initialized()) { + return -1; + } + + if (map_projection_global_getref(lat_0, lon_0)) + { + return -1; + } + + if (alt_0 != NULL) { + *alt_0 = gl_ref.alt; + } + + return 0; } __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 98dfbd14b9..4bc3cc17c3 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -195,6 +195,11 @@ __EXPORT int map_projection_global_reproject(float x, float y, double *lat, doub */ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); +/** + * Get reference position of the global map projection + */ +__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0); + /** * Initialize the global mapping between global position (spherical) and local position (NED). */ @@ -216,6 +221,11 @@ __EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, flo */ __EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt); +/** + * Get reference position of the global to local converter + */ +__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0); + /** * Returns the distance to the next waypoint in meters. * From a43e963bdb73e6fa0480d442e72e1d764418cad4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 30 May 2014 16:09:05 +0200 Subject: [PATCH 37/65] geo: fix logic of globallocal converter initialization to depend on map projection initialization --- src/lib/geo/geo.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index b1e0383233..235da5b394 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -207,8 +207,14 @@ __EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, { if (strcmp("commander", getprogname() == 0)) { gl_ref.alt = alt_0; - gl_ref.init_done = true; - return map_projection_global_init(lat_0, lon_0, timestamp); + if (!map_projection_global_init(lat_0, lon_0, timestamp)) + { + gl_ref.init_done = true; + return 0; + } else { + gl_ref.init_done = false; + return -1; + } } else { return -1; } From cb246a79cad96c469f29042dd33658d63cbcc743 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Jun 2014 16:47:16 +0200 Subject: [PATCH 38/65] Added vision position estimate topic --- src/modules/uORB/objects_common.cpp | 3 + .../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 90675bb2e9..fc9920a418 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 0000000000..74c96cf2e0 --- /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_ */ From 7a776eacb11f30a007812449a02916c6f00d2ad0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Jun 2014 16:50:09 +0200 Subject: [PATCH 39/65] 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 53769e0cfe..6b73a4bfa2 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 9ab84b58a3..b5e6b1b1a3 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; From 57a7e764068178e365b05b7baaa39041762f2e96 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Jun 2014 16:51:09 +0200 Subject: [PATCH 40/65] 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 d7503e42da..9ab21ee38c 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 8aa08b6f2c..c7c1c070ce 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 08ec996a13..b7826793aa 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 * From 78b7ba33a5f04a11ac859aecfb72ff34d75cb7ff Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Sun, 22 Jun 2014 09:53:33 +0800 Subject: [PATCH 41/65] Fix to allow filter correction with vision position estimate --- .../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 9ab21ee38c..a6a4db3eb2 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); From 34c13df3dd6edb68921b9cdc34f2d87ac4251e12 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 09:50:48 +0200 Subject: [PATCH 42/65] geo: Fix a series of warnings, some which actually pointed to real issues --- src/lib/geo/geo.c | 23 +++++++++++------------ src/lib/geo/geo.h | 6 +++--- 2 files changed, 14 insertions(+), 15 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index e03ec65c4d..36be32158f 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -49,14 +49,18 @@ #include #include #include +#include + +#include +#include /* * Azimuthal Equidistant Projection * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ -static struct map_projection_reference_s mp_ref = {0}; -static struct globallocal_converter_reference_s gl_ref = {0}; +static struct map_projection_reference_s mp_ref = {0.0, 0.0, 0.0, 0.0, false, 0}; +static struct globallocal_converter_reference_s gl_ref = {0.0f, false}; __EXPORT bool map_projection_global_initialized() { @@ -80,7 +84,7 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - if (strcmp("commander", getprogname() == 0)) { + if (strcmp("commander", getprogname()) == 0) { return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); } else { return -1; @@ -103,7 +107,7 @@ __EXPORT int map_projection_init_timestamped(struct map_projection_reference_s * __EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time()); + return map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time()); } __EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad) @@ -153,22 +157,17 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref __EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon) { - map_projection_reproject(&mp_ref, x, y, lat, lon); + return map_projection_reproject(&mp_ref, x, y, lat, lon); } __EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) { -<<<<<<< HEAD if (!map_projection_initialized(ref)) { return -1; } - float x_rad = x / CONSTANTS_RADIUS_OF_EARTH; - float y_rad = y / CONSTANTS_RADIUS_OF_EARTH; -======= double x_rad = x / CONSTANTS_RADIUS_OF_EARTH; double y_rad = y / CONSTANTS_RADIUS_OF_EARTH; ->>>>>>> 48f4a1e5cd6ef653b466eb68c1073fb47cbefbd7 double c = sqrtf(x_rad * x_rad + y_rad * y_rad); double sin_c = sin(c); double cos_c = cos(c); @@ -210,7 +209,7 @@ __EXPORT int map_projection_global_getref(double *lat_0, double *lon_0) } __EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp) { - if (strcmp("commander", getprogname() == 0)) { + if (strcmp("commander", getprogname()) == 0) { gl_ref.alt = alt_0; if (!map_projection_global_init(lat_0, lon_0, timestamp)) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index fbcfe1c2dc..2311e0a7c9 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -86,7 +86,7 @@ struct globallocal_converter_reference_s { * Checks if global projection was initialized * @return true if map was initialized before, false else */ -__EXPORT bool map_projection_global_initialized(); +__EXPORT bool map_projection_global_initialized(void); /** * Checks if projection given as argument was initialized @@ -98,7 +98,7 @@ __EXPORT bool map_projection_initialized(const struct map_projection_reference_s * Get the timestamp of the global map projection * @return the timestamp of the map_projection */ -__EXPORT uint64_t map_projection_global_timestamp(); +__EXPORT uint64_t map_projection_global_timestamp(void); /** * Get the timestamp of the map projection given by the argument @@ -209,7 +209,7 @@ __EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, * Checks if globallocalconverter was initialized * @return true if map was initialized before, false else */ -__EXPORT bool globallocalconverter_initialized(); +__EXPORT bool globallocalconverter_initialized(void); /** * Convert from global position coordinates to local position coordinates using the global reference From 801d1d31983d404d5ebe8f5750359f2d8c7fdf43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 09:51:35 +0200 Subject: [PATCH 43/65] commander: Update after merge --- src/modules/commander/commander.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 326a3e1f35..d7425cbb47 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -826,8 +826,8 @@ int commander_thread_main(int argc, char *argv[]) int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); struct vehicle_gps_position_s gps_position; memset(&gps_position, 0, sizeof(gps_position)); - gps_position.eph_m = FLT_MAX; - gps_position.epv_m = FLT_MAX; + gps_position.eph = FLT_MAX; + gps_position.epv = FLT_MAX; /* Subscribe to sensor topic */ int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -1261,8 +1261,8 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize map projection if gps is valid */ if (!map_projection_global_initialized() - && (gps_position.eph_m < eph_epv_threshold) - && (gps_position.epv_m < eph_epv_threshold) + && (gps_position.eph < eph_epv_threshold) + && (gps_position.epv < eph_epv_threshold) && hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) { /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); From a063f58e006689bee2e28d85930cba11245b9597 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Tue, 22 Jul 2014 12:36:28 +0800 Subject: [PATCH 44/65] Add vision weight parameters to structure --- .../position_estimator_inav_params.c | 6 ++++++ .../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 0d9fbef278..36e5567566 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 08098d6c30..a64205c1f5 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; From dd0e39972fb5a8e4b881e71d8bdb53e1ce82e380 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Tue, 22 Jul 2014 14:00:49 +0800 Subject: [PATCH 45/65] Add weight parameter for vision velocity --- .../position_estimator_inav/position_estimator_inav_params.c | 3 +++ .../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 36e5567566..1867f65dad 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 a64205c1f5..d0a65e42ea 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; From ef74f4ad8920b59a80158ac8820ed497c9dbbe13 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Tue, 22 Jul 2014 20:48:18 +0800 Subject: [PATCH 46/65] Shorten vision parameter name --- .../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 1867f65dad..90d8d2b502 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"); From dfbbc6600e67618e030c366dca5b440dcd0ddcb7 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Tue, 22 Jul 2014 21:01:10 +0800 Subject: [PATCH 47/65] 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 71abcc1704..398fddd692 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 { From a48bede96f79c626e17584f75865ef7839744bf0 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Wed, 23 Jul 2014 14:24:51 +0800 Subject: [PATCH 48/65] Remove unused commented code --- .../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 398fddd692..77e94a27c4 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); - } } From 38d3efa985be394c846e6afe294006ab074335f7 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Wed, 23 Jul 2014 15:12:26 +0800 Subject: [PATCH 49/65] Correct vision velocity estimate correction type --- .../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 77e94a27c4..d3192fec21 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); } } From 25ef4bc4a0557327af1f32d81c86e8981772a844 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Thu, 24 Jul 2014 22:31:45 +0800 Subject: [PATCH 50/65] Use current rotation matrix for vision instead of delayed rotation --- .../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 d3192fec21..06a81e3fee 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)) { From ee42d5f53d81580bee56b1cdcf69ebfdefb6ba09 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Jul 2014 12:15:33 +0200 Subject: [PATCH 51/65] Comment fix --- .../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 dff2768c77..cc0fb4155e 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 From 89986cf3e5cc8a9e18d56fe971d3e089232e3beb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 11 Aug 2014 13:46:13 +0200 Subject: [PATCH 52/65] epv/eph threshold variable names changed --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 02571f56fb..6c2c030702 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1341,8 +1341,8 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize map projection if gps is valid */ if (!map_projection_global_initialized() - && (gps_position.eph < eph_epv_threshold) - && (gps_position.epv < eph_epv_threshold) + && (gps_position.eph < eph_threshold) + && (gps_position.epv < epv_threshold) && hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) { /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); From 258fb27e146ad175ca89a84ebc9c340a53db0884 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 14:54:18 +0200 Subject: [PATCH 53/65] Ensure that a failing dataman start does not abort boot --- ROMFS/px4fmu_common/init.d/rcS | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 24b2a299af..72cd4b0e1e 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -280,9 +280,11 @@ then nshterm /dev/ttyACM0 & # - # Start the datamanager + # Start the datamanager (and do not abort boot if it fails) # - dataman start + if dataman start + then + fi # # Start the Commander (needs to be this early for in-air-restarts) From 2ca89032409d0519e242006fa34ab26548fdd7e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 14:56:13 +0200 Subject: [PATCH 54/65] If we failed loading params, reset them (if the file handle is valid the file is corrupted and there is no hope, if its just not present the reset command will not do anything) --- ROMFS/px4fmu_common/init.d/rcS | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 72cd4b0e1e..c9e6a27cac 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -83,9 +83,12 @@ then param select $PARAM_FILE if param load then - echo "[init] Params loaded: $PARAM_FILE" + echo "[param] Loaded: $PARAM_FILE" else - echo "[init] ERROR: Params loading failed: $PARAM_FILE" + echo "[param] FAILED loading $PARAM_FILE" + if param reset + then + fi fi # From 5baf9cea0d8fcf6e0c6b3c92b26167067e9cb0fa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 11 Aug 2014 15:29:59 +0200 Subject: [PATCH 55/65] geo: fix some warnings --- src/lib/geo/geo.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index ad1ff37e30..1c8d2a2a75 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -50,6 +50,7 @@ #include #include #include +#include #include #include @@ -147,7 +148,7 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref double cos_d_lon = cos(lon_rad - ref->lon_rad); double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon); - double k = (c == 0.0) ? 1.0 : (c / sin(c)); + double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c)); *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; *y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH; @@ -175,7 +176,7 @@ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *r double lat_rad; double lon_rad; - if (c != 0.0) { + if (fabs(c) > DBL_EPSILON) { lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c); lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); From 7d1cd7b994240d441a37b36f74598980f3f460db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 19:28:14 +0200 Subject: [PATCH 56/65] Fix NO RC handling (fixes #1263) --- src/drivers/px4io/px4io.cpp | 36 +++++++++++++++++++++++++++++------- 1 file changed, 29 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d93009c47d..bc0e6a221c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -135,6 +135,15 @@ public: */ virtual int init(); + /** + * Initialize the PX4IO class. + * + * Retrieve relevant initial system parameters. Initialize PX4IO registers. + * + * @param disable_rc_handling set to true to forbid override / RC handling on IO + */ + virtual int init(bool disable_rc_handling); + /** * Detect if a PX4IO is connected. * @@ -579,6 +588,12 @@ PX4IO::detect() return 0; } +int +PX4IO::init(bool rc_handling_disabled) { + _rc_handling_disabled = rc_handling_disabled; + init(); +} + int PX4IO::init() { @@ -778,6 +793,11 @@ PX4IO::init() if (_rc_handling_disabled) { ret = io_disable_rc_handling(); + if (ret != OK) { + log("failed disabling RC handling"); + return ret; + } + } else { /* publish RC config to IO */ ret = io_set_rc_config(); @@ -1175,6 +1195,7 @@ PX4IO::io_set_arming_state() int PX4IO::disable_rc_handling() { + _rc_handling_disabled = true; return io_disable_rc_handling(); } @@ -2613,24 +2634,25 @@ start(int argc, char *argv[]) errx(1, "driver alloc failed"); } - if (OK != g_dev->init()) { - delete g_dev; - g_dev = nullptr; - errx(1, "driver init failed"); - } + bool rc_handling_disabled = false; /* disable RC handling on request */ if (argc > 1) { if (!strcmp(argv[1], "norc")) { - if (g_dev->disable_rc_handling()) - warnx("Failed disabling RC handling"); + rc_handling_disabled = true; } else { warnx("unknown argument: %s", argv[1]); } } + if (OK != g_dev->init(rc_handling_disabled)) { + delete g_dev; + g_dev = nullptr; + errx(1, "driver init failed"); + } + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 int dsm_vcc_ctl; From 0c0c8e943a2ec70a91386b65c17c563b6e40a782 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Aug 2014 07:30:01 +0200 Subject: [PATCH 57/65] Fixed missing return value after last change set --- src/drivers/px4io/px4io.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index bc0e6a221c..32069cf09b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -142,7 +142,7 @@ public: * * @param disable_rc_handling set to true to forbid override / RC handling on IO */ - virtual int init(bool disable_rc_handling); + int init(bool disable_rc_handling); /** * Detect if a PX4IO is connected. @@ -591,7 +591,7 @@ PX4IO::detect() int PX4IO::init(bool rc_handling_disabled) { _rc_handling_disabled = rc_handling_disabled; - init(); + return init(); } int From 3f4aef60c88b1e570dd30bc47a13d5340073e9a9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Aug 2014 08:21:38 +0200 Subject: [PATCH 58/65] Increase timeout in an attempt to prevent timout python failure --- Tools/px_uploader.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index cd7884f6d2..d8f4884bc0 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -180,7 +180,7 @@ class uploader(object): def __init__(self, portname, baudrate): # open the port, keep the default timeout short so we can poll quickly - self.port = serial.Serial(portname, baudrate, timeout=0.5) + self.port = serial.Serial(portname, baudrate, timeout=2.0) self.otp = b'' self.sn = b'' From c8ecf59ab70780efed6ac48967caa8e2358a48ba Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 12 Aug 2014 14:23:26 -0700 Subject: [PATCH 59/65] 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 17d1babffe..00c8df18c5 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 800c98b69f..43de89de91 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]; } From 2cc5c6e84f2fed508a358d2b2fc9c38085c446db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Aug 2014 09:14:43 +0200 Subject: [PATCH 60/65] INAV: Add braces to ensure statements are evaluated correctly --- .../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 06a81e3fee..81bbaa6587 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; } From ae4339ce1e3046e44387f9c9e7a66827b859a653 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Aug 2014 09:42:41 +0200 Subject: [PATCH 61/65] 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 4cd384001b..4d7487c2bc 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 4cd384001b5373e1ecaa6c4cd66994855cec4742 +Subproject commit 4d7487c2bc5f5ccf87bca82970fb2c08d6d8bd50 From 54fc6aa6788a125b387926a45023844daa42ec48 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Aug 2014 10:33:46 +0200 Subject: [PATCH 62/65] 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 3b8553489d..70c75b10cb 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 f00b0f5926..5000790a59 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 990c567681..ce87eb3e2c 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 b3fceceb5c..1bc4f414ec 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 e2fa0ff804..7d2c59f913 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 13a24150f2..a51ac8e0cf 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 From 54a6e66adafb7cca710bab3dc9e9c78c8b2390e8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 13 Aug 2014 19:46:35 +0200 Subject: [PATCH 63/65] mavlink: in HIL_CONTROLS send 0 for disabled channels --- src/modules/mavlink/mavlink_messages.cpp | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c10be77b52..f292605c9b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1333,7 +1333,7 @@ protected: } for (unsigned i = 0; i < 8; i++) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + if (act.output[i] > PWM_LOWEST_MIN / 2) { if (i < n) { /* scale PWM out 900..2100 us to 0..1 for rotors */ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); @@ -1344,7 +1344,7 @@ protected: } } else { - /* send 0 when disarmed */ + /* send 0 when disarmed and for disabled channels */ out[i] = 0.0f; } } @@ -1353,15 +1353,20 @@ protected: /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ for (unsigned i = 0; i < 8; i++) { - if (i != 3) { - /* scale PWM out 900..2100 us to -1..1 for normal channels */ - out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + if (act.output[i] > PWM_LOWEST_MIN / 2) { + if (i != 3) { + /* scale PWM out 900..2100 us to -1..1 for normal channels */ + out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + + } else { + /* scale PWM out 900..2100 us to 0..1 for throttle */ + out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + } } else { - /* scale PWM out 900..2100 us to 0..1 for throttle */ - out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + /* set 0 for disabled channels */ + out[i] = 0.0f; } - } } From 73b4c6eba16d57550c4e745f99428b3580cbe4aa Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 13 Aug 2014 20:09:57 +0200 Subject: [PATCH 64/65] mavlink: stream names updated in configuration --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index f1c21f295d..ee040a706d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -23,7 +23,7 @@ mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5 usleep 100000 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 usleep 100000 -mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20 +mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10 usleep 100000 # Exit shell to make it available to MAVLink diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9a39d3bed2..4086059398 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1388,8 +1388,8 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GLOBAL_POSITION_INT", 3.0f); configure_stream("LOCAL_POSITION_NED", 3.0f); configure_stream("RC_CHANNELS_RAW", 1.0f); - configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f); - configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); + configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); break; From e03c4d4b65d956a90a9e95b35718ebc323ff7539 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 Aug 2014 12:34:47 +0200 Subject: [PATCH 65/65] Fixed copy & paste error on RC output --- src/modules/mavlink/mavlink_messages.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c10be77b52..0d0896353e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1637,10 +1637,10 @@ protected: msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX; msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX; msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX; + msg.chan5_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX; + msg.chan6_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX; + msg.chan7_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX; + msg.chan8_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX; msg.rssi = rc.rssi; _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg);