From 7e1d8c5244859033ac30bca4736c6726d59d9df2 Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Wed, 25 Jan 2017 17:30:11 -0500
Subject: [PATCH] takeoff height

---
 src/buzzuav_closures.cpp | 4 ++++
 src/roscontroller.cpp    | 3 ++-
 src/test1.bzz            | 6 +++---
 3 files changed, 9 insertions(+), 4 deletions(-)

diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index a2195c4..5cd8aaf 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -116,6 +116,10 @@ rc_cmd=rc_cmd_in;
 /****************************************/
 
 int buzzuav_takeoff(buzzvm_t vm) {
+   buzzvm_lnum_assert(vm, 1);
+   buzzvm_lload(vm, 1); /* Altitude */
+   buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
+   goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value; 
    cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
    printf(" Buzz requested Take off !!! \n");
    buzz_cmd=1;
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 39415e1..4d97bec 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -156,13 +156,14 @@ namespace rosbzz_node{
  		/* flight controller client call if requested from Buzz*/
 		/*FC call for takeoff,land and gohome*/
 		int tmp = buzzuav_closures::bzz_cmd();
+    		double* goto_pos = buzzuav_closures::getgoto();
 		if (tmp == 1){
+    			cmd_srv.request.z = 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"); 
 		} else if (tmp == 2) { /*FC call for goto*/ 
 			/*prepare the goto publish message */
-    			double* goto_pos = buzzuav_closures::getgoto();
 			cmd_srv.request.x = goto_pos[0]*pow(10,7);
     			cmd_srv.request.y = goto_pos[1]*pow(10,7);
     			cmd_srv.request.z = goto_pos[2];
diff --git a/src/test1.bzz b/src/test1.bzz
index a177980..6cb8501 100644
--- a/src/test1.bzz
+++ b/src/test1.bzz
@@ -89,12 +89,12 @@ neighbors.listen("cmd",
 }
 
 function takeoff() {
-	if( flight.status == 3) {
+	if( flight.status == 2 and position.altitude >= 10.0) {
 		barrier_set(ROBOTS,hexagon)
 		barrier_ready()
 	}
-	else
-		uav_takeoff()
+	else if( flight.status !=3)
+		uav_takeoff(10.1)
 }
 function land() {
 	if( flight.status == 1) {