From 67976b1be2c0e633ae543fb98572ccb042a3b1da Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Fri, 27 Jan 2017 18:00:41 -0500
Subject: [PATCH] minor fix in goto

---
 launch/rosbuzzm100.launch |  2 +-
 src/buzzuav_closures.cpp  | 11 +++++++----
 src/roscontroller.cpp     |  2 +-
 src/test1.bzz             |  5 ++---
 4 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/launch/rosbuzzm100.launch b/launch/rosbuzzm100.launch
index 3dd8394..74de01b 100644
--- a/launch/rosbuzzm100.launch
+++ b/launch/rosbuzzm100.launch
@@ -9,7 +9,7 @@
 		<param name="in_payload" value="/inMavlink"/>
 		<param name="out_payload" value="/outMavlink"/>
 		<param name="robot_id" value="3"/>
-		<param name="No_of_Robots" value="2"/>
+		<param name="No_of_Robots" value="3"/>
 		<param name="stand_by" value="../ROS_WS/src/ROSBuzz/src/stand_by.bo"/>
 		</node>
 		
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index bc49964..8eeb005 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -16,6 +16,7 @@ static uint8_t status;
 static int cur_cmd = 0;
 static int rc_cmd=0;
 static int buzz_cmd=0;
+static float height=0;
 /****************************************/
 /****************************************/
 
@@ -61,12 +62,13 @@ int buzzros_print(buzzvm_t vm) {
 
 /****************************************/
 /****************************************/
+#define EARTH_RADIUS 6371000
 /*convert from spherical to cartesian coordinate system callback */
         void 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;
@@ -82,7 +84,7 @@ int buzzros_print(buzzvm_t vm) {
                 y = cartesian_pos_payload[1];
                 z = cartesian_pos_payload[2];
                 try {
-                out[2]=sqrt(pow(x,2.0)+pow(y,2.0)+pow(z,2.0))-6371000;
+                out[2]=sqrt(pow(x,2.0)+pow(y,2.0)+pow(z,2.0))-EARTH_RADIUS;
                 out[1]=atan2(y,x)*180/M_PI;
                 out[0]=atan2(z,(sqrt(pow(x,2.0)+pow(y,2.0))))*180/M_PI;
                 } catch (std::overflow_error e) {
@@ -110,7 +112,7 @@ int buzzuav_goto(buzzvm_t vm) {
    spherical_coordinates(goto_cartesian, goto_pos);
 //   goto_pos[0]=cur_pos[0]+dlat*180/M_PI;
 //   goto_pos[1]=cur_pos[1]+dlon*180/M_PI;
-//   goto_pos[2]=cur_pos[2];
+   goto_pos[2]=height;	// force a constant altitude
    cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
    printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f  \n",goto_pos[0],goto_pos[1],goto_pos[2]);
    buzz_cmd=2;
@@ -157,7 +159,8 @@ 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; 
+   goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value;
+   height = goto_pos[2];
    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 32e6619..6cc59fe 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -431,7 +431,7 @@ namespace rosbzz_node{
 		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));  
-//		cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[2] << ", "<< cvt_neighbours_pos_payload[3] << endl;
+		cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[2] << ", "<< cvt_neighbours_pos_payload[3] << endl;
 //		cout << "Rel Test Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_test[0] << ", "<< cvt_neighbours_pos_test[2] << ", "<< cvt_neighbours_pos_test[3] << endl;
 		/*pass neighbour position to local maintaner*/
 		buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]);
diff --git a/src/test1.bzz b/src/test1.bzz
index 6172eb4..fd793d7 100644
--- a/src/test1.bzz
+++ b/src/test1.bzz
@@ -1,4 +1,3 @@
-
 # 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"
@@ -15,8 +14,8 @@ neighbors.broadcast(updated, update_no)
 TARGET_ALTITUDE = 10.1
  
 # Lennard-Jones parameters
-TARGET     = 0.000001001
-EPSILON    = 0.001
+TARGET     = 5	#0.000001001
+EPSILON    = 20	#0.001
  
 # Lennard-Jones interaction magnitude
 function lj_magnitude(dist, target, epsilon) {