diff options
-rw-r--r-- | src/lib/geo/geo.c | 44 | ||||
-rw-r--r-- | src/lib/geo/geo.h | 9 | ||||
-rw-r--r-- | src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 6 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 4 | ||||
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 | ||||
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 20 |
6 files changed, 48 insertions, 44 deletions
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 9a24ff50e..32d52aadc 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -55,33 +55,47 @@ * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ -__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +struct map_projection_reference_s mp_ref; + +__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - ref->lat = lat_0 / 180.0 * M_PI; - ref->lon = lon_0 / 180.0 * M_PI; + mp_ref.lat = lat_0 / 180.0 * M_PI; + mp_ref.lon = lon_0 / 180.0 * M_PI; + + mp_ref.sin_lat = sin(mp_ref.lat); + mp_ref.cos_lat = cos(mp_ref.lat); - ref->sin_lat = sin(ref->lat); - ref->cos_lat = cos(ref->lat); + mp_ref.init_done = true; } -__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +__EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) { + if (!mp_ref.init_done) { + return false; + } + double lat_rad = lat / 180.0 * M_PI; double lon_rad = lon / 180.0 * M_PI; double sin_lat = sin(lat_rad); double cos_lat = cos(lat_rad); - double cos_d_lon = cos(lon_rad - ref->lon); + double cos_d_lon = cos(lon_rad - mp_ref.lon); - double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon); + double c = acos(mp_ref.sin_lat * sin_lat + mp_ref.cos_lat * cos_lat * cos_d_lon); double k = (c == 0.0) ? 1.0 : (c / sin(c)); - *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; - *y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH; + *x = k * (mp_ref.cos_lat * sin_lat - mp_ref.sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; + *y = k * cos_lat * sin(lon_rad - mp_ref.lon) * CONSTANTS_RADIUS_OF_EARTH; + + return true; } -__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +__EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon) { + if (!mp_ref.init_done) { + return false; + } + float x_rad = x / CONSTANTS_RADIUS_OF_EARTH; float y_rad = y / CONSTANTS_RADIUS_OF_EARTH; double c = sqrtf(x_rad * x_rad + y_rad * y_rad); @@ -92,12 +106,12 @@ __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, f double lon_rad; if (c != 0.0) { - lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c); - lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); + lat_rad = asin(cos_c * mp_ref.sin_lat + (x_rad * sin_c * mp_ref.cos_lat) / c); + lon_rad = (mp_ref.lon + atan2(y_rad * sin_c, c * mp_ref.cos_lat * cos_c - x_rad * mp_ref.sin_lat * sin_c)); } else { - lat_rad = ref->lat; - lon_rad = ref->lon; + lat_rad = mp_ref.lat; + lon_rad = mp_ref.lon; } *lat = lat_rad * 180.0 / M_PI; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 0a3f85d97..b48d26a66 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -71,6 +71,7 @@ struct map_projection_reference_s { double lon; double sin_lat; double cos_lat; + bool init_done; }; /** @@ -80,7 +81,7 @@ struct map_projection_reference_s { * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0); +__EXPORT void map_projection_init(double lat_0, double lon_0); /** * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane @@ -88,8 +89,9 @@ __EXPORT void map_projection_init(struct map_projection_reference_s *ref, double * @param y east * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) + * @return true if map_projection_init was called before, false else */ -__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y); +__EXPORT bool map_projection_project(double lat, double lon, float *x, float *y); /** * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system @@ -98,8 +100,9 @@ __EXPORT void map_projection_project(struct map_projection_reference_s *ref, dou * @param y east * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) + * @return true if map_projection_init was called before, false else */ -__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); +__EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon); /** * Returns the distance to the next waypoint in meters. diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 9d2d9ed5f..420a03644 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -177,8 +177,6 @@ private: struct sensor_combined_s _sensor_combined; #endif - struct map_projection_reference_s _pos_ref; - float _baro_ref; /**< barometer reference altitude */ float _baro_gps_offset; /**< offset between GPS and baro */ @@ -822,8 +820,6 @@ FixedwingEstimator::task_main() _ekf->baroHgt = _baro.altitude - _baro_ref; _baro_gps_offset = _baro_ref - _local_pos.ref_alt; - // XXX this is not multithreading safe - map_projection_init(&_pos_ref, lat, lon); mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); _gps_initialized = true; @@ -1046,7 +1042,7 @@ FixedwingEstimator::task_main() if (_local_pos.xy_global) { double est_lat, est_lon; - map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); + map_projection_reproject(_local_pos.x, _local_pos.y, &est_lat, &est_lon); _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_gps_usec = _gps.time_gps_usec; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index adafdd139..df61c3c15 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -795,7 +795,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; - map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); +// map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); //XXX fix reference update hil_local_pos.ref_timestamp = timestamp; hil_local_pos.ref_lat = lat; hil_local_pos.ref_lon = lon; @@ -804,7 +804,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) float x; float y; - map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y); + map_projection_project(lat, lon, &x, &y); hil_local_pos.timestamp = timestamp; hil_local_pos.xy_valid = true; hil_local_pos.z_valid = true; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index dc0aa172a..963349d42 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -478,17 +478,17 @@ MulticopterPositionControl::update_ref() if (_ref_timestamp != 0) { /* calculate current position setpoint in global frame */ - map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); + map_projection_reproject(_pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); alt_sp = _ref_alt - _pos_sp(2); } /* update local projection reference */ - map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); +// map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); //XXX fix reference update _ref_alt = _local_pos.ref_alt; if (_ref_timestamp != 0) { /* reproject position setpoint to new reference */ - map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); + map_projection_project(lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); _pos_sp(2) = -(alt_sp - _ref_alt); } @@ -687,8 +687,7 @@ MulticopterPositionControl::task_main() _reset_alt_sp = true; /* project setpoint to local frame */ - map_projection_project(&_ref_pos, - _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, + map_projection_project(_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, &_pos_sp.data[0], &_pos_sp.data[1]); _pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 368424853..b803dfb58 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -558,12 +558,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ref_inited) { /* calculate current estimated position in global frame */ est_alt = local_pos.ref_alt - local_pos.z; - map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); + map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); } - /* update reference */ - map_projection_init(&ref, home.lat, home.lon); - /* update baro offset */ baro_offset += home.alt - local_pos.ref_alt; @@ -572,9 +569,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.ref_alt = home.alt; local_pos.ref_timestamp = home.timestamp; - if (ref_inited) { + if (ref_inited) { //XXX fix reference update /* reproject position estimate with new reference */ - map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]); + map_projection_project(est_lat, est_lon, &x_est[0], &y_est[0]); z_est[0] = -(est_alt - local_pos.ref_alt); } @@ -633,18 +630,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.ref_lon = lon; local_pos.ref_alt = alt; local_pos.ref_timestamp = t; - - /* initialize projection */ - map_projection_init(&ref, lat, lon); - warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); } } - if (ref_inited) { + if (ref_inited) { //XXX fix reference update /* project GPS lat lon to plane */ float gps_proj[2]; - map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); + map_projection_project(lat, lon, &(gps_proj[0]), &(gps_proj[1])); /* reset position estimate when GPS becomes good */ if (reset_est) { @@ -938,7 +930,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.time_gps_usec = gps.time_gps_usec; double est_lat, est_lon; - map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); + map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); global_pos.lat = est_lat; global_pos.lon = est_lon; |