Update of /cvsroot/playerstage/code/stage/src
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv6713/src
Modified Files:
gui.h model_wifi.cc raytrace.c stage.h stage_internal.h
Log Message:
added Wifi model, more or less untested, from Michael Schroeder. Comments
welcome
Index: raytrace.c
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/raytrace.c,v
retrieving revision 1.17
retrieving revision 1.18
diff -C2 -d -r1.17 -r1.18
*** raytrace.c 22 Mar 2006 08:46:29 -0000 1.17
--- raytrace.c 11 Sep 2007 21:30:09 -0000 1.18
***************
*** 255,258 ****
--- 255,379 ----
}
+ // This method calculates the distance in meters which the signal
+ // has to travel through walls, stones and obstacles, which are also
+ // visible to lasers! Most of the code is taken from the already existing
+ // itl_first_matching routine above!
+
+ double itl_wall_distance( itl_t* itl,
+ stg_itl_test_func_t func,
+ stg_model_t* finder, stg_model_t*
otherrobot) {
+ itl->index = 0;
+ itl->models = NULL;
+ stg_cell_t* cell = itl->matrix->root;
+ stg_model_t* found = NULL;
+ double through_dist = 0;
+
+ double xstart, ystart;
+ stg_model_t* hitting = NULL; // What model we're travelling through
+ stg_model_t* last_model = NULL; // What model we just left.
+ double justoutm = 0; // "When" we just left this model.
+
+
+
+ while (LT(itl->range, itl->max_range)) {
+ // Locate the leaf cell at X, Y
+ cell = stg_cell_locate( cell, itl->x, itl->y );
+
+ if ( cell == NULL ) {
+ // done.
+ itl->range = itl->max_range;
+ if (hitting != NULL) {}
+ return through_dist;
+ }
+
+ if ( fig_debug_rays )
+ stg_rtk_fig_rectangle( fig_debug_rays, cell->x, cell->y, 0, cell->size,
cell->size, 0);
+
+ if (cell->data) { // not empty
+ stg_model_t* hitmod = gslist_first_matching( (GSList*) cell->data,
func, finder );
+
+ if (!stg_model_is_related(otherrobot,hitmod)) {
+ // We hit something which matched // we should check if its the robot
+ if (hitmod == hitting) {
+ //printf("Coming out: %s %f\n", hitmod->token, through_dist);
+ // Coming out of the object
+ last_model = hitting;
+ justoutm = 0;
+ hitting = NULL;
+ } else if (hitting == NULL) {
+ if ((last_model == hitmod) && (justoutm < 0.1)) {}
+ else {
+ hitting = hitmod;
+ }
+ }
+ }
+ }
+
+ double c = itl->y - itl->tana * itl->x;
+
+ double xleave = itl->x;
+ double yleave = itl->y;
+
+ if ( GT(itl->a, 0) ) { // upwards
+ yleave = cell->ymax; // top edge
+ xleave = (yleave - c) / itl->tana;
+
+ // if the edge crossing was not in cell bounds
+ if (!(GTE(xleave,cell->xmin) && LT(xleave,cell->xmax))) {
+ // it must have left the cell on the left or right instead
+
+ if (GT(itl->a, M_PI/2.0)) {
+ xleave = cell->xmin-0.0001;
+ } else {
+ xleave = cell->xmax;
+ }
+
+ yleave = itl->tana * xleave + c;
+ }
+ } else { // bottom edge?
+ yleave = cell->ymin;
+ xleave = (yleave - c) / itl->tana;
+
+ if (!(GTE(xleave,cell->xmin) && LT(xleave,cell->xmax))) {
+ // left or right instead
+ if (LT(itl->a,-M_PI/2.0)) {
+ xleave = cell->xmin-0.00001;
+ } else {
+ xleave = cell->xmax;
+ }
+
+ yleave = itl->tana * xleave + c;
+ } else {
+ yleave -= 0.00001;
+ }
+ }
+
+ if ( fig_debug_rays ) { // draw the ray going through the rectangle
+ stg_rtk_fig_color_rgb32 ( fig_debug_rays, 0xFFBBBBB );
+ stg_rtk_fig_arrow_ex ( fig_debug_rays, itl->x, itl->y, xleave, yleave,
0.01 );
+ stg_rtk_fig_color_rgb32 ( fig_debug_rays, 0xFF00000 );
+ }
+
+ // jump to the leave point
+ double distance = hypot( yleave - itl->y, xleave - itl->x );
+ itl->range += distance;
+ itl->x = xleave;
+ itl->y = yleave;
+ if (hitting != NULL) {
+ through_dist += distance;
+ } else {
+ justoutm += distance;
+ }
+ }
+
+ if (hitting != NULL) {
+ //printf("Finished");
+ }
+
+ return through_dist;
+
+ }
+
+
Index: stage_internal.h
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/stage_internal.h,v
retrieving revision 1.59
retrieving revision 1.60
diff -C2 -d -r1.59 -r1.60
*** stage_internal.h 27 Jul 2006 02:33:02 -0000 1.59
--- stage_internal.h 11 Sep 2007 21:30:09 -0000 1.60
***************
*** 567,570 ****
--- 567,573 ----
stg_model_t* finder );
+ double itl_wall_distance( itl_t* itl,
+ stg_itl_test_func_t func,
+ stg_model_t* finder, stg_model_t* otherrobot);
/** @} */
Index: gui.h
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/gui.h,v
retrieving revision 1.36
retrieving revision 1.37
diff -C2 -d -r1.36 -r1.37
*** gui.h 27 Jul 2006 02:33:02 -0000 1.36
--- gui.h 11 Sep 2007 21:30:09 -0000 1.37
***************
*** 52,55 ****
--- 52,58 ----
#define STG_LAYER_BUMPERDATA 62
+ #define STG_LAYER_WIFICONFIG 64
+ #define STG_LAYER_WIFIDATA 65
+
#define STG_RANGER_COLOR "gray75"
#define STG_RANGER_GEOM_COLOR "orange"
***************
*** 70,73 ****
--- 73,80 ----
#define STG_LASER_BRIGHT_COLOR "blue"
+ #define STG_WIFI_CFG_COLOR "royal blue"
+ #define STG_WIFI_CFG_COLOR_GREEN "DarkGreen"
+
+
#define STG_LAYER_GRIPPERGEOM 85
#define STG_LAYER_GRIPPERDATA 87
Index: stage.h
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/stage.h,v
retrieving revision 1.192
retrieving revision 1.193
diff -C2 -d -r1.192 -r1.193
*** stage.h 5 Sep 2007 19:01:43 -0000 1.192
--- stage.h 11 Sep 2007 21:30:09 -0000 1.193
***************
*** 1063,1070 ****
typedef struct
{
! // Configuration for the wifi model goes here. E.g., power, range of
! // propagation.
} stg_wifi_config_t;
/** wifi data packet
*/
--- 1063,1095 ----
typedef struct
{
! char essid[32];
! char mac[32];
! char ip[32];
! double power;
! double sensitivity;
! double freq;
! stg_meters_t range;
! char model[32];
! double plc; // distance power loss coefficient
! double ple; // path loss distance exponent
! double sigma; // standard derivation of gaussian random variable
! double range_db; // setting the individual ranges to render in
render_cfg
! double wall_factor;
} stg_wifi_config_t;
+ /** wifi sample packet
+ */
+ typedef struct
+ {
+ // this is how an entry in neighbours looks like
+ stg_pose_t pose; // global pose of corresponding neighbour
+ char essid[32];
+ char mac[32];
+ char ip[32]; // IP adress of corresponding card
+ double freq;
+ double db; // signal-strength
+
+ } stg_wifi_sample_t;
+
/** wifi data packet
*/
***************
*** 1073,1076 ****
--- 1098,1102 ----
// Simulated wifi data goes here. E.g., for each neighbor within
// range, record the corresponding signal strength.
+ GArray* neighbours;
} stg_wifi_data_t;
Index: model_wifi.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/model_wifi.cc,v
retrieving revision 1.1
retrieving revision 1.2
diff -C2 -d -r1.1 -r1.2
*** model_wifi.cc 2 Mar 2006 23:19:03 -0000 1.1
--- model_wifi.cc 11 Sep 2007 21:30:09 -0000 1.2
***************
*** 2,7 ****
//
// File: model_wifi.cc
! // Authors: Benoit Morisset
! // Date: 2 March 2006
//
// CVS info:
--- 2,7 ----
//
// File: model_wifi.cc
! // Authors: Michael Schroeder
! // Date: 10 July 2007
//
// CVS info:
***************
*** 16,42 ****
@defgroup model_wifi Wifi model
! @todo Fill this in
<h2>Worldfile properties</h2>
! @todo Fill this in
- */
#include <sys/time.h>
#include <math.h>
#include "gui.h"
- // Number pulled directly from my ass
- #define STG_WIFI_WATTS 2.5
! //#define DEBUG
- #include "stage_internal.h"
// standard callbacks
extern "C" {
-
int wifi_update( stg_model_t* mod );
int wifi_startup( stg_model_t* mod );
--- 16,185 ----
@defgroup model_wifi Wifi model
! The wifi model simulates a wifi device. There are currently five radio
propagation models
! implemented. The wifi device reports simulated link information for all
corresponding
! nodes within range.
<h2>Worldfile properties</h2>
! @par Simple Wifi Model
!
! @verbatim
! wifi
! (
! # network properties
! ip "192.168.0.1"
! mac "08-00-20-ae-fd-7e"
! essid "test network"
!
! # radio propagation model
! model "simple"
! range 5
! )
! @endverbatim
! For the simple model it is only necessary to specify the range property which
serves as the
! radio propagation radius.
+ @par Friis Outdoor Model
+
+ @verbatim
+ wifi
+ (
+ # network properties
+ ip "192.168.0.1"
+ mac "08-00-20-ae-fd-7e"
+ essid "test network"
+ # radio propagation model
+ model "friis"
+ power 30
+ sensitivity -80
+ )
+ @endverbatim
+ The friis model simulates the path loss for each link according to Friis
Transmission Equation. Basically it serves as a free space model. The
parameters power(dbm) and sensitivity(dbm) are mandatory.
+
+
+ @par ITU Indoor Propagation Model
+
+ @verbatim
+ wifi
+ (
+ # network properties
+ ip "192.168.0.1"
+ mac "08-00-20-ae-fd-7e"
+ essid "test network"
+
+ # radio propagation model
+ model "ITU indoor"
+ power 30
+ sensitivity -70
+ plc 30
+ )
+ @endverbatim
+ This model estimates the path loss inside rooms or closed areas. The distance
power loss
+ coefficient must be set according to the environmental settings. Values vary
between 20 to 35.
+ Higher values indicate higher path loss.
+
+
+ @par Log Distance Path Loss Model
+
+ @verbatim
+ wifi
+ (
+ # network properties
+ ip "192.168.0.1"
+ mac "08-00-20-ae-fd-7e"
+ essid "test network"
+
+ # radio propagation model
+ model "log distance"
+ power 30
+ sensitivity -70
+ ple 4
+ sigma 6
+ )
+ @endverbatim
+ The Log Distance Path Loss Model approximates the total path loss inside a
building.
+ It uses the path loss distance exponent (ple) which varies between 1.8 and 10
to accommodate for different environmental settings. A Gaussian random variable
with zero mean and standard deviation sigma
+ is used to reflect shadow fading.
+
+
+ @par Simple Raytracing Model
+
+ @verbatim
+ wifi
+ (
+ # network properties
+ ip "192.168.0.1"
+ mac "08-00-20-ae-fd-7e"
+ essid "test network"
+
+ # radio propagation model
+ model "raytrace"
+ wall_factor 10
+ )
+ @endverbatim
+ In this model Raytracing is used to reflect wifi-blocking obstacles within
range. The wall_factor
+ is used to specify how hard it is for the signal to penetrate obstacles.
+
+
+ @par Details
+ - model
+ - the model to use. Choose between the five existing radio propagation
models
+ - ip
+ - the ip to fake
+ - mac
+ - the corresponding mac addess
+ - essid
+ - networks essid
+ - freq
+ - the operating frequency [default 2450 (MHz)]
+ - power
+ - Transmitter output power in dbm [default 45 (dbm)]
+ - sensitivity
+ - Receiver sensitivity in dbm [default -75 (dbm)]
+ - range
+ - propagation radius in meters used by simple model
+ - plc
+ - power loss coefficient used by the ITU Indoor model
+ - ple
+ - power loss exponent used by the Log Distance Path Loss Model
+ - sigma
+ - standard deviation used by Log Distance Path Loss Model
+ - range_db
+ - parameter used for visualization of radio wave propagation. It is usually
set to a negative value
+ to visualize a certain db range.
+ - wall_factor
+ - factor reflects the strength of obstacles
+ */
+
+ #include <assert.h>
#include <sys/time.h>
#include <math.h>
#include "gui.h"
+ #include <limits.h>
+ #include "stage_internal.h"
! #define RANDOM_LIMIT INT_MAX
! #define STG_WIFI_WATTS 2.5 // wifi power consumption
! #define STG_DEFAULT_WIFI_IP "192.168.0.1"
! #define STG_DEFAULT_WIFI_MAC "00:00:00:00:00"
! #define STG_DEFAULT_WIFI_ESSID "any"
! #define STG_DEFAULT_WIFI_POWER 45
! #define STG_DEFAULT_WIFI_FREQ 2450
! #define STG_DEFAULT_WIFI_SENSITIVITY -75
! #define STG_DEFAULT_WIFI_RANGE 0
! #define STG_DEFAULT_WIFI_PLC 30
! #define STG_DEFAULT_WIFI_SIGMA 5 // can take values from 2 to 12 or even
higher
! #define STG_DEFAULT_WIFI_PATH_LOSS_EXPONENT 2.5 // 2.0 for free space,
!
// 1.8 in corridor, higher
!
// values indicate more obstacles
! #define STG_DEFAULT_WIFI_RANGE_DB -50 //in dbm
! #define STG_DEFAULT_WIFI_WALL_FACTOR 1
// standard callbacks
extern "C" {
int wifi_update( stg_model_t* mod );
int wifi_startup( stg_model_t* mod );
***************
*** 44,62 ****
void wifi_load( stg_model_t* mod );
int wifi_render_data( stg_model_t* mod, void* userp );
int wifi_unrender_data( stg_model_t* mod, void* userp );
! void
! wifi_load( stg_model_t* mod )
{
! stg_wifi_config_t cfg;
! // TODO: read wifi params from the world file
!
! stg_model_set_cfg( mod, &cfg, sizeof(cfg));
}
! int
! wifi_init( stg_model_t* mod )
{
// we don't consume any power until subscribed
--- 187,244 ----
void wifi_load( stg_model_t* mod );
+ int wifi_render_cfg( stg_model_t* mod, void* userp );
+ int wifi_unrender_cfg( stg_model_t* mod, void* userp );
int wifi_render_data( stg_model_t* mod, void* userp );
int wifi_unrender_data( stg_model_t* mod, void* userp );
! static stg_color_t cfg_color = 0;
! static stg_color_t cfg_color_green= 0;
! // variable to save outcome of random variable
! static double norand;
! static double power_dbm;
!
! void wifi_load( stg_model_t* mod )
{
! stg_wifi_config_t* cfg = (stg_wifi_config_t*) mod->cfg;
! // read wifi parameters from worldfile
! strncpy(cfg->ip, wf_read_string(mod->id, "ip", cfg->ip), 32 );
! strncpy(cfg->mac, wf_read_string(mod->id, "mac", cfg->mac), 32 );
! strncpy(cfg->essid, wf_read_string(mod->id, "essid", cfg->essid), 32 );
! cfg->power = wf_read_float( mod->id, "power", cfg->power);
! cfg->freq = wf_read_float( mod->id, "freq",
cfg->freq);
! cfg->sensitivity = wf_read_float( mod->id, "sensitivity",
cfg->sensitivity);
! cfg->range = wf_read_length( mod->id, "range", cfg->range);
! strncpy(cfg->model, wf_read_string(mod->id, "model", cfg->model), 32);
! cfg->plc = wf_read_float( mod->id, "plc",
cfg->plc);
! cfg->ple = wf_read_float( mod->id, "ple",
cfg->ple);
! cfg->sigma = wf_read_float( mod->id, "sigma", cfg->sigma);
! // specifiy db-config ranges for rendering
! cfg->range_db = wf_read_float( mod->id, "range_db",
cfg->range);
! cfg->wall_factor= wf_read_float( mod->id, "wall_factor",
cfg->wall_factor);
! power_dbm = 10*log10( cfg->power );
!
! model_change( mod, &mod->cfg );
}
! // Marsaglia polar method to obtain a gaussian random variable
! double nrand(double d) // additional parameter because of update cycle
! {
! srand(time(0)+d); // time(0) is different only every second
! double u1,u2,s;
!
! do
! {
! u1= ((double) rand()/RANDOM_LIMIT)*2.0-1.0;
! u2= ((double) rand()/RANDOM_LIMIT)*2.0-1.0;
! s=u1*u1+u2*u2;
! }
! while (s>=1.0);
!
! return ( u1*sqrt(-2* log(s) /s) );
! }
!
!
! int wifi_init( stg_model_t* mod )
{
// we don't consume any power until subscribed
***************
*** 84,100 ****
stg_model_set_blob_return( mod, 0 );
stg_model_set_color( mod, (stg_color_t)0 );
// adds a menu item and associated on-and-off callbacks
stg_model_add_property_toggles( mod, &mod->data,
wifi_render_data, NULL,
wifi_unrender_data, NULL,
"wifi_data",
! "wifi data",
TRUE );
return 0;
}
! int
! wifi_update( stg_model_t* mod )
{
// no work to do if we're unsubscribed
--- 266,436 ----
stg_model_set_blob_return( mod, 0 );
stg_model_set_color( mod, (stg_color_t)0 );
+ cfg_color = stg_lookup_color(STG_WIFI_CFG_COLOR);
+ cfg_color_green= stg_lookup_color(STG_WIFI_CFG_COLOR_GREEN);
+
+
+ // set up a wifi-specific config structure
+ stg_wifi_config_t lconf;
+ memset(&lconf,0,sizeof(lconf));
+
+ strncpy(lconf.ip, STG_DEFAULT_WIFI_IP, 32);
+ strncpy(lconf.mac, STG_DEFAULT_WIFI_MAC, 32);
+ strncpy(lconf.essid, STG_DEFAULT_WIFI_ESSID, 32);
+ lconf.power = STG_DEFAULT_WIFI_POWER;
+ lconf.sensitivity = STG_DEFAULT_WIFI_SENSITIVITY;
+ lconf.range = STG_DEFAULT_WIFI_RANGE;
+ lconf.freq = STG_DEFAULT_WIFI_FREQ;
+ lconf.plc = STG_DEFAULT_WIFI_PLC;
+ lconf.ple =
STG_DEFAULT_WIFI_PATH_LOSS_EXPONENT;
+ lconf.sigma = STG_DEFAULT_WIFI_SIGMA;
+ lconf.range_db = STG_DEFAULT_WIFI_RANGE_DB;
+ lconf.wall_factor = STG_DEFAULT_WIFI_WALL_FACTOR;
+ stg_model_set_cfg( mod, &lconf, sizeof(lconf) );
+
+ //create array to hold the link data
+ stg_wifi_data_t data;
+ data.neighbours = g_array_new( FALSE, FALSE, sizeof( stg_wifi_sample_t ) );
+ stg_model_set_data( mod, &data, sizeof( data ) );
// adds a menu item and associated on-and-off callbacks
stg_model_add_property_toggles( mod, &mod->data,
+ wifi_render_cfg, NULL,
+ wifi_unrender_cfg, NULL,
+ "wifi_cfg",
+ "wifi range",
+ TRUE );
+
+ // adds a menu item and associated on-and-off callbacks
+ stg_model_add_property_toggles( mod, &mod->data,
wifi_render_data, NULL,
wifi_unrender_data, NULL,
"wifi_data",
! "wifi connections",
TRUE );
+
return 0;
}
!
! int wifi_raytrace_match( stg_model_t* mod, stg_model_t* hitmod )
! {
! // ignore my relatives and things that are invisible to lasers
! return( (!stg_model_is_related( mod,hitmod )) &&
! ( hitmod->laser_return > 0) );
! }
!
!
! stg_meters_t calc_distance( stg_model_t* mod1,
!
stg_model_t* mod2,
!
stg_pose_t* pose1,
!
stg_pose_t* pose2 )
! {
! itl_t* itl = NULL;
! itl = itl_create( pose1->x, pose1->y, pose2->x, pose2->y,
! mod1->world->matrix, PointToPoint );
!
! return itl_wall_distance( itl, wifi_raytrace_match, mod1, mod2 );
! }
!
!
! void append_link_information( stg_model_t* mod1, stg_model_t* mod2, double db
)
! {
! stg_wifi_config_t* cfg2 = (stg_wifi_config_t*) mod2->cfg;
! stg_pose_t pose2;
! stg_model_get_global_pose( mod2, &pose2 );
!
! stg_wifi_sample_t sample;
! memset( &sample,0,sizeof( sample ) );
! strcpy( sample.ip, cfg2->ip );
! strcpy( sample.mac, cfg2->mac );
! strcpy( sample.essid, cfg2->essid) ;
! sample.db = db;
! sample.pose = pose2;
! sample.freq = cfg2->freq;
!
! stg_wifi_data_t* data = (stg_wifi_data_t*) mod1->data;
! g_array_append_val( data->neighbours, sample );
! }
!
! void compare_models( gpointer key, gpointer value, gpointer user )
! {
! stg_model_t *mod1 = (stg_model_t *) user;
! stg_model_t *mod2 = (stg_model_t *) value;
!
! // proceed if models are different and wifi devices
! if ( (mod1->typerec != mod2->typerec) ||
! (mod1 ==mod2) ||
! (stg_model_is_related(mod1,mod2)) )
! return;
!
! stg_pose_t pose1;
! stg_model_get_global_pose( mod1, &pose1 );
! stg_pose_t pose2;
! stg_model_get_global_pose( mod2, &pose2 );
!
! stg_wifi_config_t* cfg1 = (stg_wifi_config_t*) mod1->cfg;
! stg_wifi_config_t* cfg2 = (stg_wifi_config_t*) mod2->cfg;
!
! double distance = hypot( pose2.x-pose1.x, pose2.y-pose1.y );
!
! // store link information for reachable nodes
! if ( strcmp( cfg1->model, "friis" )== 0 )
! {
! double fsl =
32.44177+20*log10(cfg1->freq)+20*log10(distance/1000);
! if( fsl<= ( power_dbm-cfg2->sensitivity ) )
! {
! append_link_information( mod1, mod2,
power_dbm-fsl );
! }
! }
!
! else if ( strcmp( cfg1->model, "ITU indoor" ) ==0 )
! {
! double loss = 20*log10( cfg1->freq )+( cfg1->plc
)*log10( distance )-28;
! if( loss<=( power_dbm-cfg2->sensitivity ) )
! {
! append_link_information( mod1, mod2,
power_dbm-loss );
! }
! }
!
! else if(strcmp(cfg1->model, "log distance" )==0 )
! {
! double loss_0 = 32.44177+20*log10( cfg1->freq )+
!
20*log10( 0.001 ); // path loss in 1 meter
! norand = nrand( power_dbm );
! double loss = loss_0+10*cfg1->ple*log10( distance
)+cfg1->sigma*norand;
!
! if( loss<= (power_dbm-cfg2->sensitivity) )
! {
! append_link_information(mod1, mod2, power_dbm-loss);
! }
! }
!
! else if( strcmp( cfg1->model, "raytrace" )==0 )
! {
! double loss = 20*log10( cfg1->freq )+
!
(cfg1->plc)*log10( distance )-28;
!
! if( loss<= ( power_dbm-cfg2->sensitivity ) )
! {
! stg_meters_t me = calc_distance( mod1, mod2, &pose1,
&pose2 );
! double new_loss = cfg1->wall_factor*me*100+loss;
! if ( new_loss <=( power_dbm-cfg2->sensitivity ) )
! {
! append_link_information( mod1, mod2,
power_dbm-new_loss );
! }
! }
! }
!
! else if( strcmp( cfg1->model, "simple" )==0 )
! {
! if (cfg1->range >= distance)
! {
! append_link_information( mod1, mod2, 1 );
! }
! }
! }
!
!
! int wifi_update( stg_model_t* mod )
{
// no work to do if we're unsubscribed
***************
*** 102,176 ****
return 0;
! puts("wifi_update");
// Retrieve current configuration
stg_wifi_config_t cfg;
! memcpy(&cfg, mod->cfg, sizeof(cfg));
// Retrieve current geometry
stg_geom_t geom;
stg_model_get_geom( mod, &geom );
-
- // get the sensor's pose in global coords
- stg_pose_t pz;
- memcpy( &pz, &geom.pose, sizeof(pz) );
- stg_model_local_to_global( mod, &pz );
-
// We'll store current data here
stg_wifi_data_t data;
! // DO RADIO PROPAGATION HERE, and fill in the data structure
! // publish the new stuff
! stg_model_set_data( mod, &data, sizeof(data));
- // inherit standard update behaviour
- _model_update( mod );
! return 0; //ok
}
! int
! wifi_render_data( stg_model_t* mod, void* userp )
{
! //puts( "wifi render data" );
! // only draw if someone is using the gripper
if( mod->subs < 1 )
return 0;
! // DRAW WIFI DATA HERE. Look at other models for examples.
return 0;
}
! int
! wifi_startup( stg_model_t* mod )
{
PRINT_DEBUG( "wifi startup" );
stg_model_set_watts( mod, STG_WIFI_WATTS );
return 0; // ok
}
! int
! wifi_shutdown( stg_model_t* mod )
{
PRINT_DEBUG( "wifi shutdown" );
stg_model_set_watts( mod, 0 );
! // unrender the data
! //stg_model_fig_clear( mod, "wifi_data_fig" );
return 0; // ok
}
! int
! wifi_unrender_data( stg_model_t* mod, void* userp )
{
! // CLEAR STUFF THAT YOU DREW
! //stg_model_fig_clear( mod, "wifi_data_fig" );
return 1; // callback just runs one time
}
}
--- 438,703 ----
return 0;
! //puts("\nwifi_update");
// Retrieve current configuration
stg_wifi_config_t cfg;
! memcpy( &cfg, mod->cfg, sizeof(cfg) );
// Retrieve current geometry
stg_geom_t geom;
stg_model_get_geom( mod, &geom );
// We'll store current data here
stg_wifi_data_t data;
+ memcpy( &data, mod->data, sizeof(data) );
! g_array_free( data.neighbours, TRUE );
!
! data.neighbours = g_array_new( FALSE, TRUE, sizeof( stg_wifi_sample_t )
);
! stg_model_set_data( mod, &data, sizeof( data ) );
! g_hash_table_foreach(mod->world->models_by_name, compare_models, mod);
! stg_model_set_data( mod, &data, sizeof( data ) );
!
! return 0; //ok
! }
!
! int wifi_render_cfg( stg_model_t* mod, void* userp )
! {
!
! //puts( "\nwifi render cfg" );
! stg_rtk_fig_t* fig =
! stg_model_fig_get_or_create( mod, "wifi_cfg_fig", "top",
!
STG_LAYER_WIFICONFIG );
! stg_rtk_fig_t* bg =
! stg_model_fig_get_or_create( mod, "wifi_cfg_bg_fig", "top",
!
STG_LAYER_BACKGROUND );
!
! stg_rtk_fig_clear( bg );
! stg_rtk_fig_clear( fig );
!
! // get config
! stg_wifi_config_t *cfg = (stg_wifi_config_t*) mod->cfg;
! assert( cfg ); // make sure config is available
!
! // get pose
! stg_pose_t pose;
! stg_model_get_global_pose( mod, &pose );
!
! // render corresponding config ranges
! if( strcmp( cfg->model, "simple" )==0 )
! {
! stg_rtk_fig_ellipse( fig, 0, 0, 0, 2*cfg->range, 2*cfg->range,
0 );
! }
!
! double dist_green=0;// dist_red=0, dist_yellow=0 ;
!
! if ( strcmp( cfg->model, "friis" )==0 )
! {
! dist_green = pow( 10, ((-cfg->range_db+power_dbm-
!
32.44177-20*log10(cfg->freq))/20)
)*1000;
! }
!
! if ( strcmp( cfg->model, "ITU indoor" )==0 )
! {
! dist_green = pow( 10, (-cfg->range_db+power_dbm+28-
!
20*log10(cfg->freq))/cfg->plc );
! }
!
! if( strcmp( cfg->model, "log distance")==0 )
! {
! double loss_0 =
32.44177+20*log10(cfg->freq)+20*log10(0.001);
! dist_green = pow( 10,
(-cfg->range_db+power_dbm-loss_0-
!
cfg->sigma*norand)/(10*cfg->ple) );
! }
!
! if ( strcmp( cfg->model, "raytrace")==0 )
! {
! double points[72][2];
! stg_pose_t frompose;
! stg_model_get_global_pose(mod, &frompose);
! double bearing = 0;
! double sample_incr = (2.0 * M_PI) / 72.0;
! double me = 0;
! double actualrange = 0;
! double dist_red = pow( 10, (-cfg->range_db+power_dbm+
!
28-20*log10(cfg->freq))/cfg->plc );
! itl_t* itl = NULL;
!
! for ( int s = 0; s < 72; s++ )
! {
! // calculate the maximum range!
! itl = itl_create( frompose.x, frompose.y,
bearing, dist_red,
!
mod->world->matrix, PointToBearingRange );
! // check the fieldstrength on first obstacle
! // and in the end and just interpolate
! itl_first_matching(itl,
wifi_raytrace_match,mod);
!
! double x = itl->x;
! double y = itl->y;
! double dist_to_obst = hypot(pose.x-x,pose.y-y);
!
! // the ray hits an obstacle
! if ( dist_to_obst>0 )
! {
! double loss_to_obst =
20*log10(cfg->freq)+
!
(cfg->plc)*log10(dist_to_obst)-28;
! double loss_to_targ =
20*log10(cfg->freq)+
!
(cfg->plc)*log10(dist_red)-28;
!
! // calculate distance through
wall
! me = itl_wall_distance(itl,
wifi_raytrace_match, mod, mod);
! itl_destroy( itl );
! double dis =
(dist_red-dist_to_obst)*
!
(power_dbm - cfg->range_db-loss_to_obst) /
!
((cfg->wall_factor*me*100+loss_to_targ)-loss_to_obst);
!
! actualrange = dist_to_obst+dis;
! }
! else
! {
! actualrange= dist_red;
! }
!
! double x3 =actualrange * cos(bearing);
! double y4 =actualrange * sin(bearing);
!
! // are we inside the world?
! if ( -mod->world->width/2 <= (x3+frompose.x)
&&
! (x3+frompose.x) <=
mod->world->width/2 &&
! -mod->world->height/2 <=
(y4+frompose.y) &&
! (y4+frompose.y) <=
mod->world->height/2 )
! {
!
points[s][0]=x3*cos(-frompose.a)- sin(-frompose.a)*y4;
!
points[s][1]=sin(-frompose.a)*x3 +y4*cos(-frompose.a);
! } // we are outside! correct to border!
! else
! {
! double ac =
hypot(frompose.x-x,frompose.y-y);
! points[s][0]=
ac*cos(bearing)*cos(-frompose.a)-
!
sin(-frompose.a)*ac*sin(bearing);
! points[s][1]=
sin(-frompose.a)*ac*cos(bearing)+
!
ac*sin(bearing)*cos(-frompose.a);
! }
!
! bearing += sample_incr;
! }
! // print polygons
! stg_rtk_fig_color_rgb32( bg, 0xc1ffc1);
! stg_rtk_fig_color_rgb32( fig, 0x2e8b57);
! stg_rtk_fig_polygon( bg, 0,0,0, 72, points, TRUE );
! stg_rtk_fig_polygon( fig, 0, 0, 0, 72, points, 0 );
! return 0;
! }
! // print ellipses
! stg_rtk_fig_color_rgb32( bg, 0xc1ffc1);
! stg_rtk_fig_color_rgb32( fig, 0x2e8b57);
! stg_rtk_fig_ellipse( fig, 0, 0, 0, 2*dist_green, 2*dist_green, 0);
! stg_rtk_fig_ellipse( bg, 0, 0, 0, 2*dist_green, 2*dist_green, 1);
!
! return 0;
}
! // draw some fancy arrows to indicate connections
! void draw( stg_pose_t pose1, stg_pose_t pose2, stg_rtk_fig_t *fig )
{
! double length = sqrt( pow( pose2.x-pose1.x, 2 ) + pow(
pose2.y-pose1.y, 2 ) );
! double neworient;
! if ( pose1.x > pose2.x && pose1.y >pose2.y)
! {
! neworient = ( 270*3.14152/180 )-asin( ( pose1.x-pose2.x
)/length );
! stg_rtk_fig_arrow_fancy( fig, 0, 0, neworient-pose1.a,
!
length-0.2, 0.15, 0.01, 1 );
! }
! else if( pose1.x>pose2.x && pose1.y <= pose2.y)
! {
! neworient = ( 90*3.14152/180 )+asin( ( pose1.x-pose2.x )/length
);
! stg_rtk_fig_arrow_fancy( fig, 0, 0, neworient-pose1.a,
!
length-0.2, 0.15, 0.01, 1 );
! }
! else if( pose1.x<=pose2.x && pose1.y <= pose2.y )
! {
! neworient = -( 270*M_PI/180 )+asin( ( pose1.x-pose2.x )/length
);
! stg_rtk_fig_arrow_fancy( fig, 0, 0, neworient-pose1.a,
!
length-0.2, 0.15, 0.01, 1 );
! }
! else if( pose1.x<=pose2.x && pose1.y > pose2.y )
! {
! neworient = -( 90*3.14152/180 )-asin( ( pose1.x-pose2.x
)/length );
! stg_rtk_fig_arrow_fancy( fig, 0, 0, neworient-pose1.a,
!
length-0.2, 0.15, 0.01, 1 );
! }
! }
!
!
! int wifi_render_data( stg_model_t* mod, void* userp )
! {
if( mod->subs < 1 )
return 0;
! //puts("\n----------------\nrender_data");
!
! stg_rtk_fig_t* fig = stg_model_get_fig( mod, "wifi_data_fig" );
!
! if( !fig )
! fig = stg_model_fig_create( mod, "wifi_data_fig", "top",
STG_LAYER_WIFIDATA );
!
! // clear figure and get color for config visualisation
! stg_rtk_fig_clear(fig);
! stg_rtk_fig_color_rgb32( fig, cfg_color);
!
! stg_wifi_data_t* sdata = (stg_wifi_data_t*)mod->data;
! assert(sdata);
+ stg_pose_t pose1;
+ stg_model_get_global_pose( mod, &pose1 );
+
+ // only draw connections between linked nodes
+ for ( int j=0; j<sdata->neighbours->len; j++ )
+ {
+ stg_wifi_sample_t sam_temp = g_array_index(
sdata->neighbours,
+
stg_wifi_sample_t, j );
+ stg_pose_t pose2 = sam_temp.pose;
+ draw(pose1, pose2, fig);
+ }
return 0;
}
!
! int wifi_startup( stg_model_t* mod )
{
PRINT_DEBUG( "wifi startup" );
stg_model_set_watts( mod, STG_WIFI_WATTS );
+
return 0; // ok
}
!
! int wifi_shutdown( stg_model_t* mod )
{
PRINT_DEBUG( "wifi shutdown" );
stg_model_set_watts( mod, 0 );
! stg_model_fig_clear( mod, "wifi_cfg_fig" );
return 0; // ok
}
!
! int wifi_unrender_cfg( stg_model_t* mod, void* userp )
{
! stg_model_fig_clear( mod, "wifi_cfg_fig" );
! stg_model_fig_clear( mod, "wifi_cfg_bg_fig" );
return 1; // callback just runs one time
}
+
+ int wifi_unrender_data( stg_model_t* mod, void* userp )
+ {
+ stg_model_fig_clear( mod, "wifi_data_fig" );
+ return 1; // callback just runs one time
}
+ }
-------------------------------------------------------------------------
This SF.net email is sponsored by: Microsoft
Defy all challenges. Microsoft(R) Visual Studio 2005.
http://clk.atdmt.com/MRT/go/vse0120000070mrt/direct/01/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
