From abdef63da6f4806ea89b19412c1662f2d372d18c Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Wed, 15 Mar 2017 16:53:32 -0400
Subject: [PATCH] timeout in bzz

---
 src/roscontroller.cpp | 1 +
 src/testflockfev.bzz  | 5 +++--
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 2075f4f..3317fa6 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -302,6 +302,7 @@ namespace rosbzz_node{
     			pt.position.y = goto_pos[1];
     			pt.position.z = goto_pos[2];
     			pt.yaw = 0;//goto_pos[3];
+			ROS_INFO("Sending local setpoint: %.2f, %.2f, %.2f",pt.position.x,pt.position.y,pt.position.z);
 			localsetpoint_pub.publish(pt);
 		}
     		/*obtain Pay load to be sent*/  
diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz
index f74efde..cbba04d 100644
--- a/src/testflockfev.bzz
+++ b/src/testflockfev.bzz
@@ -78,7 +78,7 @@ function barrier_ready() {
 #
 # Executes the barrier
 #
-WAIT_TIMEOUT = 100
+WAIT_TIMEOUT = 500
 timeW=0
 function barrier_wait(threshold, transf) {
   barrier.get(id)
@@ -88,7 +88,7 @@ function barrier_wait(threshold, transf) {
     transf()
   } else if(timeW>=WAIT_TIMEOUT) {
     barrier = nil
-    statef=hexagon #idle
+    statef=land
     timeW=0
   }
   timeW = timeW+1
@@ -166,6 +166,7 @@ function step() {
 		neighbors.broadcast("cmd", 21)
 	} else if(flight.rc_cmd==16) {
 		flight.rc_cmd=0
+		statef = idle
 		uav_goto()
 	} else if(flight.rc_cmd==400) {
 		flight.rc_cmd=0