From 99aaee0e713efc7d807dd66461bf40dbb8a350c1 Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Mon, 15 May 2017 16:29:33 -0400
Subject: [PATCH] minor fix to user rb

---
 include/buzz_utility.h   |  1 +
 src/buzz_utility.cpp     |  5 +++--
 src/buzzuav_closures.cpp | 18 +++++++++++-------
 src/roscontroller.cpp    | 10 ++--------
 4 files changed, 17 insertions(+), 17 deletions(-)

diff --git a/include/buzz_utility.h b/include/buzz_utility.h
index 381b417..cc0ef6a 100644
--- a/include/buzz_utility.h
+++ b/include/buzz_utility.h
@@ -34,6 +34,7 @@ int make_table(buzzobj_t* t);
 int buzzusers_add(int id, double latitude, double longitude, double altitude);
 int buzzusers_reset();
 	int compute_users_rb();
+	int create_stig_tables();
 
 void in_msg_append(uint64_t* payload);
 
diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index fe3e8c8..eb07080 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -41,7 +41,8 @@ namespace buzz_utility{
 	void update_users(){
 		if(users_map.size()>0) {
 			/* Reset users information */
-			buzzusers_reset();
+//			buzzusers_reset();
+			create_stig_tables();
 			/* Get user id and update user information */
 			map< int, Pos_struct >::iterator it;
 			for (it=users_map.begin(); it!=users_map.end(); ++it){
@@ -370,7 +371,7 @@ namespace buzz_utility{
    	return VM->state;
 	}
 
-static int create_stig_tables() {
+int create_stig_tables() {
 /*
    		// usersvstig = stigmergy.create(123)
         buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index c75357d..72b42e5 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -7,6 +7,7 @@
  */
 //#define _GNU_SOURCE
 #include "buzzuav_closures.h"
+#include "math.h"
 
 namespace buzzuav_closures{
 
@@ -89,10 +90,10 @@ namespace buzzuav_closures{
 	}
 
 	void rb_from_gps(double nei[], double out[], double cur[]){   
-       	double d_lon = nei[1] - cur[1];
-       	double d_lat = nei[0] - cur[0];
-        double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
-        double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
+       		double d_lon = nei[1] - cur[1];
+       		double d_lat = nei[0] - cur[0];
+        	double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
+        	double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
 		out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
 		out[1] = atan2(ned_y,ned_x);
 		out[2] = 0.0;
@@ -187,10 +188,13 @@ namespace buzzuav_closures{
 	   double rb[3];
 
 	   rb_from_gps(tmp, rb, cur_pos);
+	   if(fabs(rb[0])<100.0) {
+	   	printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
+		return users_add2localtable(vm, uid, rb[0], rb[1]);
+	   } else
+		printf(" ---------- User too far %f\n",rb[0]);
 
-	   //printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
-
-	   return users_add2localtable(vm, uid, rb[0], rb[1]);
+		return 0;
 	}
 
 	/*----------------------------------------/
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 2af3440..dd2cfa1 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -676,20 +676,14 @@ namespace rosbzz_node{
 	}
 
 	void roscontroller::users_pos(const rosbuzz::neigh_pos data){
-		//ROS_INFO("Altitude out: %f", cur_rel_altitude);
 
-                double us[3];
                 int n = data.pos_neigh.size();
-        //	ROS_INFO("Neighbors array size: %i\n", n);
+        //	ROS_INFO("Users array size: %i\n", n);
                 if(n>0)
                 {
                         for(int it=0; it<n; ++it)
                         {
-                                us[0] = data.pos_neigh[it].latitude;
-                                us[1] = data.pos_neigh[it].longitude;
-                                us[2] = data.pos_neigh[it].altitude;
-								
-								buzz_utility::add_user(data.pos_neigh[it].position_covariance_type,data.pos_neigh[it].latitude, data.pos_neigh[it].longitude, data.pos_neigh[it].altitude);
+				buzz_utility::add_user(data.pos_neigh[it].position_covariance_type,data.pos_neigh[it].latitude, data.pos_neigh[it].longitude, data.pos_neigh[it].altitude);
                         }
 
                 }