From 2fc36ebf092a3700c41f3a64d860a436d9fd2aeb Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 23 Feb 2017 21:14:42 -0500 Subject: [PATCH 1/3] 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) From f166d4e5b4dd60cc2568e3c9db8e93712c1cfcc6 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 23 Feb 2017 23:09:04 -0500 Subject: [PATCH 2/3] arm and disarm simplification --- include/buzzuav_closures.h | 5 +---- launch/rosbuzz_2_parallel.launch | 2 +- src/buzz_utility.cpp | 6 ++++++ src/buzzuav_closures.cpp | 27 +++++++-------------------- src/roscontroller.cpp | 29 +++++++++++------------------ src/testflockfev.bzz | 32 ++++++++++---------------------- 6 files changed, 36 insertions(+), 65 deletions(-) diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index c05a847..a58c367 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -48,6 +48,7 @@ void set_obstacle_dist(float dist[]); int buzzuav_takeoff(buzzvm_t vm); int buzzuav_arm(buzzvm_t vm); +int buzzuav_disarm(buzzvm_t vm) ; /* Commands the UAV to land */ int buzzuav_land(buzzvm_t vm); @@ -83,12 +84,8 @@ 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/launch/rosbuzz_2_parallel.launch b/launch/rosbuzz_2_parallel.launch index 93a456b..7180e5b 100644 --- a/launch/rosbuzz_2_parallel.launch +++ b/launch/rosbuzz_2_parallel.launch @@ -2,7 +2,7 @@ - + diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 32233ad..9c88de0 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -242,6 +242,9 @@ namespace buzz_utility{ buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm)); buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_disarm)); + buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff)); buzzvm_gstore(VM); @@ -274,6 +277,9 @@ namespace buzz_utility{ buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index e8dafb4..74674cf 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -19,7 +19,6 @@ static int cur_cmd = 0; static int rc_cmd=0; static int buzz_cmd=0; static float height=0; -static int arm_state=-1; /****************************************/ /****************************************/ @@ -173,15 +172,17 @@ 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"); + printf(" Buzz requested Arm \n"); buzz_cmd=3; return buzzvm_ret0(vm); } +int buzzuav_disarm(buzzvm_t vm) { + cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1; + printf(" Buzz requested Disarm \n"); + buzz_cmd=4; + return buzzvm_ret0(vm); +} /******************************/ double* getgoto() { @@ -220,13 +221,6 @@ 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); - -} /****************************************/ /****************************************/ @@ -425,11 +419,4 @@ 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 1ffeef8..f04802b 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -33,11 +33,6 @@ 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(); @@ -72,17 +67,14 @@ 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 */ @@ -276,7 +268,11 @@ namespace rosbzz_node{ 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 if (tmp == 3) { /*FC call for arm*/ + armstate=1; Arm(); + } else if (tmp == 4){ + armstate=0; + Arm(); } /*obtain Pay load to be sent*/ //fprintf(stdout, "before getting msg from utility\n"); @@ -693,7 +689,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(); @@ -713,15 +709,13 @@ namespace rosbzz_node{ if(armstate){ ROS_INFO("RC_Call: ARM!!!!"); 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); - buzzuav_closures::rc_call_setarmparm(armstate); - res.success = true; - } + buzzuav_closures::rc_call(rc_cmd+1); + res.success = true; + } break; case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH: ROS_INFO("RC_Call: GO HOME!!!!"); @@ -754,4 +748,3 @@ namespace rosbzz_node{ } - diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz index 01ceceb..3d66077 100644 --- a/src/testflockfev.bzz +++ b/src/testflockfev.bzz @@ -1,4 +1,3 @@ - # We need this for 2D vectors # Make sure you pass the correct include path to "bzzc -I ..." include "/home/vivek/ubuntu/buzz/src/include/vec2.bzz" @@ -100,12 +99,10 @@ neighbors.listen("cmd", statef=takeoff } else if(value==21) { statef=land - } - else if(value==401){ - uav_arm(1) - } - else if(value==402){ - uav_arm(0) + } else if(value==400) { + uav_arm() + } else if(value==401){ + uav_disarm() } } @@ -137,20 +134,7 @@ 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 @@ -178,8 +162,12 @@ function step() { uav_goto() } else if(flight.rc_cmd==400) { flight.rc_cmd=0 - arm() - } + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + uav_disarm() + neighbors.broadcast("cmd", 401) + } statef() log("Current state: ", CURSTATE) } From 6593215a5726fb5e700cc72691049f1b28bbc5fc Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 23 Feb 2017 23:10:01 -0500 Subject: [PATCH 3/3] bzz har code include correction --- src/testflockfev.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz index 3d66077..98abcd6 100644 --- a/src/testflockfev.bzz +++ b/src/testflockfev.bzz @@ -1,6 +1,6 @@ # We need this for 2D vectors # Make sure you pass the correct include path to "bzzc -I ..." -include "/home/vivek/ubuntu/buzz/src/include/vec2.bzz" +include "/home/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