From a0fb91adb30484e22e23f3ce8a796217163af32c Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Sun, 19 Feb 2017 16:42:53 -0500
Subject: [PATCH] arm fixes

---
 log.txt                  | 3 +++
 src/buzz_utility.cpp     | 3 +++
 src/buzzuav_closures.cpp | 2 +-
 src/roscontroller.cpp    | 2 +-
 4 files changed, 8 insertions(+), 2 deletions(-)
 create mode 100644 log.txt

diff --git a/log.txt b/log.txt
new file mode 100644
index 0000000..e038009
--- /dev/null
+++ b/log.txt
@@ -0,0 +1,3 @@
+
+2017-01-08-14-24-24    enter TranslateFunc
+2017-01-08-14-24-24    start to read frames!
\ No newline at end of file
diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index edb71c8..741d24b 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -233,6 +233,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::buzzuav_moveto));
+   		buzzvm_gstore(VM);
                 buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_goto", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto));
    		buzzvm_gstore(VM);
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index 334359c..3c89b30 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -99,8 +99,8 @@ void gps_from_rb(double  range, double bearing, double out[3]) {
 	double lon = cur_pos[1]*M_PI/180.0;
 //	bearing = bearing*M_PI/180.0;
  	out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing));
-	out[0] = out[0]*180.0/M_PI;
 	out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0]));
+	out[0] = out[0]*180.0/M_PI;
 	out[1] = out[1]*180.0/M_PI;
 	out[2] = height; //constant height.
 }
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 19234f3..0695a62 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -184,7 +184,7 @@ namespace rosbzz_node{
 		payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
 		neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);	
 		/* Service Clients*/
-		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armingclient);
+		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
 		mode_client =  n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
 		mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);