Community Company Search Contact

coord_guess.c from EmStar at Krugle

Show coord_guess.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"

QUEUE_FUNCTION_INSTANTIATIONS(guess_list,_,guesses,struct _coord_guess,struct _multilat_stages);





void coord_guess_add_or_update_neighbor(coord_guess_t *cgt, multilat_range_t *mrt) {
  /* check if have the neighbor first */
  int i = 0;
  for (i = 0; i < GUESS_MAX_NEIGHBORS; ++i) {
    if (cgt->neighs[i].inited == 0) {
      goto new;
    } else if (cgt->neighs[i].neighbor == mrt->chirp_from) {
      elog(LOG_ERR,"This should not be happening... duplicate in range list! (i=%d)  %d == %d",
	   i, cgt->neighs[i].neighbor, mrt->chirp_from);
      if (cgt->neighs[i].distance > mrt->distance) 
	goto up_distance;
      else
	goto out;
    }
  }
 new:
  cgt->neighs[i].inited = 1;
  cgt->total = cgt->total + 1;
 up_distance:
  cgt->neighs[i].distance = mrt->distance;
 out:
  cgt->neighs[i].neighbor = mrt->chirp_from;
  cgt->neighs[i].theta = mrt->theta;
  cgt->neighs[i].phi = mrt->phi;
  return;
}


int coord_guess_get_neighbor(coord_guess_t *cgt, uint32_t node) {
  int i = 0;
  for (i = 0; i < cgt->total; ++i) {
    if (cgt->neighs[i].neighbor == node) {
      return i;
    }
  }
  return -1;
}


/* lookup a coord_guess in the list, and figure out if */
coord_guess_t *coord_guess_get_init(ml_state_t *mls, uint32_t node, int init) {
  coord_guess_t *cgt = guess_list_top(mls->multilat_lists);
  for ( ; cgt != NULL; cgt = guess_list_next(cgt)) {
    if (cgt->node == node) {
      goto out;
    }
  }
  if (init) {
    cgt = g_new0(coord_guess_t, 1);
    cgt->node = node;
    guess_list_push(mls->multilat_lists, cgt);
  }
 out:
  return cgt;
}

void corrd_guess_initnode(ml_state_t *mls, uint32_t node) {
  coord_guess_get_init(mls, node, 1);
}

coord_guess_t *coord_guess_get(ml_state_t *mls, uint32_t node) {
  return coord_guess_get_init(mls, node, 0);
}


/* used to sort the cgt_array on total number of neighbors */
int sort_cgt_array_on_neighs(const void *a, const void *b) {
  return ((coord_guess_t *) b)->total - ((coord_guess_t *) a)->total;

  //  if (((coord_guess_t *) a)->total > ((coord_guess_t *) b)->total)
  //   return 1;
  //else if (((coord_guess_t *) a)->total > ((coord_guess_t *) b)->total) 
  // return -1;
  //  else if (((coord_guess_t *) a)->total == ((coord_guess_t *) b)->total)
  //return 0;
}




/* public functions */

void initialize_guess_stage(ml_state_t *mls) {
  
  multilat_range_t *mrt = range_list_top(mls->multilat_lists);
  
  for ( ; mrt != NULL; mrt = range_list_next(mrt)) {
    coord_guess_t *cgt = NULL;
    cgt = coord_guess_get_init(mls, mrt->data_from, 1);
    coord_guess_add_or_update_neighbor(cgt, mrt);
  }

  /* now keep only the short distances between two nodes */
#if 0  
  coord_guess_t *cgt = guess_list_top(mls->multilat_lists);
  coord_guess_t *cgt_temp = NULL;
  for ( ; cgt != NULL; cgt = guess_list_next(cgt)) {
    int i = 0;
    for ( i = 0; i < cgt->total; ++i ) {
      cgt_temp = coord_guess_find(mls, cgt->neigh[i].neighbor, 0);
      int loc = coord_guess_has_neighbor(cgt_temp, cgt->node);
      if (loc != -1) {
	/* set the distance to the shorter of the two */
	if (cgt->neigh[i].distance <= cgt_temp->neigh[loc].distance) {
	  cgt_temp->neigh[loc].distance = cgt->neigh[i].distance;
	} else {
	  cgt->neigh[i].distance = cgt_temp->neigh[loc].distance;
	}
	//	cgt_temp->neigh[loc].neighbor_cgt = cgt; /* carefull */
      }
//      cgt->neigh[i].neighbor_cgt = cgt_temp; /* carefull! */
    }
  }
#endif

}
 
#if 0
void fix_no_hear(ml_state_t *mls, uint32_t node) {

  coord_guess_t *cgt = coord_guess_get(mls, node);
  multilat_result_t *my_res = result_list_get(mls, node);
  int i = 0;
  mreal x = 0, y = 0, z = 0;
  for (i = 0; i < cgt->total; ++i) {
    multilat_result_t *neigh_coords = result_list_get(mls, cgt->neighs[i]);
    x = sinf(cgt->neighs[i].theta) - 

  }



}
#endif
/* VERY INEFFICIENT... but clear for now */
void calculate_coords_and_yaw(ml_state_t *mls, coord_guess_t *cgt) {

  multilat_result_t *listener_result = result_list_get(mls, cgt->node);

  if (listener_result == NULL) {
    elog(LOG_WARNING, "unable to look up result for node %d", cgt->node);
    return;
  }

  if (listener_result->total_heard_me == 0) {
    elog(LOG_WARNING, "no-one has heard me.. giving up");
    return;
  }

  //  elog(LOG_WARNING, "\n\nFind coords for listener: %d at (%f, %f, %f) %f", cgt->node, listener_result->x, listener_result->y, listener_result->z, listener_result->yaw);

  /* first find coords  and yaw of each of the neighs */
  int i = 0;
  for (i = 0; i < cgt->total; ++i) {
    multilat_result_t *chirper_result = result_list_get(mls, cgt->neighs[i].neighbor);

    if (chirper_result->state == RESULT_COORD_ROOT) {
      continue;
    }

#ifdef USEZ
    /* distance */
    chirper_result->x += cgt->neighs[i].distance 
      * cosf(listener_result->yaw + cgt->neighs[i].theta)
      * cosf(cgt->neighs[i].phi)
      + listener_result->x / (listener_result->total_heard_me + 0.0);
    
    chirper_result->y += cgt->neighs[i].distance 
      * sinf(listener_result->yaw + cgt->neighs[i].theta)
      * cosf(cgt->neighs[i].phi)
      + listener_result->y / (listener_result->total_heard_me + 0.0);

    chirper_result->z += cgt->neighs[i].distance 
      * sinf(cgt->neighs[i].phi)
      + listener_result->z / (listener_result->total_heard_me + 0.0);
#else
    /* distance */
    chirper_result->x += cgt->neighs[i].distance 
      * cosf(listener_result->yaw + cgt->neighs[i].theta)
      + listener_result->x / (listener_result->total_heard_me + 0.0);
    
    chirper_result->y += cgt->neighs[i].distance 
      * sinf(listener_result->yaw + cgt->neighs[i].theta)
      + listener_result->y / (listener_result->total_heard_me + 0.0);
#endif

    chirper_result->total_heard_me = chirper_result->total_heard_me + 1;
#if 0
    elog(LOG_WARNING, "& coords of node %d are: ", cgt->neighs[i].neighbor);
    elog(LOG_WARNING, "--- x = %f = dist %f * cosf(orr %f + theta %f) + cur x %f", 
	 chirper_result->x / chirper_result->total_heard_me, 
	 cgt->neighs[i].distance, listener_result->yaw,cgt->neighs[i].theta,
	 listener_result->x / (listener_result->total_heard_me + 0.0));   
    elog(LOG_WARNING, "--- y = %f = dist %f * sinf(orr %f + theta %f) + cur y %f",
	 chirper_result->y / chirper_result->total_heard_me,
	 cgt->neighs[i].distance, listener_result->yaw,cgt->neighs[i].theta,
	 listener_result->y / (listener_result->total_heard_me + 0.0));   
#endif

    /* yaw */
    coord_guess_t *chirper_cgt = coord_guess_get(mls, cgt->neighs[i].neighbor);
    if (chirper_cgt == NULL) {
      elog(LOG_CRIT, "oops!!! null cgt for neighbor %d -- skipping", i);
      continue;
    }
    int chirper_neigh_offset_listener = coord_guess_get_neighbor(chirper_cgt, cgt->node);

    if (chirper_neigh_offset_listener == -1 && chirper_result->state == RESULT_INITIALIZED) {
      chirper_result->state = RESULT_NO_YAW;
    } else if (chirper_result->state == RESULT_INITIALIZED ||
	       chirper_result->state == RESULT_NO_YAW) {
      chirper_result->yaw =
	misc_normalize_angle_rad(listener_result->yaw +
				 cgt->neighs[i].theta + M_PI
				 - chirper_cgt->neighs[chirper_neigh_offset_listener].theta);
#if 0
      elog(LOG_WARNING, "--- orr = %f = orr %f + thetac %f + pi - thetal %f",
	   chirper_result->yaw, listener_result->yaw, cgt->neighs[i].theta, 
	   chirper_cgt->neighs[chirper_neigh_offset_listener].theta);
#endif
      chirper_result->state = RESULT_YAW_COMPLETE;
    }
#if 1
    elog(LOG_WARNING, "Guessing node %d from neigh %d: %f, %f, %f  theta: %f   status %d", 
	 chirper_result->node, cgt->node, 
	 chirper_result->x, chirper_result->y,
	 chirper_result->z, chirper_result->yaw, chirper_result->state);
#endif
  }
  cgt->done_processing = 1;
  /* then call recursivly on cgt's that are not 'done' */
  for (i = 0; i < cgt->total; ++i) {
    coord_guess_t *chirper_cgt = coord_guess_get(mls, cgt->neighs[i].neighbor);
    if (chirper_cgt == NULL) {
      elog(LOG_CRIT, "oops2, n %d", i);
      continue;
    }
    if (!chirper_cgt->done_processing) {
      calculate_coords_and_yaw(mls, chirper_cgt);
    }
  }
}


int guess_coords(ml_state_t *mls) {
  
  /* first sort on total number of neighbors */
  int i = 0;
  int arr_size = guess_list_qlen(mls->multilat_lists);

  if (arr_size < 3) return -1;

  coord_guess_t *cgt = guess_list_top(mls->multilat_lists);

  coord_guess_t *(cgt_arrayview[arr_size]);
  int first = -1;
  int second = -1;
  /* build an array out of the list so we can sort and then iterate */
  for ( i = 0; i < arr_size; ++i) {
    cgt_arrayview[i] = cgt;
    if (cgt->node == mls->anode1) {
      //      elog(LOG_WARNING, "fixed node %i at %i", mls->anode1, i);
      multilat_result_t *temp_res = result_list_get(mls, mls->anode1);
      temp_res->state = RESULT_COORD_ROOT;
      temp_res->total_heard_me = 1;
      temp_res->x = mls->x1;
      temp_res->y = mls->y1;
      temp_res->z = mls->z1;
      temp_res->yaw = mls->a1;
      cgt->done_processing = 1;
      first = i;
    }
    if (cgt->node == mls->anode2) {
      //      elog(LOG_WARNING, "fixed node %i at %i", mls->anode2, i);
      multilat_result_t *temp_res = result_list_get(mls, mls->anode2);
      temp_res->state = RESULT_COORD_ROOT;
      temp_res->total_heard_me = 1;
      temp_res->x = mls->x2;
      temp_res->y = mls->y2;
      temp_res->z = mls->z2;
      temp_res->yaw = mls->a2;
      cgt->done_processing = 1;
      second = i;
    }    
    cgt = guess_list_next(cgt);    
  }
  if (first == -1 && second == -1) {
    qsort(cgt_arrayview, arr_size, sizeof(coord_guess_t *),
	  sort_cgt_array_on_neighs);
  }
  
  /* mark the first node at 0,0,0 */  
  if (first == -1 && second == -1) {
    elog(LOG_WARNING, "Marking %i as 0,0,0", cgt_arrayview[0]->node);
    multilat_result_t *temp_res = result_list_get(mls, cgt_arrayview[0]->node);
    temp_res->state = RESULT_COORD_ROOT;
    temp_res->total_heard_me = 1;
    cgt_arrayview[0]->done_processing = 1;
  }

  /* now SOHCAHTOA until you drop */
  if (first != -1) {
        elog(LOG_WARNING, "Wroking on fixed node %i", cgt_arrayview[first]->node);
    calculate_coords_and_yaw(mls,cgt_arrayview[first]);
  }
  if (second != -1) {
        elog(LOG_WARNING, "Wroking on fixed node %i", cgt_arrayview[second]->node);
    calculate_coords_and_yaw(mls,cgt_arrayview[second]);
  }
  if (first == -1 && second == -1) {
    calculate_coords_and_yaw(mls,cgt_arrayview[0]);
  }

  for ( i = 0 ; i < arr_size; ++i) {
        elog(LOG_WARNING, "\n\nTop level processing %d\n", cgt_arrayview[i]->node);
    if (!cgt_arrayview[i]->done_processing) {
      calculate_coords_and_yaw(mls, cgt_arrayview[i]);
    } else {
      elog(LOG_WARNING, "skipping\n");
    }
  }
  
  /* now average out all the resulting positions */
  multilat_result_t *res = result_list_top(mls->multilat_lists);
  for ( ; res != NULL; res = result_list_next(res)) {
    if (res->total_heard_me != 0) {
      res->x = res->x / (res->total_heard_me + 0.0);
      res->y = res->y / (res->total_heard_me + 0.0);
      res->z = res->z / (res->total_heard_me + 0.0);
    } else {
      elog(LOG_WARNING, "No one has heard from node %d", res->node);
      //      fix_no_hear(mls, res->node);
      res->x = 0;
      res->y = 0;
      res->z = 0;
      res->yaw = 0;
      /*
	res->x = 700;
	res->y = 5500;
	res->z = 0;
	res->yaw = M_PI;
      */
    }
    res->col = -1;
  }

  return 1;

}

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