Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

gps fix

This commit is contained in:
David St-Onge 2017-01-31 00:36:18 -05:00
parent f94b3dc12c
commit 9dbd248cd3
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/buzzuav_closures.cpp
View File

@ -97,7 +97,7 @@ void gps_from_rb(double range, double bearing, double out[3]) {
double lon = cur_pos[1]*M_PI/180.0;
bearing = bearing*M_PI/180.0;
out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing));
out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(bearing/EARTH_RADIUS) - sin(lat)*sin(out[0]));
out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0]));
out[2] = height; //constant height.
}