diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-08 21:49:59 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-08 21:49:59 +0200 |
| commit | 11e4fbc3745193a61f6f56619318dc1bc0b60307 (patch) | |
| tree | c62437439c681c706a2473f92429091eb0eb29a9 /src/lib/geo | |
| parent | 14828cfda5f4c81cb8366d5eee4eb1e1cab45370 (diff) | |
| download | px4-firmware-11e4fbc3745193a61f6f56619318dc1bc0b60307.tar.gz px4-firmware-11e4fbc3745193a61f6f56619318dc1bc0b60307.tar.bz2 px4-firmware-11e4fbc3745193a61f6f56619318dc1bc0b60307.zip | |
Added additional vector functions, fixed seatbelt for global estimators
Diffstat (limited to 'src/lib/geo')
| -rw-r--r-- | src/lib/geo/geo.c | 34 | ||||
| -rw-r--r-- | src/lib/geo/geo.h | 8 |
2 files changed, 38 insertions, 4 deletions
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 63792dda5..e862b1dc0 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -210,6 +210,40 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub return theta; } +__EXPORT float get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + double lat_next_rad = lat_next * M_DEG_TO_RAD; + double lon_next_rad = lon_next * M_DEG_TO_RAD; + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + /* conscious mix of double and float trig function to maximize speed and efficiency */ + *vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad) + *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + + return theta; +} + +__EXPORT float get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + double lat_next_rad = lat_next * M_DEG_TO_RAD; + double lon_next_rad = lon_next * M_DEG_TO_RAD; + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + /* conscious mix of double and float trig function to maximize speed and efficiency */ + *vy = CONSTANTS_RADIUS_OF_EARTH * d_lon; + *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad); + + return theta; +} + // Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index dadec51ec..0459909e4 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -57,10 +57,6 @@ __BEGIN_DECLS #define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */ #define CONSTANTS_RADIUS_OF_EARTH 6371000 /* meters (m) */ -/* compatibility aliases */ -#define RADIUS_OF_EARTH CONSTANTS_RADIUS_OF_EARTH -#define GRAVITY_MSS CONSTANTS_ONE_G - // XXX remove struct crosstrack_error_s { bool past_end; // Flag indicating we are past the end of the line/arc segment @@ -116,6 +112,10 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou */ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); +__EXPORT float get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy); + +__EXPORT float get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy); + __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, |