Spiri/ROSBuzz_MISTLab
13
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

more coord. fixes

Browse Source
This commit is contained in:
David St-Onge 2017-01-26 19:49:08 -05:00
parent 1ac062ea3d
commit 641e07fe81
3 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/buzzuav_closures.cpp
View File

@ -79,7 +79,7 @@ int buzzuav_goto(buzzvm_t vm) {
goto_pos[1]=cur_pos[1]+dlon*180/3.1416; goto_pos[1]=cur_pos[1]+dlon*180/3.1416;
goto_pos[2]=cur_pos[2]; goto_pos[2]=cur_pos[2];
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
printf(" Buzz requested Go To, to Latitude: %15f , Longitude: %15f, Altitude: %15f \n",goto_pos[0],goto_pos[1],goto_pos[2]); printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]);
buzz_cmd=2; buzz_cmd=2;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }

1
src/roscontroller.cpp
View File

@ -328,6 +328,7 @@ namespace rosbzz_node{
cvt_spherical_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload); cvt_spherical_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload);
/*Extract robot id of the neighbour*/ /*Extract robot id of the neighbour*/
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3)); uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));
cout << "Rel Pos of " << out << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[2] << ", "<< cvt_neighbours_pos_payload[3] << endl;
/*pass neighbour position to local maintaner*/ /*pass neighbour position to local maintaner*/
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]); buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]);
/*Put RID and pos*/ /*Put RID and pos*/

2
src/test1.bzz
View File

@ -34,7 +34,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+position.latitude, accum.y+position.longitude, TARGET_ALTITUDE) uav_goto(accum.x, accum.y)
} }
######################################## ########################################