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 00:18:32 -05:00
parent 6ab4e35b61
commit 333fdfc0a1
3 changed files with 16 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/buzzuav_closures.cpp
View File

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

18
src/roscontroller.cpp
View File

@ -14,7 +14,7 @@ namespace rosbzz_node{
/*Compile the .bzz file to .basm, .bo and .bdbg*/ /*Compile the .bzz file to .basm, .bo and .bdbg*/
Compile_bzz(); Compile_bzz();
set_bzz_file(bzzfile_name.c_str()); 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 temp2_gps[3]={45.565246,-73.561951,10.1};
double temp_car[3], temp2_car[3]; double temp_car[3], temp2_car[3];
double out[3], dif[3], out2[3]; double out[3], dif[3], out2[3];
@ -25,10 +25,10 @@ namespace rosbzz_node{
for(int i=0;i<3;i++) for(int i=0;i<3;i++)
dif[i]=temp2_car[i]-temp_car[i]; dif[i]=temp2_car[i]-temp_car[i];
cvt_spherical_coordinates(dif,out); 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); // cvt_rangebearing2D_coordinates(temp2_gps,out2,temp_gps);
printf("TEST RESULT: %.7f,%.7f,%.7f\n",out[0],out[1],out[2]); 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***/ /***Destructor***/
@ -322,10 +322,14 @@ namespace rosbzz_node{
#define LAT_AVERAGE 45.564898 #define LAT_AVERAGE 45.564898
/*rectangular projection range and bearing coordinate system callback */ /*rectangular projection range and bearing coordinate system callback */
void roscontroller::cvt_rangebearingGB_coordinates(double nei[], double out[], double cur[]){ 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 lat1 = cur[0]*M_PI/180.0;
double y = sin(cur[1]-nei[1])*cos(cur[0]); double lon1 = cur[1]*M_PI/180.0;
double x = cos(cur[0])*sin(nei[0]) - sin(nei[0])*cos(cur[0])*cos(cur[1]-nei[1]); double lat2 = nei[0]*M_PI/180.0;
out[1] = atan2(y,x)/M_PI*180+180; 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; out[2] = 0.0;
} }

2
src/test1.bzz
View File

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