From f94b3dc12cb8f1883631037c22b215f5f897930c Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Tue, 31 Jan 2017 00:31:20 -0500 Subject: [PATCH] gps fix --- src/buzzuav_closures.cpp | 3 ++- src/test1.bzz | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 0ff3cb4..5e1a41f 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -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) { diff --git a/src/test1.bzz b/src/test1.bzz index ffecafe..d15a7fa 100644 --- a/src/test1.bzz +++ b/src/test1.bzz @@ -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) } ########################################