diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/lib/geo/geo.c | 201 | ||||
-rw-r--r-- | src/lib/geo/geo.h | 141 | ||||
-rw-r--r-- | src/modules/commander/commander.cpp | 14 |
3 files changed, 330 insertions, 26 deletions
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index e600976ce..1c8d2a2a7 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,39 +49,124 @@ #include <stdio.h> #include <math.h> #include <stdbool.h> +#include <string.h> +#include <float.h> + +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> /* * Azimuthal Equidistant Projection * 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 +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() +{ + return map_projection_initialized(&mp_ref); +} + +__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref) +{ + return ref->init_done; +} + +__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) { - ref->lat = lat_0 / 180.0 * M_PI; - ref->lon = lon_0 / 180.0 * M_PI; + return ref->timestamp; +} - ref->sin_lat = sin(ref->lat); - ref->cos_lat = cos(ref->lat); +__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; + } } -__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +__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 { - double lat_rad = lat / 180.0 * M_PI; - double lon_rad = lon / 180.0 * M_PI; + + 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; + + 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 +{ + 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) +{ + 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_rad, double *ref_lon_rad) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + *ref_lat_rad = ref->lat_rad; + *ref_lon_rad = ref->lon_rad; + + 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; + } + + 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); - 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)); + 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) * CONSTANTS_RADIUS_OF_EARTH; + *y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH; + + return 0; +} + +__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon) +{ + return map_projection_reproject(&mp_ref, x, y, lat, lon); } -__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) { + if (!map_projection_initialized(ref)) { + return -1; + } + double x_rad = x / CONSTANTS_RADIUS_OF_EARTH; double y_rad = y / CONSTANTS_RADIUS_OF_EARTH; double c = sqrtf(x_rad * x_rad + y_rad * y_rad); @@ -91,19 +176,101 @@ __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, f 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 + 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; *lon = lon_rad * 180.0 / M_PI; + + 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) { + gl_ref.alt = alt_0; + 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; + } +} + +__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; + + return 0; +} + +__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; + + 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 8b286af36..2311e0a7c 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -69,39 +69,162 @@ 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; + 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 + */ +__EXPORT bool map_projection_global_initialized(void); + +/** + * 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_global_timestamp(void); + +/** + * 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_global_reference(double *ref_lat_rad, double *ref_lon_rad); + /** - * 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_rad, double *ref_lon_rad); + +/** + * Initializes the global map transformation. + * + * Initializes the transformation between the geographic coordinate system and + * the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp); + +/** + * Initializes the map transformation 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_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 + * 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 void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0); +__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 azimuthal equidistant plane + * Transforms a point in the geographic coordinate system to the local + * azimuthal equidistant plane using the global projection * @param x north * @param y east * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) + * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT void map_projection_project(struct map_projection_reference_s *ref, 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 using the global projection + * + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + * @return 0 if map_projection_init was called before, -1 else + */ +__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon); /** - * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system + * Transforms a point in the local azimuthal equidistant plane to the + * geographic coordinate system using the projection given by the argument * * @param x north * @param y east * @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); + +/** + * 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). + */ +__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(void); + +/** + * 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); + +/** + * Get reference position of the global to local converter */ -__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); +__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0); /** * Returns the distance to the next waypoint in meters. diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 4f976546e..6c2c03070 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -59,6 +59,7 @@ #include <string.h> #include <math.h> #include <poll.h> +#include <float.h> #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> @@ -92,6 +93,7 @@ #include <systemlib/err.h> #include <systemlib/cpuload.h> #include <systemlib/rc_check.h> +#include <geo/geo.h> #include <systemlib/state_table.h> #include <dataman/dataman.h> @@ -875,6 +877,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 = FLT_MAX; + gps_position.epv = FLT_MAX; /* Subscribe to sensor topic */ int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -1335,6 +1339,16 @@ 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_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()); + } + + /* start mission result check */ orb_check(mission_result_sub, &updated); if (updated) { |