From 31718bc01e8fd576bdc8809c484f10d7a21a0353 Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Thu, 27 Apr 2017 12:36:12 -0400
Subject: [PATCH] set home only once

---
 launch/rosbuzz-testing-sitl.launch |  2 ++
 src/roscontroller.cpp              | 23 ++++++++++++++++-------
 src/testflockfev.bzz               | 10 +++++++---
 3 files changed, 25 insertions(+), 10 deletions(-)

diff --git a/launch/rosbuzz-testing-sitl.launch b/launch/rosbuzz-testing-sitl.launch
index 141f0a0..e086595 100644
--- a/launch/rosbuzz-testing-sitl.launch
+++ b/launch/rosbuzz-testing-sitl.launch
@@ -33,6 +33,8 @@
 		<param name="in_payload" value="/inMavlink"/>
 		<param name="out_payload" value="/outMavlink"/>
 		<param name="xbee_status_srv" value="/xbee_status"/>
+		<param name="xbee_plugged" value="false"/>
+		<param name="name" value="solos1"/>
 		<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
 	</node>
 	
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index abd99e4..a948de0 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -74,6 +74,7 @@ namespace rosbzz_node{
                         //////////////////////////////////////////////////////
 			while (ros::ok() && !buzz_utility::buzz_script_done())
   			{
+				std::cout<<std::setprecision(17)<<"Home: " << home[0] << home[1] << std::endl;
       				/*Update neighbors position inside Buzz*/
      				buzz_utility::neighbour_pos_callback(neighbours_pos_map);
 				/*Neighbours of the robot published with id in respective topic*/
@@ -219,8 +220,8 @@ namespace rosbzz_node{
 		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
 		mode_client =  n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
 		mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
-                if(rcclient==true)
-                    service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this);
+		if(rcclient==true)
+			service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this);
 		ROS_INFO("Ready to receive Mav Commands from RC client");
 		xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
 		stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
@@ -425,12 +426,19 @@ namespace rosbzz_node{
 				break;
 			case mavros_msgs::CommandCode::NAV_TAKEOFF:
 				if(!armstate){
+
 					SetMode("LOITER", 0);
 					armstate = 1;
 					Arm();
 					ros::Duration(0.5).sleep();
-                                        // Registering HOME POINT.
-                                        home[0] = cur_pos[0];home[1] = cur_pos[1];home[2] = cur_pos[2];
+					// Registering HOME POINT.
+					if(home[0] == 0){
+						//test #1: set home only once -- ok
+						home[0] = cur_pos[0]; home[1] = cur_pos[1]; home[2] = cur_pos[2];
+						//test #2: set home mavros -- nope
+						//SetMavHomePosition(cur_pos[0], cur_pos[1], cur_pos[2]);
+
+					}
 				}
 				if(current_mode != "GUIDED")
 					SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
@@ -510,9 +518,9 @@ namespace rosbzz_node{
 	void roscontroller::set_cur_pos(double latitude,
 			 double longitude,
 			 double altitude){
-		cur_pos [0] =latitude;
-		cur_pos [1] =longitude;
-		cur_pos [2] =altitude;
+		cur_pos [0] = latitude;
+		cur_pos [1] = longitude;
+		cur_pos [2] = altitude;
 	}
 
 	/*-----------------------------------------------------------
@@ -781,6 +789,7 @@ namespace rosbzz_node{
 		}
 	}
 
+
 	void roscontroller::SetStreamRate(int id, int rate, int on_off){
 		mavros_msgs::StreamRate message;
 		message.request.stream_id = id;
diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz
index f3b58f8..c3573cd 100644
--- a/src/testflockfev.bzz
+++ b/src/testflockfev.bzz
@@ -49,8 +49,12 @@ function hexagon() {
   	timeW =0
   	statef=land
   	} else {
-  	timeW = timeW+1
-  	uav_moveto(0.2,0.0)
+  		if(timeW >= (WAIT_TIMEOUT / 2)){
+  			uav_moveto(5.0,0.0)
+  		} else {
+  			uav_moveto(0.0,5.0)
+  		}
+  		timeW = timeW+1
   	}
 }
  
@@ -86,7 +90,7 @@ function barrier_ready() {
 #
 # Executes the barrier
 #
-WAIT_TIMEOUT = 200
+WAIT_TIMEOUT = 300
 timeW=0
 function barrier_wait(threshold, transf) {
   barrier.get(id)