From 9c7ae5f06b75a113d5f065c2210b28f944ee7bd3 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Tue, 31 Jan 2017 01:33:13 -0500 Subject: [PATCH] gps fix --- src/buzzuav_closures.cpp | 2 +- src/roscontroller.cpp | 4 ++-- src/test1.bzz | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 96fbae2..aaac839 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -95,7 +95,7 @@ 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; +// bearing = bearing*M_PI/180.0; out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing)); out[0] = out[0]*180.0/M_PI; out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0])); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 863715f..0fa172b 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -13,7 +13,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]; @@ -27,7 +27,7 @@ namespace rosbzz_node{ 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***/ diff --git a/src/test1.bzz b/src/test1.bzz index db07bca..ca21bff 100644 --- a/src/test1.bzz +++ b/src/test1.bzz @@ -2,7 +2,7 @@ # We need this for 2D vectors # Make sure you pass the correct include path to "bzzc -I ..." -include "/home/vivek/catkin_ws/src/rosbuzz/src/vec2.bzz" +include "/home/ubuntu/buzz/src/include/vec2.bzz" #################################################################################################### # Updater related # This should be here for the updater to work, changing position of code will crash the updater