From e01e6e6c5e706ddbacf7e7ecde7408d939fb8588 Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Mon, 17 Apr 2017 11:51:07 -0400
Subject: [PATCH] initialization

---
 src/buzzuav_closures.cpp |  1 +
 src/roscontroller.cpp    | 16 +++++++++++-----
 src/testflockfev.bzz     |  2 +-
 3 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index 6fa15b9..2f1f754 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -109,6 +109,7 @@ namespace buzzuav_closures{
 	   printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
 	   gps_from_rb(d, b, goto_pos);
 	   cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
+       printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
 	   printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f  \n",goto_pos[0], goto_pos[1], goto_pos[2]);
 	   buzz_cmd= COMMAND_MOVETO; // TO DO what should we use
 	   return buzzvm_ret0(vm);
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 47473d1..1e086b3 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -26,6 +26,9 @@ namespace rosbzz_node{
 		GetRobotId();
 		setpoint_counter = 0;
 		fcu_timeout = TIMEOUT;
+		home[0] = 0.0;
+		home[1] = 0.0;
+		home[2] = 0.0;
 	}
 
 	/*---------------------
@@ -43,6 +46,7 @@ namespace rosbzz_node{
 	void roscontroller::GetRobotId()
 	{
 
+		/*
 		mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
         	mavros_msgs::ParamGet::Response robot_id_srv_response;
 		while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
@@ -51,8 +55,8 @@ namespace rosbzz_node{
 		}
 
 		robot_id=robot_id_srv_response.value.integer;
-
-		//robot_id = 8;
+		*/
+		robot_id = 8;
 	}
 
 	/*-------------------------------------------------
@@ -512,9 +516,9 @@ namespace rosbzz_node{
 
 	void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){
  		double d_lon = nei[1] - cur[1];
-        	double d_lat = nei[0] - cur[0];
-        	out[0] = DEG2RAD(d_lat) * EARTH_RADIUS;
-        	out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
+		double d_lat = nei[0] - cur[0];
+		out[0] = DEG2RAD(d_lat) * EARTH_RADIUS;
+		out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
 		out[2] = cur[2];
 	}
 
@@ -796,6 +800,8 @@ namespace rosbzz_node{
 		moveMsg.pose.orientation.z = 0;
 		moveMsg.pose.orientation.w = 1;
 
+		std::cout<<"Home (0,1,2): " << home[0] <<", " << home[1] << ", " << home[2] <<std::endl;
+
 		localsetpoint_pub.publish(moveMsg);
 
 		ROS_INFO("Sent local NON RAW position message!");
diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz
index 30e5b80..f3b58f8 100644
--- a/src/testflockfev.bzz
+++ b/src/testflockfev.bzz
@@ -50,7 +50,7 @@ function hexagon() {
   	statef=land
   	} else {
   	timeW = timeW+1
-  	uav_moveto(0.0,0.0)
+  	uav_moveto(0.2,0.0)
   	}
 }