Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

minor fix to user rb

This commit is contained in:
David St-Onge 2017-05-15 16:29:33 -04:00
parent 1920f86326
commit 99aaee0e71
4 changed files with 17 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/buzz_utility.h
View File

@ -34,6 +34,7 @@ int make_table(buzzobj_t* t);
int buzzusers_add(int id, double latitude, double longitude, double altitude); int buzzusers_add(int id, double latitude, double longitude, double altitude);
int buzzusers_reset(); int buzzusers_reset();
int compute_users_rb(); int compute_users_rb();
int create_stig_tables();
void in_msg_append(uint64_t* payload); void in_msg_append(uint64_t* payload);

5
src/buzz_utility.cpp
View File

@ -41,7 +41,8 @@ namespace buzz_utility{
void update_users(){ void update_users(){
if(users_map.size()>0) { if(users_map.size()>0) {
/* Reset users information */ /* Reset users information */
buzzusers_reset(); // buzzusers_reset();
create_stig_tables();
/* Get user id and update user information */ /* Get user id and update user information */
map< int, Pos_struct >::iterator it; map< int, Pos_struct >::iterator it;
for (it=users_map.begin(); it!=users_map.end(); ++it){ for (it=users_map.begin(); it!=users_map.end(); ++it){
@ -370,7 +371,7 @@ namespace buzz_utility{
return VM->state; return VM->state;
} }
static int create_stig_tables() { int create_stig_tables() {
/* /*
// usersvstig = stigmergy.create(123) // usersvstig = stigmergy.create(123)
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));

18
src/buzzuav_closures.cpp
View File

@ -7,6 +7,7 @@
*/ */
//#define _GNU_SOURCE //#define _GNU_SOURCE
#include "buzzuav_closures.h" #include "buzzuav_closures.h"
#include "math.h"
namespace buzzuav_closures{ namespace buzzuav_closures{
@ -89,10 +90,10 @@ namespace buzzuav_closures{
} }
void rb_from_gps(double nei[], double out[], double cur[]){ void rb_from_gps(double nei[], double out[], double cur[]){
double d_lon = nei[1] - cur[1]; double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0]; double d_lat = nei[0] - cur[0];
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
out[0] = sqrt(ned_x*ned_x+ned_y*ned_y); out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
out[1] = atan2(ned_y,ned_x); out[1] = atan2(ned_y,ned_x);
out[2] = 0.0; out[2] = 0.0;
@ -187,10 +188,13 @@ namespace buzzuav_closures{
double rb[3]; double rb[3];
rb_from_gps(tmp, rb, cur_pos); 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 0;
return users_add2localtable(vm, uid, rb[0], rb[1]);
} }
/*----------------------------------------/ /*----------------------------------------/

10
src/roscontroller.cpp
View File

@ -676,20 +676,14 @@ namespace rosbzz_node{
} }
void roscontroller::users_pos(const rosbuzz::neigh_pos data){ 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(); 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) if(n>0)
{ {
for(int it=0; it<n; ++it) for(int it=0; it<n; ++it)
{ {
us[0] = data.pos_neigh[it].latitude; 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);
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);
} }
} }