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_reset();
int compute_users_rb();
int create_stig_tables();
void in_msg_append(uint64_t* payload);

5
src/buzz_utility.cpp
View File

@ -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));

18
src/buzzuav_closures.cpp
View File

@ -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;
}
/*----------------------------------------/

10
src/roscontroller.cpp
View File

@ -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);
}
}