change in filght controller cmd handling

include/buzzuav_closures.h

@@ -7,6 +7,7 @@
 #include "mavros_msgs/CommandCode.h"
 #include "ros/ros.h"
 
+
 namespace buzzuav_closures{
 /*
  * prextern int() function in Buzz
@@ -71,5 +72,7 @@ int buzzuav_update_flight_status(buzzvm_t vm);
  */
 int buzzuav_update_prox(buzzvm_t vm);
 
+int bzz_cmd();
+
 //#endif
 }

include/roscontroller.h

@@ -11,7 +11,6 @@
 #include "sensor_msgs/NavSatStatus.h"
 #include <sstream>
 #include <buzz/buzzasm.h>
-#include "buzzuav_closures.h"
 #include "buzz_utility.h"
 #include "uav_utility.h"
 #include <stdlib.h>
@@ -20,6 +19,7 @@
 #include <signal.h>
 #include <ostream>
 #include <map>
+#include "buzzuav_closures.h"
 
 using namespace std;
 
@@ -40,9 +40,8 @@ private:
 	std::map< int,  buzz_utility::Pos_struct> neighbours_pos_map;
 	int timer_step=0;
 	int robot_id=0;
-        int oldcmdID=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;

src/buzzuav_closures.cpp

@@ -15,6 +15,7 @@ static double cur_pos[3];
 static uint8_t status;
 static int cur_cmd = 0;
 static int rc_cmd=0;
+static int buzz_cmd=0;
 /****************************************/
 /****************************************/
 
@@ -74,6 +75,7 @@ int buzzuav_goto(buzzvm_t vm) {
    goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value; 
    cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
    printf(" Buzz requested Go To, to Latitude: %15f , Longitude: %15f, Altitude: %15f  \n",goto_pos[0],goto_pos[1],goto_pos[2]);
+   buzz_cmd=2;
    return buzzvm_ret0(vm);
 }
 
@@ -94,6 +96,12 @@ goto_pos[2]=pos[2];
     
 }
 
+int bzz_cmd(){
+int cmd = buzz_cmd;
+buzz_cmd=0;
+return cmd;
+}
+
 void rc_set_goto(double pos[]){
 rc_goto_pos[0]=pos[0];
 rc_goto_pos[1]=pos[1];
@@ -110,18 +118,21 @@ rc_cmd=rc_cmd_in;
 int buzzuav_takeoff(buzzvm_t vm) {
    cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
    printf(" Buzz requested Take off !!! \n");
+   buzz_cmd=1;
    return buzzvm_ret0(vm);
 }
 
 int buzzuav_land(buzzvm_t vm) {
    cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
    printf(" Buzz requested Land !!! \n");
+   buzz_cmd=1;
    return buzzvm_ret0(vm);
 }
 
 int buzzuav_gohome(buzzvm_t vm) {
    cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
    printf(" Buzz requested gohome !!! \n");
+   buzz_cmd=1;
    return buzzvm_ret0(vm);
 }
 
@@ -252,4 +263,7 @@ int buzzuav_update_prox(buzzvm_t vm) {
 /****************************************/
 /****************************************/
 
+
+
 }
+

src/roscontroller.cpp

@@ -124,8 +124,15 @@ namespace rosbzz_node{
 	
 	void roscontroller::prepare_msg_and_publish(){
 		
- 		/* flight controller client call*/	
-		if (bzz_old_cmd != buzzuav_closures::getcmd()) {  
+ 		/* flight controller client call if requested from Buzz*/
+		/*FC call for takeoff,land and gohome*/
+		if (buzzuav_closures::bzz_cmd()==1){
+			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"); 
+		}
+		/*FC call for goto*/	
+		else if (buzzuav_closures::bzz_cmd() == 2) {  
 			/*prepare the goto publish message */
     			double* goto_pos = buzzuav_closures::getgoto();
 			cmd_srv.request.param1 = goto_pos[0];
@@ -134,7 +141,6 @@ namespace rosbzz_node{
     			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();

src/test1.bzz

@@ -72,7 +72,7 @@ function takeoff() {
 		barrier_set(ROBOTS,transition_to_land)
 		barrier_ready()
 	}
-	else
+	else if(flight.status!=3)
 		uav_takeoff()
 }
 function land() {
@@ -80,18 +80,17 @@ function land() {
 		barrier_set(ROBOTS,idle)
 		barrier_ready()
 	}
-	else
+	else if(flight.status!=0 and flight.status!=4)
 		uav_land()
 }
 
 
 function transition_to_land() {
 	statef=transition_to_land
-	if(tim>=100){
+	if(battery.capacity<50){
+		print("Low battery! Landing the fleet")
 		statef=land
-		tim=0
-	}else{
-		tim=tim+1
+		neighbors.broadcast("cmd", 21)
 	}
 }