arm and disarm simplification

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
 }

launch/rosbuzz_2_parallel.launch

@@ -2,7 +2,7 @@
 
 <launch>
 		<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
-		<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/testflockfev_barrier.bzz" />
+		<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/testflockfev.bzz" />
 		<rosparam file="/home/vivek/catkin_ws/src/rosbuzz/launch/launch_config/m100.yaml"/>
                 <param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="rc_cmd" />

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);

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;
-}
-
-
 }

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{
 	
 }
 
-

src/testflockfev.bzz

@@ -1,4 +1,3 @@
-
 # We need this for 2D vectors
 # Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
 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)
 }