From 1ac062ea3d4d9d3ba01f1e6835fc63cacf4e9cf3 Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Thu, 26 Jan 2017 19:23:33 -0500
Subject: [PATCH] more coord. fixes

---
 include/roscontroller.h   |  4 ++--
 launch/rosbuzzm100.launch |  2 +-
 src/buzzuav_closures.cpp  | 21 ++++++++++++--------
 src/roscontroller.cpp     | 42 +++++++++++++++++++--------------------
 src/test1.bzz             |  2 ++
 5 files changed, 39 insertions(+), 32 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index eb9d1b5..ee44f0b 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -86,10 +86,10 @@ private:
 			 double longitude,
 			 double altitude);
 	/*convert from cartesian to spherical coordinate system callback */
-	double* cvt_spherical_coordinates(double neighbours_pos_payload []);
+	void cvt_spherical_coordinates(double neighbours_pos_payload [], double out[]);
 
 	/*convert from spherical to cartesian coordinate system callback */
-	double* cvt_cartesian_coordinates(double neighbours_pos_payload []);
+	void cvt_cartesian_coordinates(double neighbours_pos_payload [], double out[]);
 	
 	/*battery status callback*/ 
 	void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
diff --git a/launch/rosbuzzm100.launch b/launch/rosbuzzm100.launch
index 4af488e..0c5c75c 100644
--- a/launch/rosbuzzm100.launch
+++ b/launch/rosbuzzm100.launch
@@ -2,7 +2,7 @@
 
 <launch>
 		<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
-		<param name="bzzfile_name" value="../djiros_ws/src/ROSBuzz/src/test1.bzz" />
+		<param name="bzzfile_name" value="../ROS_WS/src/ROSBuzz/src/test1.bzz" />
                 <param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="/buzzcmd" />
 		<param name="fcclient_name" value="/dji_mavcmd" />
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index 5cd8aaf..a743bbb 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -63,16 +63,21 @@ int buzzros_print(buzzvm_t vm) {
 /****************************************/
 
 int buzzuav_goto(buzzvm_t vm) {
-   buzzvm_lnum_assert(vm, 3);
-   buzzvm_lload(vm, 1); /* Altitude */
-   buzzvm_lload(vm, 2); /* Longitude */
-   buzzvm_lload(vm, 3); /* Latitude */
-   buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
+   buzzvm_lnum_assert(vm, 2);
+   buzzvm_lload(vm, 1); /* dx */
+   buzzvm_lload(vm, 2); /* dy */
+   //buzzvm_lload(vm, 3); /* Latitude */
+   //buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
    buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
    buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
-   goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value; 
-   goto_pos[1]=buzzvm_stack_at(vm, 2)->f.value; 
-   goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value; 
+   float dy = buzzvm_stack_at(vm, 1)->f.value;
+   float dx = buzzvm_stack_at(vm, 2)->f.value;
+   //goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value;
+   double dlat=dx/(6371000+cur_pos[2]);
+   double dlon=dy/cos(cur_pos[0]/180*3.1416)/(6371000+cur_pos[2]);
+   goto_pos[0]=cur_pos[0]+dlat*180/3.1416;
+   goto_pos[1]=cur_pos[1]+dlon*180/3.1416;
+   goto_pos[2]=cur_pos[2];
    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;
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 33d4fa6..1e51ce5 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -247,36 +247,33 @@ namespace rosbzz_node{
 	}
 
 	/*convert from spherical to cartesian coordinate system callback */
-	double* roscontroller::cvt_cartesian_coordinates(double neighbours_pos_payload []){
+	void roscontroller::cvt_cartesian_coordinates(double spherical_pos_payload [], double out[]){
 		double latitude, longitude, rho;
-		latitude=neighbours_pos_payload[0];
-		longitude = neighbours_pos_payload[1];
-		rho=neighbours_pos_payload[2]+6371000; //centered on Earth
+		latitude = spherical_pos_payload[0] / 180 * 3.1416;
+		longitude = spherical_pos_payload[1] / 180 * 3.1416;
+		rho = spherical_pos_payload[2]+6371000; //centered on Earth
 		try {
-			neighbours_pos_payload[0] = cos(latitude) * cos(longitude) * rho;
-    			neighbours_pos_payload[1] = cos(latitude) * sin(longitude) * rho;
-    			neighbours_pos_payload[2] = sin(latitude) * rho; // z is 'up'
+			out[0] = cos(latitude) * cos(longitude) * rho;
+    			out[1] = cos(latitude) * sin(longitude) * rho;
+    			out[2] = sin(latitude) * rho; // z is 'up'
    		} catch (std::overflow_error e) {
         	std::cout << e.what() << " Error in convertion to cartesian coordinate system "<<endl;
    		}
-		
-		return neighbours_pos_payload;
 	}
+
 	/*convert from cartesian to spherical coordinate system callback */
-	double* roscontroller::cvt_spherical_coordinates(double neighbours_pos_payload []){
+	void roscontroller::cvt_spherical_coordinates(double cartesian_pos_payload [], double out[]){
 		double x, y, z;
-		x = neighbours_pos_payload[0];
-		y = neighbours_pos_payload[1];
-		z = neighbours_pos_payload[2];
+		x = cartesian_pos_payload[0];
+		y = cartesian_pos_payload[1];
+		z = cartesian_pos_payload[2];
 		try {
-       		neighbours_pos_payload[0]=sqrt(pow(x,2.0)+pow(y,2.0)+pow(z,2.0));
-		neighbours_pos_payload[1]=atan(y/x);
-		neighbours_pos_payload[2]=atan((sqrt(pow(x,2.0)+pow(y,2.0)))/z);
+       		out[0]=sqrt(pow(x,2.0)+pow(y,2.0)+pow(z,2.0));
+		out[1]=atan(y/x);
+		out[2]=atan((sqrt(pow(x,2.0)+pow(y,2.0)))/z);
    		} catch (std::overflow_error e) {
         	std::cout << e.what() << " Error in convertion to spherical coordinate system "<<endl;
    		}
-		
-		return neighbours_pos_payload;
 	}
 
 	/*battery status callback*/ 
@@ -320,12 +317,15 @@ namespace rosbzz_node{
 		buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0],neighbours_pos_payload[1],neighbours_pos_payload[2]);
 		//cout<<"obt lat ,long alt"<<neighbours_pos_payload[0]<<neighbours_pos_payload[1]<<neighbours_pos_payload[2];
 		/*Convert obtained position to relative CARTESIAN position*/
-		double* cartesian_neighbours_pos = cvt_cartesian_coordinates(neighbours_pos_payload);
-		double* cartesian_cur_pos = cvt_cartesian_coordinates(cur_pos);
+		double cartesian_neighbours_pos[3], cartesian_cur_pos[3];
+		cvt_cartesian_coordinates(neighbours_pos_payload, cartesian_neighbours_pos);
+		cvt_cartesian_coordinates(cur_pos, cartesian_cur_pos);
+		/*Compute the relative position*/
 		for(i=0;i<3;i++){
 			neighbours_pos_payload[i]=cartesian_neighbours_pos[i]-cartesian_cur_pos[i];
 		}
-		double* cvt_neighbours_pos_payload = cvt_spherical_coordinates(neighbours_pos_payload);
+		double cvt_neighbours_pos_payload[3];
+		cvt_spherical_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload);
 		/*Extract robot id of the neighbour*/
  		uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));  
 		/*pass neighbour position to local maintaner*/
diff --git a/src/test1.bzz b/src/test1.bzz
index 1d58cb8..ed14122 100644
--- a/src/test1.bzz
+++ b/src/test1.bzz
@@ -1,3 +1,5 @@
+
+
 # We need this for 2D vectors
 # Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
 include "/home/ubuntu/buzz/src/include/vec2.bzz"