From 333fdfc0a164d56940f8087ea07c58fa1820961d Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Tue, 31 Jan 2017 00:18:32 -0500 Subject: [PATCH] gps fix --- src/buzzuav_closures.cpp | 6 ++++-- src/roscontroller.cpp | 18 +++++++++++------- src/test1.bzz | 2 +- 3 files changed, 16 insertions(+), 10 deletions(-) diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index e94edd4..0ff3cb4 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -93,8 +93,10 @@ int buzzros_print(buzzvm_t vm) { } void gps_from_rb(double range, double bearing, double out[3]) { - out[0] = asin(sin(cur_pos[0]) * cos(range/EARTH_RADIUS) + cos(cur_pos[0]) * sin(range/EARTH_RADIUS) * cos(bearing)); - out[1] = cur_pos[1] + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(cur_pos[0]), cos(bearing/EARTH_RADIUS) - sin(cur_pos[0])*sin(out[0])); + double lat = cur_pos[0]*M_PI/180.0; + double lon = cur_pos[1]*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. } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 23be135..f760c5f 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -14,7 +14,7 @@ namespace rosbzz_node{ /*Compile the .bzz file to .basm, .bo and .bdbg*/ Compile_bzz(); set_bzz_file(bzzfile_name.c_str()); -/* double temp_gps[3]={45.564898,-73.563072,10.1}; + double temp_gps[3]={45.564898,-73.563072,10.1}; double temp2_gps[3]={45.565246,-73.561951,10.1}; double temp_car[3], temp2_car[3]; double out[3], dif[3], out2[3]; @@ -25,10 +25,10 @@ namespace rosbzz_node{ for(int i=0;i<3;i++) dif[i]=temp2_car[i]-temp_car[i]; cvt_spherical_coordinates(dif,out); - cvt_rangebearing_coordinates(temp2_gps,out2,temp_gps); + cvt_rangebearingGB_coordinates(temp2_gps,out2,temp_gps); // cvt_rangebearing2D_coordinates(temp2_gps,out2,temp_gps); printf("TEST RESULT: %.7f,%.7f,%.7f\n",out[0],out[1],out[2]); - printf("TEST RESULT2: %.7f,%.7f,%.7f\n",out2[0],out2[1],out2[2]);*/ + printf("TEST RESULT2: %.7f,%.7f,%.7f\n",out2[0],out2[1],out2[2]); } /***Destructor***/ @@ -322,10 +322,14 @@ namespace rosbzz_node{ #define LAT_AVERAGE 45.564898 /*rectangular projection range and bearing coordinate system callback */ void roscontroller::cvt_rangebearingGB_coordinates(double nei[], double out[], double cur[]){ - out[1] = acos(sin(cur[0]) * sin(nei[0]) +cos(nei[0]) * cos(cur[0])*cos(nei[1]-cur[1]))*EARTH_RADIUS; - double y = sin(cur[1]-nei[1])*cos(cur[0]); - double x = cos(cur[0])*sin(nei[0]) - sin(nei[0])*cos(cur[0])*cos(cur[1]-nei[1]); - out[1] = atan2(y,x)/M_PI*180+180; + double lat1 = cur[0]*M_PI/180.0; + double lon1 = cur[1]*M_PI/180.0; + double lat2 = nei[0]*M_PI/180.0; + double lon2 = nei[1]*M_PI/180.0; + out[0] = acos(sin(lat1) * sin(lat2) +cos(lat2) * cos(lat1)*cos(lon2-lon1))*EARTH_RADIUS; + double y = sin(lon1-lon2)*cos(lat1); + double x = cos(lat2)*sin(lat1) - sin(lat2)*cos(lat1)*cos(lon1-lon2); + out[1] = atan2(y,x)+M_PI; out[2] = 0.0; } diff --git a/src/test1.bzz b/src/test1.bzz index d15a7fa..ffecafe 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(accum.x, accum.y) + uav_goto(0.0, 0.0) #accum.x, accum.y) } ########################################