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/geo.c | |
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/geo.c')
-rw-r--r-- | src/lib/geo/geo.c | 34 |
1 files changed, 34 insertions, 0 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) |