From b9b845802b7cf2d17b4f63975344eb713e0e239a Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Wed, 8 Feb 2017 11:23:12 -0500
Subject: [PATCH] fix gps precision

---
 include/buzzuav_closures.h |  1 +
 src/buzz_utility.cpp       |  3 +++
 src/buzzuav_closures.cpp   | 10 +++++++++-
 src/roscontroller.cpp      |  4 ++--
 src/test1.bzz              | 11 +++++++----
 5 files changed, 22 insertions(+), 7 deletions(-)

diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h
index 428c745..b334d53 100644
--- a/include/buzzuav_closures.h
+++ b/include/buzzuav_closures.h
@@ -20,6 +20,7 @@ int buzzros_print(buzzvm_t vm);
  * buzzuav_goto(latitude,longitude,altitude) function in Buzz
  * commands the UAV to go to a position supplied
  */
+int buzzuav_moveto(buzzvm_t vm);
 int buzzuav_goto(buzzvm_t vm);
 /* Returns the current command from local variable*/
 int getcmd();
diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index a48cf0a..931c1eb 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -259,6 +259,9 @@ namespace buzz_utility{
    		buzzvm_pushs(VM,  buzzvm_string_register(VM, "log", 1));
                 buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
                 buzzvm_gstore(VM);
+                buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_moveto", 1));
+   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
+   		buzzvm_gstore(VM);
                 buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_goto", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
    		buzzvm_gstore(VM);
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index 867e8d5..613f050 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -107,7 +107,8 @@ void gps_from_rb(double  range, double bearing, double out[3]) {
 double hcpos1[4] = {45.564489, -73.562537, 45.564140, -73.562336};
 double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958};
 double hcpos3[4] = {45.564969, -73.562838, 45.564636, -73.563677};
-int buzzuav_goto(buzzvm_t vm) {
+
+int buzzuav_moveto(buzzvm_t vm) {
    buzzvm_lnum_assert(vm, 2);
    buzzvm_lload(vm, 1); /* dx */
    buzzvm_lload(vm, 2); /* dy */
@@ -160,6 +161,13 @@ int buzzuav_goto(buzzvm_t vm) {
    return buzzvm_ret0(vm);
 }
 
+int buzzuav_goto(buzzvm_t vm) {
+   set_goto(rc_goto_pos);
+   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;
+   return buzzvm_ret0(vm);
+}
 /******************************/
 
 double* getgoto(){
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index af14b8c..16387be 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -609,8 +609,8 @@ namespace rosbzz_node{
 			case mavros_msgs::CommandCode::NAV_WAYPOINT:
    				ROS_INFO("RC_Call: GO TO!!!! ");
 				double rc_goto[3];
-   				rc_goto[0]=req.x;
-				rc_goto[1]=req.y;
+   				rc_goto[0]=req.x/pow(10,7);
+				rc_goto[1]=req.y/pow(10,7);
 				rc_goto[2]=req.z;
 				buzzuav_closures::rc_set_goto(rc_goto);
 				rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
diff --git a/src/test1.bzz b/src/test1.bzz
index 7ae18bd..caa76ba 100644
--- a/src/test1.bzz
+++ b/src/test1.bzz
@@ -16,7 +16,7 @@ TARGET_ALTITUDE = 10.0
  
 # Lennard-Jones parameters
 TARGET     = 50.0	#0.000001001
-EPSILON    = 200.0	#0.001
+EPSILON    = 0.5	#0.001
  
 # Lennard-Jones interaction magnitude
 function lj_magnitude(dist, target, epsilon) {
@@ -41,8 +41,8 @@ function hexagon() {
   if(neighbors.count() > 0)
     math.vec2.scale(accum, 1.0 / neighbors.count())
   # Move according to vector
-  print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
-  uav_goto(accum.x, accum.y)
+#  print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
+#  uav_moveto(accum.x, accum.y)
 }
  
 ########################################
@@ -55,7 +55,7 @@ function hexagon() {
 # Constants
 #
 BARRIER_VSTIG = 1
-ROBOTS = 3 # number of robots in the swarm
+ROBOTS = 1 # number of robots in the swarm
 
 #
 # Sets a barrier
@@ -142,6 +142,9 @@ function step() {
 		flight.rc_cmd=0
 		statef=land
 		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16 and flight.status==2) {
+		flight.rc_cmd=0
+		uav_goto()
 	}
 statef()
 }