merge with master

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,6 +84,8 @@ int buzzuav_update_prox(buzzvm_t vm);
 
 int bzz_cmd();
 
+
 int dummy_closure(buzzvm_t vm);
+
 //#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

@@ -173,10 +173,16 @@ int buzzuav_goto(buzzvm_t vm) {
 
 int buzzuav_arm(buzzvm_t vm) {
    cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
-   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() {
@@ -414,4 +420,3 @@ int buzzuav_update_prox(buzzvm_t vm) {
 int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);}
 
 }
-

src/roscontroller.cpp

@@ -290,6 +290,10 @@ 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();
 		}
 	}
@@ -358,10 +362,10 @@ namespace rosbzz_node{
     		payload_out.payload64.push_back(position[1]);
     		payload_out.payload64.push_back(position[2]);
     		/*Append Buzz message*/
-    		uint16_t* out = buzz_utility::u64_cvt_u16(payload_out_ptr[0]);
+    		uint16_t* out = buzz_utility::u64_cvt_u16(payload_out_ptr[0]);  
     		for(int i=0;i<out[0];i++){
     			payload_out.payload64.push_back(payload_out_ptr[i]);
-    		}
+    		}	
     		/*int i=0;
 		uint64_t message_obt[payload_out.payload64.size()];
     		for(std::vector<long unsigned int>::const_iterator it = payload_out.payload64.begin();
@@ -432,10 +436,10 @@ namespace rosbzz_node{
 		//	mavros_msgs::Mavlink stop_req_packet;
 		//	stop_req_packet.payload64.push_back((uint64_t) XBEE_STOP_TRANSMISSION);
 		//	payload_pub.publish(stop_req_packet);
-		//	std::cout << "request xbee to stop multi-packet transmission" << std::endl;
+		//	std::cout << "request xbee to stop multi-packet transmission" << std::endl;	
-
+		
 		//}
-
+		
 		}
 
 
@@ -804,12 +808,16 @@ 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);
+					res.success = true;	
+				}			
+				else{
    					ROS_INFO("RC_Call: DISARM!!!!");
-				buzzuav_closures::rc_call(rc_cmd);
+					buzzuav_closures::rc_call(rc_cmd+1);
-				res.success = true;
+					res.success = true;	
+				}			
 				break;
 			case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
    				ROS_INFO("RC_Call: GO HOME!!!!");
@@ -818,6 +826,7 @@ namespace rosbzz_node{
 				res.success = true;
 				break;
 			case mavros_msgs::CommandCode::NAV_WAYPOINT:
+   				ROS_INFO("RC_Call: GO TO!!!! ");
 				double rc_goto[3];
 				rc_goto[0] = req.param5;
 				rc_goto[1] = req.param6;

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/ubuntu/buzz/src/include/vec2.bzz"
@@ -100,6 +99,10 @@ neighbors.listen("cmd",
 		statef=takeoff
 	} else if(value==21) {
 		statef=land
+	} else if(value==400) {
+		uav_arm()
+	} else if(value==401){
+		uav_disarm()
 	}
    }
    
@@ -160,7 +163,11 @@ function step() {
 	} else if(flight.rc_cmd==400) {
 		flight.rc_cmd=0
 		uav_arm()
-	} 
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
 	statef()
 	log("Current state: ", CURSTATE)
 }