fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 09:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

towards a global map projection instance, WIP: need to remove local updates of the reference and add a global map update

Browse Source
This commit is contained in:
Thomas Gubler
2014-04-24 14:30:29 +02:00
parent 320c97c498
commit 3a898e54ad
6 changed files with 49 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/geo/geo.c
+30 -16
View File
@@ -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;
src/lib/geo/geo.h
+6 -3
View File
@@ -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.
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+1 -5
View File
@@ -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;
src/modules/mavlink/mavlink_receiver.cpp
+2 -2
View File
@@ -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;
src/modules/mc_pos_control/mc_pos_control_main.cpp
+4 -5
View File
@@ -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);
src/modules/position_estimator_inav/position_estimator_inav_main.c
+6 -14
View File
@@ -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;