Community Company Search Contact

multilat_main.c from EmStar at Krugle

Show multilat_main.c syntax highlighted

/*
 *
 * Copyright (c) 2005 The Regents of the University of California.  All 
 * rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * - Redistributions of source code must retain the above copyright
 *   notice, this list of conditions and the following disclaimer.
 *
 * - Neither the name of the University nor the names of its
 *   contributors may be used to endorse or promote products derived
 *   from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "multilat_i.h"
#include <libdev/command_dev.h>
#include <libdev/option_dev.h>

#define PERSISTENT_RANGE_TABLE 
#undef  AUTO_COMPUTE


// this may be unneeded.. ???
static
int ml_sync_status(status_context_t *info, buf_t *buf) {
  ml_state_t *mls = (ml_state_t *) sd_data(info);
  int i = 0;
  struct timeval now;
  
  gettimeofday(&now, NULL);
  
  bufprintf(buf, "%-10s %-10s %-10s %-10s %-10s %-10s %-10s\n",
	    "node:",
	    "c-cnt:",
	    "synced:",
	    "neigh:",
	    "RMS:",
	    "to/frm:",
	    "seq:");
  bufprintf(buf, "%-10s %-10s %-10s %-10s %-10s %-10s %-10s\n",
	    "-----",
	    "-----",
	    "-----",
	    "-----",
	    "-----",
	    "-----",
	    "-----");

  for (i = 0; i < ML_MAX_NODES; ++i) {
    if (mls->nodes[i].id != 0) {
      bufprintf(buf, "%-10d %-10d %-10d %-10d %-10d %-10d %-10d\n",
		mls->nodes[i].id,
		mls->nodes[i].times_chirped,
		mls->nodes[i].sync_ok,
		mls->nodes[i].is_neighbor,
		mls->nodes[i].last_RMS,
		mls->nodes[i].to_me,
		mls->nodes[i].seqno % 256);
    }
  }
  return STATUS_MSG_COMPLETE;
}


static
int ml_error_status(status_context_t *info, buf_t *buf)
{
  ml_state_t *mls = (ml_state_t *) sd_data(info);
  if (mls->current_errors)
    bufcat(buf, mls->current_errors);
  return STATUS_MSG_COMPLETE;
}


static
int ml_compare_status(status_context_t *info, buf_t *buf)
{
  ml_state_t *mls = (ml_state_t *) sd_data(info);
  if (mls->current_studbuf)
    bufcat(buf, mls->current_studbuf);
  return STATUS_MSG_COMPLETE;
}


static
int ml_coords_status(status_context_t *info, buf_t *buf)
{
  ml_state_t *mls = (ml_state_t *) sd_data(info);
  if (mls->current_coords)
    bufcat(buf, mls->current_coords);
  return STATUS_MSG_COMPLETE;
}


static
int ml_convergence_status(status_context_t *info, buf_t *buf)
{
  ml_state_t *mls = (ml_state_t *) sd_data(info);
  switch (mls->failed_convergence) {
  default:
    bufprintf(buf, "Multilat never started\n"); break;
  case 1:
    bufprintf(buf, "***Convergence FAILED!***\n"); break;
  case 2:
    bufprintf(buf, "Converged!\n"); break;
  case 3:
    bufprintf(buf, "Processing...\n"); break;
  }
  bufprintf(buf, "%d ranges, %d ranges processed, %d nodes processed\n",
	    mls->last_table_count,
	    mls->range_element_count,
	    mls->result_element_count);
  if (mls->latest_table) {
    bufprintf(buf, "**Data Ready** for %ld seconds\n", time(0) - mls->last_update_time);
  }

  int i;
  bufprintf(buf, 
	    "%20s  Range Count\n"
	    "%20s  -----------\n",
	    "Node", "----");
  for (i = 0; i < ML_MAX_NODES; ++i)
    if (mls->nodes[i].id != 0) {
      bufprintf(buf, "%20s  %d\n",
		print_if_id(mls->nodes[i].id),
		mls->nodes[i].range_count);
    }

  return STATUS_MSG_COMPLETE;
}



static
void ml_init_node(ml_node_t* node, node_id_t n) {
  /* init some stuff */
  node->id = n;
  node->sync_ok = 0;
  node->last_RMS = 500; /* dummy value */
  /* should be set by cluster map... but not working yet */
  node->is_neighbor = 1; 
  node->to_me = -1;
}

static
int ml_lookup_node(ml_node_t* nodes, node_id_t nid) {
  int i = 0;
  int first_empty = -1;
  for (i = 0; i < ML_MAX_NODES; ++i) {
    if (nodes[i].id == nid) {
      elog(LOG_DEBUG_0, "$$$$$ Found existing slot for %d at %d", nid, i);
      return i;
    } else {
      if (nodes[i].id == 0 && first_empty == -1) {
	first_empty = i;
      }
    }
  }

  if (first_empty == -1) {
    elog(LOG_CRIT, "Ran out of space in ml_node_t array!!!");
  }
  elog(LOG_DEBUG_0, "$$$$$ Found new slot for %d at %d", nid, first_empty);
  return first_empty;
}


static
int ml_maybe_add_node(ml_node_t* nodes, node_id_t nid) 
{
  if (nid == 0)   
    return -1;

  int i = ml_lookup_node(nodes, nid);
  if (i>=0 && nodes[i].id == 0) ml_init_node(&nodes[i], nid);
  return i;
}


static 
int mlat_trigger_now(void *data, int interval, g_event_t *ev)
{
  ml_state_t *mls = (ml_state_t *) data;
  elog(LOG_WARNING, "Triggering thread!");
  if (mls->master_mode) {
    pthread_cond_broadcast(&(mls->range_snapshot_cond));
  }
  return TIMER_DONE;
}


#ifdef PERSISTENT_RANGE_TABLE
static
int range_table_cmp(const void *a, const void *b)
{
  range_entry_table_t *a_t = (range_entry_table_t *)a;
  range_entry_table_t *b_t = (range_entry_table_t *)b;
  if (a_t->header.flow_id.src == b_t->header.flow_id.src) 
    return (a_t->range_entry.source - b_t->range_entry.source);
  return (a_t->header.flow_id.src - b_t->header.flow_id.src);
}
#endif

static
int ml_range_update(ssync_sub_t *sub, range_entry_table_t *table, 
		    int count, void *data)
{
  ml_state_t *mls = (ml_state_t *) data;

  if (table == NULL) {
    elog(LOG_WARNING, "new table was null!");
    return EVENT_RENEW;
  }
  
  /* add any mentioned nodes if new */
  int i;
  for (i = 0; i < count; ++i) {
    ml_maybe_add_node(mls->nodes, table[i].range_entry.source);
    ml_maybe_add_node(mls->nodes, table[i].header.flow_id.src);
  }

  /* clear range counters */
  for (i = 0; i < ML_MAX_NODES; ++i)
    mls->nodes[i].range_count = 0;
 
#ifdef PERSISTENT_RANGE_TABLE
  /* merge any new data into the existing table.
   * iterate thru existing table if present,
   * and add any data from nodes that did not appear
   * in the new one */

  if (mls->last_table) {
    int i,j;
    int appended = 0;

    /* copy over to appendable buf */
    buf_t *new_table = buf_new();
    bufcpy(new_table, (char*)table, count*sizeof(table[0]));
    elog(LOG_WARNING, "copied over buffer, size %d,%d", sizeof(table[0]), 
	 count*sizeof(*table));
    
    for (i = 0; i < mls->last_table_count; ++i) {
      int data_from = mls->last_table[i].header.flow_id.src;

      /* is this node in the new table at all? */
      for (j = 0; j < count; ++j) {
	if (data_from == table[j].header.flow_id.src)
	  goto skip;
      }

      /* not present.. append it! */
      bufcpy(new_table, (char*)&(mls->last_table[i]), sizeof(*table));      
      appended++;
      elog(LOG_WARNING, "appended data from node %d... dropped out of table", 
	   data_from);
      
    skip:
      continue;
    }

    /* replace table with merged version, increase count */
    free(table);

    table = (range_entry_table_t *)new_table->buf;
    count += appended;
    new_table->buf = NULL;
    buf_free(new_table);
    qsort(table, count, sizeof(*table), range_table_cmp);
  }
#endif

  for (i = 0; i < count; ++i) {
    int index = ml_lookup_node(mls->nodes, table[i].header.flow_id.src);
    if (index >= 0) 
      mls->nodes[index].range_count++;
  }
  
  /* is this data new? */
  int is_new = 1;
  if (mls->last_table) {
    if (count == mls->last_table_count &&
	memcmp(mls->last_table, table, count*sizeof(*table)) == 0) {
      elog(LOG_WARNING, "new table data is identical to last!! skipping...");
      is_new = 0;
    }    
    free(mls->last_table);
  }
  mls->last_table = table;
  mls->last_table_count = count;
  
  if (is_new) {
    /* lock table pointer */
    pthread_mutex_lock(&(mls->range_snapshot_mutex));
    
    /* update the table pointer */
    if (mls->latest_table)
      free(mls->latest_table);
    mls->latest_table = g_memdup(table, count*sizeof(*table));
    mls->latest_table_count = count;
    pthread_mutex_unlock(&(mls->range_snapshot_mutex));
    mls->last_update_time = time(0);

#ifdef AUTO_COMPUTE
    g_event_destroy(mls->range_table_holdoff);
    g_timer_add(40000, mlat_trigger_now, mls, NULL, &(mls->range_table_holdoff));
#endif

    elog(LOG_WARNING, "Set mlat timer to process new data...");    
  }

  return EVENT_RENEW;
}


static
void multilat_shutdown(void *date) {
  elog(LOG_NOTICE, "mlat shutting down..");
  exit(1);
}


static
void ml_trigger_ranging(ml_state_t *mls, int invalidate) 
{
  elog(LOG_WARNING, "Triggering chirp %d...%s", mls->chirp_count,
       invalidate ? "  **CLEARING FIRST!**" : "");

  /* trigger */
  range_trigger_pkt_t trigger = {
    start_time: {
      timebase: {
	node: my_node_id,
	comp: CPU
      },
    },
    invalidate: invalidate
  };

  mls->failed_convergence=3;

  if (invalidate)
    LOG_ENTRY("****RESTART");
  LOG_ENTRY("****ranging start");

  if (invalidate)
    mls->chirp_count = 0;
  mls->chirp_count++;

  int i;
  for (i=0; i<20; i++) {
    if (mls->nodes[i].id == 0) break;
    trigger.sequence[i] = mls->nodes[i].id;
  }
  gettimeofday(&(trigger.start_time.stamp), NULL);

  link_pkt_t hdr = {
    dst: { id: LINK_BROADCAST },
    type: PKT_TYPE_RANGE_TRIG
  }; 

  buf_t *pkt_buf = buf_new();
  bufcpy(pkt_buf, &hdr, sizeof(hdr));
  bufcpy(pkt_buf, &trigger, sizeof(trigger));
  if (lu_send(mls->flood_event, (link_pkt_t*)(pkt_buf->buf), sizeof(trigger)) < 0) {
    elog(LOG_WARNING, "Failed to send message to flood adaptor: %m");
  }
  buf_free(pkt_buf);
}


void ml_do_match(ml_state_t *ml)
{
  /* for all valid, copy data to floats */
  int i,j;
  for (i=0; i<ML_MAX_NODES; i++) {
    ml->nodes[i].survey[0] = ml->nodes[i].survey[1] = ml->nodes[i].survey[2] = 0;
    if (ml->nodes[i].entry.valid) {
      for (j=0; j<3; j++)
	ml->nodes[i].coord[j] = ml->nodes[i].entry.coord[j] / 1000.0;
      ml->nodes[i].yaw = ml->nodes[i].entry.rpy[2] / 10.0;
    }
  }
  
  /* do matching */
  elog(LOG_WARNING, "starting match");
  
  ml->scale = 1;
  ml->rot = 0;
  ml->trans[0] = 0;  
  ml->trans[1] = 0;  
  ml->trans[2] = 0;
  int count = 0;
  
  /* match up survey points with real nodes */
  for (i=0; i<MAX_SURVEY_POINTS; i++) {
    ml->survey[i].est_used = 0;
    if (ml->survey[i].str) {
      for (j=0; j<ML_MAX_NODES; j++) {
	if (ml->nodes[j].entry.valid && ml->nodes[j].entry.node == ml->survey[i].node) {
	  int k;
	  for (k=0; k<3; k++) {
	    ml->survey[i].est_coord[k] = ml->nodes[j].coord[k];
	    ml->nodes[j].survey[k] = ml->survey[i].coord[k];
	  }
	  ml->survey[i].est_angle = ml->nodes[j].yaw;
	  ml->survey[i].est_used = 1;
	  count++;
	  elog(LOG_WARNING, "found match, node %d", ml->nodes[j].entry.node);
	}
      }
    }
  }

  if (count > 0) {
    /* compute scale */
    double sdist_tot = 0;
    double edist_tot = 0;
    for (i=0; i<MAX_SURVEY_POINTS; i++) 
      if (ml->survey[i].est_used) 
	for (j=0; j<MAX_SURVEY_POINTS; j++) 
	  if (i!=j && ml->survey[j].est_used) {
	    double sdist =
	      sqrt(sqrf(ml->survey[i].coord[0] - ml->survey[j].coord[0]) +
		   sqrf(ml->survey[i].coord[1] - ml->survey[j].coord[1]) +
		   sqrf(ml->survey[i].coord[2] - ml->survey[j].coord[2]));
	    sdist_tot += sdist;
	    
	    double edist =
	      sqrt(sqrf(ml->survey[i].est_coord[0] - ml->survey[j].est_coord[0]) +
		   sqrf(ml->survey[i].est_coord[1] - ml->survey[j].est_coord[1]) +
		   sqrf(ml->survey[i].est_coord[2] - ml->survey[j].est_coord[2]));
	    edist_tot += edist;	  
	  }
    
    /* correct scale */
    if (edist_tot > 0 && sdist_tot > 0) {
      ml->scale = sdist_tot / edist_tot;
      
      for (i=0; i<MAX_SURVEY_POINTS; i++) 
	if (ml->survey[i].est_used) 
	  for (j=0; j<3; j++) 
	    ml->survey[i].est_coord[j] *= ml->scale;
    }

    for (i=0; i<MAX_SURVEY_POINTS; i++) 
      if (ml->survey[i].est_used) {
	elog(LOG_WARNING, "node %d, %f,%f,%f  %f,%f,%f",
	     ml->survey[i].node,
	     ml->survey[i].est_coord[0],
	     ml->survey[i].est_coord[1],
	     ml->survey[i].est_coord[2],
	     ml->survey[i].coord[0],
	     ml->survey[i].coord[1],
	     ml->survey[i].coord[2]);
      }
    
    /* match up on one node */
    int choice;
    for (i=0; i<MAX_SURVEY_POINTS; i++) 
      if (ml->survey[i].est_used) {
	elog(LOG_WARNING, "chose node %d", ml->survey[i].node);
	choice = i; break;
      }

    for (j=0; j<3; j++) {
      ml->trans[j] = - ml->survey[choice].est_coord[j];
    }
    
    for (i=0; i<MAX_SURVEY_POINTS; i++) 
      if (ml->survey[i].est_used) {
	for (j=0; j<3; j++) {
	  ml->survey[i].est_coord[j] += ml->trans[j];
	}
      }
    
    for (i=0; i<MAX_SURVEY_POINTS; i++) 
      if (ml->survey[i].est_used) {
	elog(LOG_WARNING, "node %d, %f,%f,%f  %f,%f,%f",
	     ml->survey[i].node,
	     ml->survey[i].est_coord[0],
	     ml->survey[i].est_coord[1],
	     ml->survey[i].est_coord[2],
	     ml->survey[i].coord[0],
	     ml->survey[i].coord[1],
	     ml->survey[i].coord[2]);
      }
   
    /* compute rotation */
    
    if (count > 0) {
      init_cm();  
      for (i=0; i<MAX_SURVEY_POINTS; i++) 
	if (i!=choice && ml->survey[i].est_used) {
	  mreal weight = 
	    sqrt(sqrf(ml->survey[i].est_coord[0]) + sqrf(ml->survey[i].est_coord[1]));
	  
	  mreal theta = 
	    atan2(ml->survey[i].est_coord[1], ml->survey[i].est_coord[0]) -
	    atan2(ml->survey[i].coord[1] - ml->survey[choice].coord[1], 
		  ml->survey[i].coord[0] - ml->survey[choice].coord[0]);

	  elog(LOG_WARNING, "adding theta=%f", r2d(theta));

	  add_cm(theta, weight);
	}
      ml->rot = -finish_cm();
    }

    /* rotate the survey points */
    for (i=0; i<MAX_SURVEY_POINTS; i++) 
      if (ml->survey[i].est_used) {
    
 	double tmpx = ml->survey[i].est_coord[0];
	double tmpy = ml->survey[i].est_coord[1];
	
	ml->survey[i].est_coord[0] = (cos(ml->rot)*tmpx - sin(ml->rot)*tmpy);
	ml->survey[i].est_coord[1] = (sin(ml->rot)*tmpx + cos(ml->rot)*tmpy);
      }

    /* compute final translation */

    elog(LOG_WARNING, "before final translation: %f,%f,%f trans",
	 ml->trans[0], ml->trans[1], ml->trans[2]);

    for (i=0; i<MAX_SURVEY_POINTS; i++) 
      if (ml->survey[i].est_used) {
	elog(LOG_WARNING, "node %d, %f,%f,%f  %f,%f,%f",
	     ml->survey[i].node,
	     ml->survey[i].est_coord[0],
	     ml->survey[i].est_coord[1],
	     ml->survey[i].est_coord[2],
	     ml->survey[i].coord[0],
	     ml->survey[i].coord[1],
	     ml->survey[i].coord[2]);
      }

    double centroid[3] = {0,0,0};
    for (i=0; i<MAX_SURVEY_POINTS; i++) 
      if (ml->survey[i].est_used) {
	for (j=0; j<3; j++) {
	  centroid[j] += (ml->survey[i].coord[j] - ml->survey[i].est_coord[j]);
	}
      }
    for (j=0; j<3; j++) {
      centroid[j] = centroid[j] / (double)count;
    }

    /* apply to all nodes */

    elog(LOG_WARNING, "applying translation: %f scale, %f,%f,%f trans, %f rot",
	 ml->scale, ml->trans[0], ml->trans[1], ml->trans[2], r2d(ml->rot));
    
    for (i=0; i<ML_MAX_NODES; i++) {
      if (ml->nodes[i].entry.valid) {
	for (j=0; j<3; j++)
	  ml->nodes[i].coord[j] = ml->nodes[i].coord[j]*ml->scale + ml->trans[j];
	
	double tmpx = ml->nodes[i].coord[0];
	double tmpy = ml->nodes[i].coord[1];
	
	ml->nodes[i].coord[0] = (cos(ml->rot)*tmpx - sin(ml->rot)*tmpy);
	ml->nodes[i].coord[1] = (sin(ml->rot)*tmpx + cos(ml->rot)*tmpy);

	for (j=0; j<3; j++)
	  ml->nodes[i].coord[j] += centroid[j];
	
	ml->nodes[i].yaw += r2d(ml->rot);
      }
    }
  }

  /* build new vector and publish it via statesync */
  if (ml->current_coords)
    buf_free(ml->current_coords);
  ml->current_coords = buf_new();
  buf_t *coords = buf_new();
  count = 0;
  for (i=0; i<ML_MAX_NODES; i++) 
    if (ml->nodes[i].id) {
      coord_entry_table_t e = {
	coord_entry: {
	  valid: ml->nodes[i].entry.valid,
	  node: ml->nodes[i].id,
	  coord: { 
	    ml->nodes[i].coord[0] * 1000,
	    ml->nodes[i].coord[1] * 1000,
	    ml->nodes[i].coord[2] * 1000
	  },
	  rpy: {
	    ml->nodes[i].entry.rpy[0],
	    ml->nodes[i].entry.rpy[1],
	    ml->nodes[i].yaw * 10
	  }
	}
      };
      bufcpy(coords, &e, sizeof(e));

      bufprintf(ml->current_coords, "%d %f %f %f %f %f %f %f %f %f \n",
		ml->nodes[i].id,
		ml->nodes[i].coord[0] * 100.0,
		ml->nodes[i].coord[1] * 100.0,
		ml->nodes[i].coord[2] * 100.0,
		ml->nodes[i].yaw,
		ml->nodes[i].entry.rpy[0]/10.0,
		ml->nodes[i].entry.rpy[1]/10.0,
		ml->nodes[i].survey[0] * 100.0,
		ml->nodes[i].survey[1] * 100.0,
		ml->nodes[i].survey[2] * 100.0);


      count++;
    }
  
  flow_id_t fid = {
    src: my_node_id,
    dst: LINK_BROADCAST,
    max_hops: 10
  };
  if (coord_entry_pub(SSYNC_MULTIHOP_PREFIX, (coord_entry_table_t *)coords->buf,
		      count, &fid) < 0) {
    elog(LOG_WARNING, "Failed to push coords to state sync: %m");
  }
  buf_free(coords);  

  g_status_dev_notify(ml->coords_status);
}

static
void mlat_thread_notify(char opt_char, void *new_value, void *device_info)
{
  ml_state_t *ml = (ml_state_t*)device_info;
  pthread_cond_broadcast(&(ml->range_snapshot_cond));
}


static
void mlat_survey_notify(char opt_char, void *new_value, void *device_info)
{
  int i;
  ml_state_t *ml = (ml_state_t*)device_info;
  
  for (i=0; i<MAX_SURVEY_POINTS; i++) {
    int status = 0;
    if (ml->survey[i].str) 
      status = sscanf(ml->survey[i].str, "%i,%lf,%lf,%lf,%lf", 
		      &ml->survey[i].node,
		      &ml->survey[i].coord[0],
		      &ml->survey[i].coord[1],
		      &ml->survey[i].coord[2],
		      &ml->survey[i].angle);
    if (status == 5) {
      buf_t buf;
      buf_init(&buf);
      bufprintf(&buf, "%u,%.3lf,%.3lf,%.3lf,%.1lf",
		ml->survey[i].node,
		ml->survey[i].coord[0],
		ml->survey[i].coord[1],
		ml->survey[i].coord[2],
		ml->survey[i].angle);
      free(ml->survey[i].str);
      ml->survey[i].str = buf.buf;
      buf.buf = NULL;
      ml->survey[i].angle = ml->survey[i].angle/180.0*M_PI;
    }
    else {
      free(ml->survey[i].str);
      ml->survey[i].str = strdup("-none-");
    }
  }

  ml_do_match(ml);
}


static
int multilat_usage(status_context_t *ctx, buf_t *buf)
{
  bufprintf(buf, 
	    "multilat trigger device\n"
	    "Usage: \n"
	    "       inval:  reset to initial state\n"
	    "       refine: trigger another chirp\n"
	    "       comp:   start computation thread\n"
	    "       survey=<node,x,y,z,a>: define N survey points\n"
	    );

  return STATUS_MSG_COMPLETE;
}


static
int multilat_start(status_context_t *ctx, char *command, size_t buf_size)
{
  ml_state_t *ml = (ml_state_t *)sd_data(ctx);

  parser_state_t *ps = misc_parse_init(command, MISC_PARSE_COLON_SCHEME);
  int inval = 0;
  int trigger = 0;
  int comp_trigger = 0;
  int survey = 0;
  int retval = EVENT_RENEW;

  while (misc_parse_next_kvp(ps) >= 0) {

    if (strcmp(ps->key, "inval") == 0) {
      inval = 1;
      trigger = 1;
    }
    
    else if (strcmp(ps->key, "refine") == 0) {
      trigger = 1;
    }

    else if (strcmp(ps->key, "comp") == 0) {
      comp_trigger = 1;
    }

    else if (strcmp(ps->key, "survey") == 0) {
      if (survey == 0) {
	int i;
	for (i=0; i<MAX_SURVEY_POINTS; i++) {
	  free(ml->survey[i].str);
	  ml->survey[i].str = NULL;
	}
      }

      ml->survey[survey].str = strdup(ps->value);
      survey++;      
    }
    
    else 
      retval = EVENT_ERROR(ENOSYS);
  }
  
  misc_parse_cleanup(ps);

  if (trigger) 
    ml_trigger_ranging(ml, inval);

  if (comp_trigger) 
    mlat_trigger_now(ml, 0, NULL);

  if (survey)
    mlat_survey_notify(0, NULL, ml);
  
  return retval;
}


void ml_update_answers(ml_state_t *ml, coord_entry_t *coords, int count)
{
  int i;

  /* mark all as invalid */
  for (i = 0; i < ML_MAX_NODES; ++i) 
    if (ml->nodes[i].id)
      ml->nodes[i].entry.valid = 0;
  
  /* add data.. */
  for (i=0; i<count; i++) {
    int index = ml_lookup_node(ml->nodes, coords[i].node);
    if (index >= 0)
      memmove(&(ml->nodes[index].entry), &(coords[i]), sizeof(coord_entry_t));
  }

  /* match up and push the data */
  ml_do_match(ml);
}


static
int ml_results_arrived(msg_queue_opts_t *opts, buf_t *buf)
{
  ml_state_t *ml = (ml_state_t *)(opts->private_data);
  buf_t **bufs = (buf_t **)(buf->buf);
  buf_t *answers = bufs[0];  
  buf_t *errors = bufs[1];
  buf_t *stud = bufs[2];

  if (errors) {
    if (ml->current_errors) buf_free(ml->current_errors);
    ml->current_errors = errors;
    g_status_dev_notify(ml->errors_status);
  }

  if (stud) {
    if (ml->current_studbuf) buf_free(ml->current_studbuf);
    ml->current_studbuf = stud;
    g_status_dev_notify(ml->compare_status);
  }

  if (answers) {
    ml_update_answers(ml, (coord_entry_t *)(answers->buf), answers->len / sizeof(coord_entry_t));
    buf_free(answers);
  }

  buf_free(buf);

  return EVENT_RENEW;
}


int main(int argc, char **argv)
{
  ml_state_t ml = {
    flood_interface: "flood",
    useangles: 1,
    outputprefix: "/tmp/"
  };
  
  /* generic init */
  misc_init(&argc, argv, CVSTAG);

  /* set up configurable options */
  option_dev_opts_t opt_opts = {
    root: "loc",
    argc: &argc,
    argv: argv,
    notify: mlat_survey_notify,
    data: &ml
  };
  
  /* turn off abort on error */
  gsl_set_error_handler_off();

  ml.logs_off = misc_parse_out_switch(&argc, argv, "nologs", 0);

  if (g_s_option_dev("survey_points/1", 0, &(ml.survey[0].str),
		     &opt_opts, NULL) < 0) {
    elog(LOG_WARNING, "unable to create options device 0: %m");
    exit(1);
  }

  if (g_s_option_dev("survey_points/2", 0, &(ml.survey[1].str),
		     &opt_opts, NULL) < 0) {
    elog(LOG_WARNING, "unable to create options device 1: %m");
    exit(1);
  }

  if (g_s_option_dev("survey_points/3", 0, &(ml.survey[2].str),
		     &opt_opts, NULL) < 0) {
    elog(LOG_WARNING, "unable to create options device 2: %m");
    exit(1);
  }

  option_dev_opts_t opt_opts2 = {
    root: "loc",
    argc: &argc,
    argv: argv,
    notify: mlat_thread_notify,
    data: &ml
  };

  if (g_b_option_dev("master_mode", 0, &(ml.master_mode),
		     &opt_opts2, NULL) < 0) {
    elog(LOG_WARNING, "unable to create options device master_mode: %m");
    exit(1);
  }
  
  if (g_f_option_dev("resid_thresh", 0, &(ml.studthresh), 0, 100,
		     &opt_opts2, NULL) < 0) {
    elog(LOG_WARNING, "unable to create options device resid_thresh: %m");
    exit(1);
  }
  
  /* open local flood adaptor */
  lu_opts_t opts = {
    opts: {
      name: ml.flood_interface,
      data: &ml
    },
  };
  
  if (lu_open(&opts, &(ml.flood_event)) < 0) {
    elog(LOG_CRIT, "Unable to open flood adapter: %m");
    exit(1);
  }

  /* trigger to start experiment */
  status_dev_opts_t cmd_opts = {
    device: {
      devname: ML_COMMAND,
      device_info: &ml
    },
    write: multilat_start,
    printable: multilat_usage
  };
  
  if (g_status_dev(&cmd_opts, NULL) < 0) {
    elog(LOG_CRIT, "can't create command dev: %m");
    exit(1);
  }

  msg_queue_opts_t mq_opts = {
    cb: ml_results_arrived,
    private_data: &ml,
    name: "results pipe"
  };

  if (g_msg_queue(&mq_opts, &(ml.results_queue)) < 0) {
    elog(LOG_CRIT, "unable to create results queue: %m");
    exit(1);
  }

  /* prints status of multilat */
  status_dev_opts_t s_opts = {
    device: {
      devname: ML_SYNC_STATE_DEVICE,
      device_info: &ml
    },
    printable: ml_sync_status
  };

  if (g_status_dev(&s_opts, &(ml.ml_sync_state)) < 0) {
    elog(LOG_CRIT, "Unable to create ml sync state device ");
    exit(1);
  }

  /* prints status of multilat */
  status_dev_opts_t s_opts2 = {
    device: {
      devname: ML_ERROR_STATUS_DEVICE,
      device_info: &ml
    },
    printable: ml_error_status
  };

  if (g_status_dev(&s_opts2, &(ml.errors_status)) < 0) {
    elog(LOG_CRIT, "Unable to create ml error status device ");
    exit(1);
  }  

  /* prints status of multilat */
  status_dev_opts_t s_opts3 = {
    device: {
      devname: ML_COMPARE_STATUS_DEVICE,
      device_info: &ml
    },
    printable: ml_compare_status
  };

  if (g_status_dev(&s_opts3, &(ml.compare_status)) < 0) {
    elog(LOG_CRIT, "Unable to create ml compare status device ");
    exit(1);
  }  

  /* prints status of multilat */
  status_dev_opts_t s_opts4 = {
    device: {
      devname: ML_CONV_STATUS_DEVICE,
      device_info: &ml
    },
    printable: ml_convergence_status
  };

  if (g_status_dev(&s_opts4, &(ml.converge_status)) < 0) {
    elog(LOG_CRIT, "Unable to create ml conv status device ");
    exit(1);
  }  

  /* prints status of multilat */
  status_dev_opts_t s_opts5 = {
    device: {
      devname: ML_COORDS_STATUS_DEVICE,
      device_info: &ml
    },
    printable: ml_coords_status
  };

  if (g_status_dev(&s_opts5, &(ml.coords_status)) < 0) {
    elog(LOG_CRIT, "Unable to create ml coords status device ");
    exit(1);
  }  

  /* range table updates */
  if (range_entry_sub_open(SSYNC_MULTIHOP_PREFIX, ml_range_update,
			   &ml, &(ml.range_table_event)) < 0) {
    elog(LOG_CRIT, "Can't connect to range table: %m");
    exit(1);    
  }

  /* init range list for processing thread */
  ml.multilat_lists = g_new0(multilat_stages_t, 1);
  range_list_init(ml.multilat_lists);  
  guess_list_init(ml.multilat_lists);  
  result_list_init(ml.multilat_lists);  


  /* init processing thread */
  multilat_thread_init(&ml);


  emrun_opts_t emrun_opts = {
    shutdown: multilat_shutdown,
    data: &ml
  };
  emrun_init(&emrun_opts);
  
  /* run */
  elog_g(LOG_INFO, "Multilat daemon starting...");
  
  g_main();
  return 0;
}

See more files for this project here

EmStar

EmStar is a software system for developing and deploying wireless sensor networks involving Linux-based platforms. As the wireless sensor network community has attempted to deploy more complex designs---large-scale, long-lived systems that need self-organization and adaptivity---a number of difficult software design issues have arisen. Advances in software design have not kept pace with the capabilities of hardware. This is because designing for an adaptive, efficient, and useful sensor network has turned out to be surprisingly complex and difficult. EmStar is a Linux-based software framework, whose goal is to dramatically reduce this complexity, enabling work to be shared and reused, and simplifying and speeding the design of new sensor network applications.

Project homepage: http://cvs.cens.ucla.edu/emstar/
Programming language(s): C,Shell Script
License: other

  test/
    gen_gnuplot.sh
    multilat_generator.c
  coord_guess.c
  invalidate.sh
  loc_test.m
  multilat.c
  multilat_i.h
  multilat_main.c
  result.c