From b9fe7a6e2c42ca8656f8bcd6540b43605df1ba90 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sat, 24 Dec 2016 20:59:00 -0500 Subject: [PATCH] change in filght controller cmd handling --- include/buzzuav_closures.h | 3 +++ include/roscontroller.h | 5 ++--- src/buzzuav_closures.cpp | 14 ++++++++++++++ src/roscontroller.cpp | 12 +++++++++--- src/test1.bzz | 11 +++++------ 5 files changed, 33 insertions(+), 12 deletions(-) diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 99f85ef..1a9b56f 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -7,6 +7,7 @@ #include "mavros_msgs/CommandCode.h" #include "ros/ros.h" + namespace buzzuav_closures{ /* * prextern int() function in Buzz @@ -71,5 +72,7 @@ int buzzuav_update_flight_status(buzzvm_t vm); */ int buzzuav_update_prox(buzzvm_t vm); +int bzz_cmd(); + //#endif } diff --git a/include/roscontroller.h b/include/roscontroller.h index 6a706e0..5ca5220 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -11,7 +11,6 @@ #include "sensor_msgs/NavSatStatus.h" #include #include -#include "buzzuav_closures.h" #include "buzz_utility.h" #include "uav_utility.h" #include @@ -20,6 +19,7 @@ #include #include #include +#include "buzzuav_closures.h" using namespace std; @@ -40,9 +40,8 @@ private: std::map< int, buzz_utility::Pos_struct> neighbours_pos_map; int timer_step=0; int robot_id=0; - int oldcmdID=0; + //int oldcmdID=0; int rc_cmd; - int bzz_old_cmd=0; std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname,out_payload,in_payload; //, rcclient; bool rcclient; ros::ServiceClient mav_client; diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 49c64f8..1965358 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -15,6 +15,7 @@ static double cur_pos[3]; static uint8_t status; static int cur_cmd = 0; static int rc_cmd=0; +static int buzz_cmd=0; /****************************************/ /****************************************/ @@ -74,6 +75,7 @@ int buzzuav_goto(buzzvm_t vm) { goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value; 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]); + buzz_cmd=2; return buzzvm_ret0(vm); } @@ -94,6 +96,12 @@ goto_pos[2]=pos[2]; } +int bzz_cmd(){ +int cmd = buzz_cmd; +buzz_cmd=0; +return cmd; +} + void rc_set_goto(double pos[]){ rc_goto_pos[0]=pos[0]; rc_goto_pos[1]=pos[1]; @@ -110,18 +118,21 @@ rc_cmd=rc_cmd_in; int buzzuav_takeoff(buzzvm_t vm) { cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; printf(" Buzz requested Take off !!! \n"); + buzz_cmd=1; return buzzvm_ret0(vm); } int buzzuav_land(buzzvm_t vm) { cur_cmd=mavros_msgs::CommandCode::NAV_LAND; printf(" Buzz requested Land !!! \n"); + buzz_cmd=1; return buzzvm_ret0(vm); } int buzzuav_gohome(buzzvm_t vm) { cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; printf(" Buzz requested gohome !!! \n"); + buzz_cmd=1; return buzzvm_ret0(vm); } @@ -252,4 +263,7 @@ int buzzuav_update_prox(buzzvm_t vm) { /****************************************/ /****************************************/ + + } + diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 0ed7bc0..82fe479 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -124,8 +124,15 @@ namespace rosbzz_node{ void roscontroller::prepare_msg_and_publish(){ - /* flight controller client call*/ - if (bzz_old_cmd != buzzuav_closures::getcmd()) { + /* flight controller client call if requested from Buzz*/ + /*FC call for takeoff,land and gohome*/ + if (buzzuav_closures::bzz_cmd()==1){ + 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"); + } + /*FC call for goto*/ + else if (buzzuav_closures::bzz_cmd() == 2) { /*prepare the goto publish message */ double* goto_pos = buzzuav_closures::getgoto(); cmd_srv.request.param1 = goto_pos[0]; @@ -134,7 +141,6 @@ namespace rosbzz_node{ 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"); - bzz_old_cmd = cmd_srv.request.command; } /*obtain Pay load to be sent*/ uint64_t* payload_out_ptr= buzz_utility::out_msg_process(); diff --git a/src/test1.bzz b/src/test1.bzz index 3a1c02e..490e954 100644 --- a/src/test1.bzz +++ b/src/test1.bzz @@ -72,7 +72,7 @@ function takeoff() { barrier_set(ROBOTS,transition_to_land) barrier_ready() } - else + else if(flight.status!=3) uav_takeoff() } function land() { @@ -80,18 +80,17 @@ function land() { barrier_set(ROBOTS,idle) barrier_ready() } - else + else if(flight.status!=0 and flight.status!=4) uav_land() } function transition_to_land() { statef=transition_to_land - if(tim>=100){ + if(battery.capacity<50){ + print("Low battery! Landing the fleet") statef=land - tim=0 - }else{ - tim=tim+1 + neighbors.broadcast("cmd", 21) } }