From a7140c5551bb14c1c2b7a4924406bf439c59e1d3 Mon Sep 17 00:00:00 2001
From: Mario Alexis Labrana <mario-alexis.labrana-ruiz@polymtl.ca>
Date: Sat, 28 Jan 2017 11:24:54 -0500
Subject: [PATCH] minor coord. fix

---
 launch/rosbuzzm100.launch | 4 ++--
 src/roscontroller.cpp     | 7 ++++---
 2 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/launch/rosbuzzm100.launch b/launch/rosbuzzm100.launch
index 74de01b..ea67dcb 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="../ROS_WS/src/ROSBuzz/src/test1.bzz" />
+		<param name="bzzfile_name" value="../djiros_ws/src/ROSBuzz/src/test1.bzz" />
                 <param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="/buzzcmd" />
 		<param name="fcclient_name" value="/dji_mavcmd" />
@@ -10,7 +10,7 @@
 		<param name="out_payload" value="/outMavlink"/>
 		<param name="robot_id" value="3"/>
 		<param name="No_of_Robots" value="3"/>
-		<param name="stand_by" value="../ROS_WS/src/ROSBuzz/src/stand_by.bo"/>
+		<param name="stand_by" value="../djiros_ws/src/ROSBuzz/src/stand_by.bo"/>
 		</node>
 		
 		
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 981257b..962c9bf 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -294,12 +294,13 @@ namespace rosbzz_node{
 
 	}
 
+	#define EARTH_RADIUS 6371000.0
 	/*convert from spherical to cartesian coordinate system callback */
 	void roscontroller::cvt_cartesian_coordinates(double spherical_pos_payload [], double out[]){
 		double latitude, longitude, rho;
 		latitude = spherical_pos_payload[0] / 180.0 * M_PI;
 		longitude = spherical_pos_payload[1] / 180.0 * M_PI;
-		rho = spherical_pos_payload[2]+6371000; //centered on Earth
+		rho = spherical_pos_payload[2]+EARTH_RADIUS; //centered on Earth
 		try {
 			out[0] = cos(latitude) * cos(longitude) * rho;
     			out[1] = cos(latitude) * sin(longitude) * rho;
@@ -412,8 +413,8 @@ namespace rosbzz_node{
 		z = cartesian_pos_payload[2];
 		try {
        		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);
+		out[1]=atan2(y,x);
+		out[2]=atan2((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;
    		}