addition of arm/disarm for neighbours

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
 }

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

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

src/testflockfev.bzz

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