Spiri
/
ROSBuzz_MISTLab
13
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:31:20 -05:00
parent 333fdfc0a1
commit f94b3dc12c
2 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/buzzuav_closures.cpp
View File

@ -95,9 +95,10 @@ int buzzros_print(buzzvm_t vm) {
void gps_from_rb(double range, double bearing, double out[3]) {
double lat = cur_pos[0]*M_PI/180.0;
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[3] = height; //constant height.
out[2] = height; //constant height.
}
int buzzuav_goto(buzzvm_t vm) {

2
src/test1.bzz
View File

@ -42,7 +42,7 @@ function hexagon() {
math.vec2.scale(accum, 1.0 / neighbors.count())
# Move according to vector
# print("Robot ", id, "must push ",accum.x, "; ", accum.y)
uav_goto(0.0, 0.0) #accum.x, accum.y)
uav_goto(accum.x, accum.y)
}
########################################