Return to ar_table.c CVS log

Jump to this file's LXR Page

Up to [CENS] / emstar / devel / loc / ar

multilat fixes.. now working

/*
 *
 * 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 <ar.h>


QUEUE_FUNCTION_INSTANTIATIONS(ar_chirp_list,_,chirps,chirp_record_t,ar_state_t);
QUEUE_FUNCTION_INSTANTIATIONS(ar_neighbor_list,_,neighbors,range_neighbor_t,ar_state_t);
QUEUE_FUNCTION_INSTANTIATIONS(ar_result_list,_,results,chirp_result_t,range_neighbor_t);


/* lookup/create */
range_neighbor_t *ar_neighbor_lookup(ar_state_t *ar, node_id_t id, int create)
{
  range_neighbor_t *ptr;
  for (ptr=ar_neighbor_list_top(ar); ptr; ptr=ar_neighbor_list_next(ptr)) {
    if (ptr->node == id)
      return ptr;
  }
  
  if (create) {
    ptr = g_new0(range_neighbor_t, 1);
    ptr->node = id;
    ar_neighbor_list_push(ar, ptr);
  }
  
  return ptr;
}


/*
 *  Local range status output
 */

static
void ar_entry_to_buf(buf_t *buf, range_entry_t *entry) 
{
  bufprintf(buf, "Range: %.3f[%.1f] %.1f azi, %.1f zen [%.1f], seqno=%d",
	    entry->distance / 1000.0,
	    entry->conf / 10.0,
	    entry->theta / 10.0,
	    entry->phi / 10.0,
	    entry->a_conf / 10.0,
	    entry->seqno % 256);
}

static
int ar_local_ranges_print(status_context_t *ctx, buf_t *buf)
{
  ar_state_t *ar = (ar_state_t *)sd_data(ctx);
  bufprintf(buf, 
	    "Local Ranges:\n"
	    "-------------\n");

  range_neighbor_t *n;
  for (n=ar_neighbor_list_top(ar); n; n=ar_neighbor_list_next(n)) {
    chirp_result_t *ptr;
    bufprintf(buf, "Neighbor %s: %sPublished\n",
	      print_if_id(n->node), n->best ? "" : "Not ");

    if (n->best && (n->node != my_node_id)) {
      bufprintf(buf, "  * ");
      ar_entry_to_buf(buf, &(n->published));
      bufprintf(buf, "\n");
    }

    for (ptr=ar_result_list_top(n); ptr; ptr=ar_result_list_next(ptr)) {
      bufprintf(buf, "  %c%d ", ptr->best ? 'B' : ' ', ptr->angle_group);
      ar_entry_to_buf(buf, &(ptr->entry));
      bufprintf(buf, " uncomb=%.3f  status=%d\n", 
		(float)ptr->uncombined_range / 1000.0, ptr->status);
      if (ptr->mode > 0) {
	bufprintf(buf, "       dist-mode=%.3f[%.1f], angle-avg=%.1f\n",
		  ptr->mode_value, ptr->mode, ptr->angle_value);
      }
    }

    bufprintf(buf, "\n");
  }
  
  return STATUS_MSG_COMPLETE;
}

/*
 *  Publication interface to statesync
 */


void ar_republish(ar_state_t *ar)
{
  g_status_dev_notify(ar->local_ranges);

  range_neighbor_t *n;
  buf_t *table = buf_new();

  ar->published_count = 0;
  for (n=ar_neighbor_list_top(ar); n; n=ar_neighbor_list_next(n)) {
    if (n->best && (n->published.conf != 0 || n->node == my_node_id)) {
      ar->published_count++;
      range_entry_table_t entry = {
	range_entry: n->published
      };
      bufcpy(table, &entry, sizeof(entry));
    }
  }  
  
  /* push table */  
  flow_id_t fid = {
    src: my_node_id,
    dst: LINK_BROADCAST,
    max_hops: 10
  };
  if (range_entry_pub(SSYNC_MULTIHOP_PREFIX, (range_entry_table_t *)table->buf,
		      ar->published_count, &fid) < 0) {
    elog(LOG_WARNING, "Failed to push local ranges to state sync: %m");
  }
  buf_free(table);
}


#define AR_ANGLE_SLIP   10  /* degrees */
#define AR_RANGE_SLIP  100  /* mm */

void ar_update_published(ar_state_t *ar, chirp_result_t *result)
{
  chirp_result_t *ptr;
  chirp_result_t *j;
  chirp_result_t *k;

  /* look up / create */
  range_neighbor_t *n = ar_neighbor_lookup(ar, result->entry.source, 1); 
  n->best = NULL;
  
  int inval_all = (result->entry.source == my_node_id);
  
  /* toss any info from previous sequence numbers */
  for (ptr=ar_result_list_top(n); ptr; ) {
    chirp_result_t *tmp = ptr;
    ptr=ar_result_list_next(ptr);
    
    if (tmp->entry.seqno != result->entry.seqno || inval_all) {
      elog(LOG_NOTICE, "Dropping result info for node %s, old seqno %d != %d",
	   print_if_id(result->entry.source), result->entry.seqno, tmp->entry.seqno);
      ar_result_list_remove(n, tmp);
      free(tmp);
    }

    else {
      tmp->used = 0;
      tmp->curr_angle = 0;
      tmp->mode = 0;
      tmp->mode_value = 0;
      tmp->best = 0;
      tmp->angle_group = 0;
      
      /* skip over no conf data */
      if (tmp->entry.conf == 0) tmp->used = 1;
    }
  }
  
  /* push new entry */
  ar_result_list_push(n, result);

  /* 
   * recompute results 
   */

  /* process each angle class */
  int angle_group = 0;
  for (ptr=ar_result_list_top(n); ptr; ptr=ar_result_list_next(ptr)) {
    if (ptr->used == 0) {

      /* locate average of all angles in this class */
      float angle = ((float)ptr->entry.theta / 10.0);
      float angle_sum = angle;
      int count = 1;
      angle_group++;
      ptr->angle_group = angle_group;
      ptr->curr_angle = 1;
      
      for (j=ar_result_list_next(ptr); j; j=ar_result_list_next(j)) 
	if (j->used == 0) {
	  float this_angle = ((float)j->entry.theta / 10.0);
	  
	  /* correct for wrap */
	  if ((this_angle - angle) > (360-AR_ANGLE_SLIP)) {
	    this_angle -= 360.0;
	  }
	  else if ((angle - this_angle) > (360-AR_ANGLE_SLIP)) {
	    this_angle += 360.0;
	  }
	  
	  float angle_diff = (abs)(this_angle - angle);
	  if (angle_diff < AR_ANGLE_SLIP) {
	    angle_sum += this_angle;
	    count++;
	    j->curr_angle = 1;
	    j->angle_group = angle_group;
	  }
	}

      /* find approximate mode */
      for (j=ptr; j; j=ar_result_list_next(j))
	if (j->curr_angle) {

	  for (k=ptr; k; k=ar_result_list_next(k))
	    if (k->curr_angle) {
	      if (abs(k->entry.distance - j->entry.distance) < AR_RANGE_SLIP) {
		/* $$$ could weight by conf */
		j->mode++;
		j->mode_value += k->entry.distance;
	      }
	    }
	}
      
      /* find min mode */
      float min_value = -1;
      k = NULL;
      for (j=ptr; j; j=ar_result_list_next(j))
	if (j->curr_angle && (j->mode > 0)) {
	  j->mode_value = j->mode_value / j->mode;
	  if (min_value < 0 || min_value > j->mode_value) {
	    k = j;
	    min_value = j->mode_value;
	  }
	}
      if (k) {
	k->best = 1;
	k->angle_value = (float)angle_sum / (float)count;
      }

      /* clear angles to used */
      for (j=ptr; j; j=ar_result_list_next(j)) 
	if (j->curr_angle) {
	  j->curr_angle = 0;
	  j->used = 1;
	}
    }
  }

  /* find best choice overall */
  float min_value = -1;
  k = NULL;
  for (ptr=ar_result_list_top(n); ptr; ptr=ar_result_list_next(ptr)) {
    if (ptr->best) {
      if (min_value < 0 || min_value > ptr->mode_value) {
	k = ptr;
	min_value = ptr->mode_value;
      }
    }
  }

  /* update the published info */
  memset(&(n->published), 0, sizeof(n->published));
  if (k) {
    n->published.source = n->node;
    n->published.distance = k->mode_value;
    if (k->angle_value < 0) 
      k->angle_value += 360.0;
    if (k->angle_value >= 360.0)
      k->angle_value -= 360.0;
    n->published.theta = k->angle_value * 10;
    n->published.phi = k->entry.phi;
    n->published.a_conf = k->entry.a_conf;
    n->published.conf = k->entry.conf;
    n->published.seqno = k->entry.seqno;
    n->best = k;
  }

  /* publish!! */
  ar_republish(ar);
}


void ar_table_init(ar_state_t *ar)
{
  /* create local ranges status dev */
  status_dev_opts_t opts2 = {
    device: {
      devname: AR_LOCAL_RANGES_DEVICE,
      device_info: ar
    },
    printable: ar_local_ranges_print
  };

  if (g_status_dev(&opts2, &(ar->local_ranges)) < 0) {
    elog(LOG_CRIT, "Failed to create local ranges device: %m");
  }
}