aboutsummaryrefslogtreecommitdiff
path: root/src/lib/geo
diff options
context:
space:
mode:
authorThomas Gubler <thomasgubler@gmail.com>2014-05-06 11:16:15 +0200
committerThomas Gubler <thomasgubler@gmail.com>2014-05-06 11:24:33 +0200
commitf24a6187b60f5b9890d9645881b3642be420f5d6 (patch)
tree97c55c76749b929cd1ca11d49f330dc3b779b4ad /src/lib/geo
parent7ab428976e703284037eb2f24f68ba105db9d6b8 (diff)
downloadpx4-firmware-f24a6187b60f5b9890d9645881b3642be420f5d6.tar.gz
px4-firmware-f24a6187b60f5b9890d9645881b3642be420f5d6.tar.bz2
px4-firmware-f24a6187b60f5b9890d9645881b3642be420f5d6.zip
geo: map projection: fix variable name to highlight the unit
Diffstat (limited to 'src/lib/geo')
-rw-r--r--src/lib/geo/geo.c30
-rw-r--r--src/lib/geo/geo.h8
2 files changed, 18 insertions, 20 deletions
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 10a518908..ff07014aa 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -80,7 +80,6 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference
__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
if (strcmp("commander", getprogname() == 0)) {
-
return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp);
} else {
return -1;
@@ -90,11 +89,10 @@ __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t tim
__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
- ref->lat = lat_0 / 180.0 * M_PI;
- ref->lon = lon_0 / 180.0 * M_PI;
-
- ref->sin_lat = sin(ref->lat);
- ref->cos_lat = cos(ref->lat);
+ ref->lat_rad = lat_0 * M_DEG_TO_RAD;
+ ref->lon_rad = lon_0 * M_DEG_TO_RAD;
+ ref->sin_lat = sin(ref->lat_rad);
+ ref->cos_lat = cos(ref->lat_rad);
ref->timestamp = timestamp;
ref->init_done = true;
@@ -107,19 +105,19 @@ __EXPORT int map_projection_init(struct map_projection_reference_s *ref, double
map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time());
}
-__EXPORT int map_projection_global_reference(double *ref_lat, double *ref_lon)
+__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad)
{
- return map_projection_reference(&mp_ref, ref_lat, ref_lon);
+ return map_projection_reference(&mp_ref, ref_lat_rad, ref_lon_rad);
}
-__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat, double *ref_lon)
+__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad)
{
if (!map_projection_initialized(ref)) {
return -1;
}
- *ref_lat = ref->lat;
- *ref_lon = ref->lon;
+ *ref_lat_rad = ref->lat_rad;
+ *ref_lon_rad = ref->lon_rad;
return 0;
}
@@ -141,13 +139,13 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref
double sin_lat = sin(lat_rad);
double cos_lat = cos(lat_rad);
- double cos_d_lon = cos(lon_rad - ref->lon);
+ double cos_d_lon = cos(lon_rad - ref->lon_rad);
double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
double k = (c == 0.0) ? 1.0 : (c / sin(c));
*x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
- *y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH;
+ *y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH;
return 0;
}
@@ -174,11 +172,11 @@ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *r
if (c != 0.0) {
lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
- lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
+ lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
} else {
- lat_rad = ref->lat;
- lon_rad = ref->lon;
+ lat_rad = ref->lat_rad;
+ lon_rad = ref->lon_rad;
}
*lat = lat_rad * 180.0 / M_PI;
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 1d0cd524a..20fb34ca3 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -69,8 +69,8 @@ struct crosstrack_error_s {
/* lat/lon are in radians */
struct map_projection_reference_s {
- double lat;
- double lon;
+ double lat_rad;
+ double lon_rad;
double sin_lat;
double cos_lat;
bool init_done;
@@ -105,13 +105,13 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference
* Writes the reference values of the global projection to ref_lat and ref_lon
* @return 0 if map_projection_init was called before, -1 else
*/
-__EXPORT int map_projection_global_reference(double *ref_lat, double *ref_lon);
+__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad);
/**
* Writes the reference values of the projection given by the argument to ref_lat and ref_lon
* @return 0 if map_projection_init was called before, -1 else
*/
-__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat, double *ref_lon);
+__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad);
/**
* Initializes the global map transformation.