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 01:33:13 -05:00
parent b472001974
commit 9c7ae5f06b
3 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/buzzuav_closures.cpp
View File

@ -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]));

4
src/roscontroller.cpp
View File

@ -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***/

2
src/test1.bzz
View File

@ -2,7 +2,7 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
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