Made rc command available in .bzz script and removed continuous call of flight controller

CMakeLists.txt

@@ -9,7 +9,7 @@ endif()
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   std_msgs
-  mavros_msgs
+  #mavros_msgs
   sensor_msgs
 )
 

include/buzzuav_closures.h

@@ -22,8 +22,10 @@ int buzzros_print(buzzvm_t vm);
 int buzzuav_goto(buzzvm_t vm);
 /* Returns the current command from local variable*/
 int getcmd();
-/*Sets goto position could be used for bypassing*/
+/*Sets goto position */
 void set_goto(double pos[]);
+/*Sets goto position from rc client*/
+void rc_set_goto(double pos[]);
 /*sets rc requested command */
 void rc_call(int rc_cmd);
 /* sets the battery state */
@@ -57,7 +59,9 @@ int buzzuav_update_battery(buzzvm_t vm);
  */
 int buzzuav_update_currentpos(buzzvm_t vm);
 /*
- * Updates flight status in Buzz
+ * Updates flight status and rc command in Buzz, put it in a tabel to acess it 
+ * use flight.status for flight status
+ * use flight.rc_cmd for current rc cmd
  */
 int buzzuav_update_flight_status(buzzvm_t vm);
 

include/roscontroller.h

@@ -42,6 +42,7 @@ private:
 	int robot_id=0;
         int oldcmdID=0;
 	int rc_cmd;
+	int bzz_old_cmd=0;
 	std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname,out_payload,in_payload; //, rcclient;
 	bool rcclient;
 	ros::ServiceClient mav_client;

launch/rosbuzz.launch

@@ -2,13 +2,13 @@
 
 <launch>
 		<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
-		<param name="bzzfile_name" value="../ROS_WS/src/ROSBuzz/src/test.bzz" />
+		<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz" />
                 <param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="rc_cmd" />
 		<param name="fcclient_name" value="/djicmd" />
-		<param name="in_payload" value="outMavlink"/>
-		<param name="out_payload" value="outMavlink"/>
-		<param name="robot_id" value="1"/>
+		<param name="in_payload" value="/inMavlink"/>
+		<param name="out_payload" value="/outMavlink"/>
+		<param name="robot_id" value="2"/>
 		</node>
 		
 		

src/buzzuav_closures.cpp

@@ -9,10 +9,11 @@
 #include "buzzuav_closures.h"
 namespace buzzuav_closures{
 static double goto_pos[3];
+static double rc_goto_pos[3];
 static float batt[3];
 static double cur_pos[3];
 static uint8_t status;
-static int cur_cmd;
+static int cur_cmd = 0;
 static int rc_cmd=0;
 /****************************************/
 /****************************************/
@@ -83,12 +84,7 @@ return goto_pos;
 }
 /******************************/
 int getcmd(){
-if(rc_cmd==0) return cur_cmd;
-else {
-cur_cmd=rc_cmd;
-rc_cmd=0;
-return cur_cmd;
-} 
+ return cur_cmd;
 }
 
 void set_goto(double pos[]){
@@ -98,6 +94,12 @@ goto_pos[2]=pos[2];
     
 }
 
+void rc_set_goto(double pos[]){
+rc_goto_pos[0]=pos[0];
+rc_goto_pos[1]=pos[1];
+rc_goto_pos[2]=pos[2];
+    
+}
 void rc_call(int rc_cmd_in){
 rc_cmd=rc_cmd_in;
 }
@@ -188,14 +190,35 @@ int buzzuav_update_flight_status(buzzvm_t vm) {
    buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1));
    buzzvm_pusht(vm);
    buzzvm_dup(vm);
+   buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_cmd", 1));
+   buzzvm_pushi(vm, rc_cmd);
+   buzzvm_tput(vm);
+   buzzvm_dup(vm);
    buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1));
-   buzzvm_pushf(vm, status);
+   buzzvm_pushi(vm, status);
    buzzvm_tput(vm); 
    buzzvm_gstore(vm);
+   //also set rc_controllers goto
+   buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
+   buzzvm_pusht(vm);
+   buzzvm_dup(vm);
+   buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
+   buzzvm_pushf(vm, rc_goto_pos[0]);
+   buzzvm_tput(vm);
+   buzzvm_dup(vm);
+   buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
+   buzzvm_pushf(vm, rc_goto_pos[1]);
+   buzzvm_tput(vm);
+   buzzvm_dup(vm);
+   buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
+   buzzvm_pushf(vm, rc_goto_pos[2]);
+   buzzvm_tput(vm);
+   buzzvm_gstore(vm);
    return vm->state;
 }
 
 
+
 /****************************************/
 /*To do !!!!!*/
 /****************************************/

src/roscontroller.cpp

@@ -92,6 +92,7 @@ namespace rosbzz_node{
 		flight_status_sub =n_c.subscribe("/mav/flight_status",100, &roscontroller::flight_status_update,this);
   		/*publishers*/
 		payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
+		cout<<out_payload<<endl;
 		/* Clients*/
   		mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
 
@@ -122,15 +123,19 @@ namespace rosbzz_node{
 	}
 	
 	void roscontroller::prepare_msg_and_publish(){
-		/*prepare the goto publish message */
-    		double* goto_pos = buzzuav_closures::getgoto();
-		cmd_srv.request.param1 = goto_pos[0];
-    		cmd_srv.request.param2 = goto_pos[1];
-    		cmd_srv.request.param3 = goto_pos[2];
-    		cmd_srv.request.command =  buzzuav_closures::getcmd();
+		
  		/* flight controller client call*/	
-    		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"); 
+		if (bzz_old_cmd != buzzuav_closures::getcmd()) {  
+			/*prepare the goto publish message */
+    			double* goto_pos = buzzuav_closures::getgoto();
+			cmd_srv.request.param1 = goto_pos[0];
+    			cmd_srv.request.param2 = goto_pos[1];
+    			cmd_srv.request.param3 = goto_pos[2];
+    			cmd_srv.request.command =  buzzuav_closures::getcmd();  		
+			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"); 
+  			bzz_old_cmd = cmd_srv.request.command;
+		} 		
     		/*obtain Pay load to be sent*/  
    		uint64_t* payload_out_ptr= buzz_utility::out_msg_process();
     		uint64_t  position[3];
@@ -276,12 +281,12 @@ namespace rosbzz_node{
 				res.success = true;
 				break;
 			case mavros_msgs::CommandCode::NAV_WAYPOINT:
-   				ROS_INFO("RC_Call: GO TO!!!! x = %f , y = %f , Z = %f",req.param1,req.param2,req.param3);
+   				ROS_INFO("RC_Call: GO TO!!!! ");
 				double rc_goto[3];
-   				rc_goto[0]=req.param1;
-				rc_goto[1]=req.param2;
-				rc_goto[2]=req.param3;
-				buzzuav_closures::set_goto(rc_goto);
+   				rc_goto[0]=req.x;
+				rc_goto[1]=req.y;
+				rc_goto[2]=req.z;
+				buzzuav_closures::rc_set_goto(rc_goto);
 				rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
 				buzzuav_closures::rc_call(rc_cmd);
 				res.success = true;

src/test.bzz

@@ -1,6 +1,8 @@
 # Executed once at init time.
 function init() {
 i = 1
+a = 0
+val = 0
 }
  
 # Executed at each time step.
@@ -15,11 +17,19 @@ neighbors.listen("Take",
 neighbors.listen("key",
    function(vid, value, rid) {
       print("Got (", vid, ",", value, ") from robot #", rid)
+      val = value      
    }
 )
+print(val)
+if ((val == 23) and (a == 0)) { 
+	uav_takeoff()
+	a=1
+	}
+      if (a == 10) uav_land() 
+      if (a != 0) a = a+1
 }
 else{
-neighbors.broadcast("key", "yes")
+neighbors.broadcast("key", 23)
 neighbors.broadcast("Take", "no")
 }