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

fix height

This commit is contained in:
David St-Onge 2017-02-22 02:53:17 -05:00
parent bbde4dc950
commit 2e2193fcf9
2 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/buzzuav_closures.cpp
View File

@ -145,7 +145,7 @@ int buzzuav_moveto(buzzvm_t vm) {
goto_pos[0]=hcpos3[0];goto_pos[1]=hcpos3[1];goto_pos[2]=height;
}
}
if(dx == 2.0){
if(dx == 2.0){OB
if((int)buzz_utility::get_robotid()==1){
goto_pos[0]=hcpos1[2];goto_pos[1]=hcpos1[3];goto_pos[2]=height;
}
@ -163,6 +163,7 @@ int buzzuav_moveto(buzzvm_t vm) {
}
int buzzuav_goto(buzzvm_t vm) {
rc_goto_pos[2]=height;
set_goto(rc_goto_pos);
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]);

2
src/roscontroller.cpp
View File

@ -272,7 +272,7 @@ namespace rosbzz_node{
/*prepare the goto publish message */
cmd_srv.request.param5 = goto_pos[0];
cmd_srv.request.param6 = goto_pos[1];
cmd_srv.request.param7 = cur_pos[2]; //force constant altitude after takeoff, goto_pos[2];
cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
else ROS_ERROR("Failed to call service from flight controller");