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

takeoff height

This commit is contained in:
David St-Onge 2017-01-25 17:30:11 -05:00
parent 38b3998281
commit 7e1d8c5244
3 changed files with 9 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/buzzuav_closures.cpp
View File

@ -116,6 +116,10 @@ rc_cmd=rc_cmd_in;
/****************************************/ /****************************************/
int buzzuav_takeoff(buzzvm_t vm) { int buzzuav_takeoff(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 1);
buzzvm_lload(vm, 1); /* Altitude */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value;
cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
printf(" Buzz requested Take off !!! \n"); printf(" Buzz requested Take off !!! \n");
buzz_cmd=1; buzz_cmd=1;

3
src/roscontroller.cpp
View File

@ -156,13 +156,14 @@ namespace rosbzz_node{
/* flight controller client call if requested from Buzz*/ /* flight controller client call if requested from Buzz*/
/*FC call for takeoff,land and gohome*/ /*FC call for takeoff,land and gohome*/
int tmp = buzzuav_closures::bzz_cmd(); int tmp = buzzuav_closures::bzz_cmd();
double* goto_pos = buzzuav_closures::getgoto();
if (tmp == 1){ if (tmp == 1){
cmd_srv.request.z = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd(); cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); } 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"); else ROS_ERROR("Failed to call service from flight controller");
} else if (tmp == 2) { /*FC call for goto*/ } else if (tmp == 2) { /*FC call for goto*/
/*prepare the goto publish message */ /*prepare the goto publish message */
double* goto_pos = buzzuav_closures::getgoto();
cmd_srv.request.x = goto_pos[0]*pow(10,7); cmd_srv.request.x = goto_pos[0]*pow(10,7);
cmd_srv.request.y = goto_pos[1]*pow(10,7); cmd_srv.request.y = goto_pos[1]*pow(10,7);
cmd_srv.request.z = goto_pos[2]; cmd_srv.request.z = goto_pos[2];

6
src/test1.bzz
View File

@ -89,12 +89,12 @@ neighbors.listen("cmd",
} }
function takeoff() { function takeoff() {
if( flight.status == 3) { if( flight.status == 2 and position.altitude >= 10.0) {
barrier_set(ROBOTS,hexagon) barrier_set(ROBOTS,hexagon)
barrier_ready() barrier_ready()
} }
else else if( flight.status !=3)
uav_takeoff() uav_takeoff(10.1)
} }
function land() { function land() {
if( flight.status == 1) { if( flight.status == 1) {