From 2fc36ebf092a3700c41f3a64d860a436d9fd2aeb Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 23 Feb 2017 21:14:42 -0500 Subject: [PATCH] addition of arm/disarm for neighbours --- include/buzzuav_closures.h | 6 ++++++ src/buzzuav_closures.cpp | 18 ++++++++++++++++++ src/roscontroller.cpp | 30 ++++++++++++++++++++++-------- src/testflockfev.bzz | 23 +++++++++++++++++++++-- 4 files changed, 67 insertions(+), 10 deletions(-) diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 7911308..c05a847 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -83,6 +83,12 @@ int buzzuav_update_prox(buzzvm_t vm); int bzz_cmd(); +/*arm disarm*/ + +void rc_call_setarmparm(int armstate); + int dummy_closure(buzzvm_t vm); + +int get_armstate(); //#endif } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index fe3f4f7..e8dafb4 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -19,6 +19,7 @@ static int cur_cmd = 0; static int rc_cmd=0; static int buzz_cmd=0; static float height=0; +static int arm_state=-1; /****************************************/ /****************************************/ @@ -172,7 +173,11 @@ int buzzuav_goto(buzzvm_t vm) { } int buzzuav_arm(buzzvm_t vm) { + buzzvm_lnum_assert(vm, 1); + buzzvm_lload(vm, 1); /* arm param1 */ + buzzvm_type_assert(vm, 1, BUZZTYPE_INT); cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; + arm_state = buzzvm_stack_at(vm, 1)->i.value; printf(" Buzz requested Arm/Disarm \n"); buzz_cmd=3; return buzzvm_ret0(vm); @@ -215,6 +220,13 @@ void set_obstacle_dist(float dist[]) { obst[i] = dist[i]; } +void rc_call_setarmparm(int armstate){ + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "armstate", 1)); + buzzvm_pushi(VM, armstate); + buzzvm_gstore(VM); + +} /****************************************/ /****************************************/ @@ -413,5 +425,11 @@ int buzzuav_update_prox(buzzvm_t vm) { int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);} +int get_armstate(){ + int tmp= arm_state; + arm_state=-1; + return tmp; } + +} diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 559bf4e..1ffeef8 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -33,6 +33,11 @@ namespace rosbzz_node{ init_update_monitor(bcfname.c_str(),stand_by.c_str(),barrier); while (ros::ok() && !buzz_utility::buzz_script_done()) { + /*Set arm parameter*/ + int tmp_arm_state= buzzuav_closures::get_armstate(); + if(tmp_arm_state == 1) armstate=tmp_arm_state; + else if (tmp_arm_state==0) armstate=tmp_arm_state; + /*Update neighbors position inside Buzz*/ buzz_utility::neighbour_pos_callback(neighbours_pos_map); auto current_time = ros::Time::now(); @@ -67,14 +72,17 @@ namespace rosbzz_node{ set_read_update_status(); multi_msg=true; } + /*sleep for the mentioned loop rate*/ + timer_step+=1; + maintain_pos(timer_step); + /*run once*/ ros::spinOnce(); /*loop rate of ros*/ ros::Rate loop_rate(10); loop_rate.sleep(); - /*sleep for the mentioned loop rate*/ - timer_step+=1; - maintain_pos(timer_step); + + } /* Destroy updater and Cleanup */ @@ -685,7 +693,7 @@ namespace rosbzz_node{ ROS_INFO("RC_call: TAKE OFF!!!!"); rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; /* arming */ - SetMode(); + //SetMode(); if(!armstate) { armstate =1; Arm(); @@ -702,12 +710,18 @@ namespace rosbzz_node{ case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM: rc_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; armstate = req.param1; - if(armstate) + if(armstate){ ROS_INFO("RC_Call: ARM!!!!"); - else + buzzuav_closures::rc_call(rc_cmd); + buzzuav_closures::rc_call_setarmparm(armstate); + res.success = true; + } + else{ ROS_INFO("RC_Call: DISARM!!!!"); - buzzuav_closures::rc_call(rc_cmd); - res.success = true; + buzzuav_closures::rc_call(rc_cmd); + buzzuav_closures::rc_call_setarmparm(armstate); + res.success = true; + } break; case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH: ROS_INFO("RC_Call: GO HOME!!!!"); diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz index fe4b475..01ceceb 100644 --- a/src/testflockfev.bzz +++ b/src/testflockfev.bzz @@ -1,7 +1,7 @@ # We need this for 2D vectors # Make sure you pass the correct include path to "bzzc -I ..." -include "/home/ubuntu/buzz/src/include/vec2.bzz" +include "/home/vivek/ubuntu/buzz/src/include/vec2.bzz" #################################################################################################### # Updater related # This should be here for the updater to work, changing position of code will crash the updater @@ -101,6 +101,12 @@ neighbors.listen("cmd", } else if(value==21) { statef=land } + else if(value==401){ + uav_arm(1) + } + else if(value==402){ + uav_arm(0) + } } @@ -131,7 +137,20 @@ function land() { statef=idle } } +function arm(){ + log("arm: ", armstate) + if(armstate == 1){ + neighbors.broadcast("cmd", 401) + uav_arm(armstate) + } + else { + neighbors.broadcast("cmd", 402) + uav_arm(armstate) + } + + +} # Executed once at init time. function init() { statef=idle @@ -159,7 +178,7 @@ function step() { uav_goto() } else if(flight.rc_cmd==400) { flight.rc_cmd=0 - uav_arm() + arm() } statef() log("Current state: ", CURSTATE)