Return to result.c CVS log

Jump to this file's LXR Page

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

modifications needed for rerunning data from marmots demo

/*
 *
 * 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(result_list,_,results,struct _multilat_result,struct _multilat_stages);

void result_list_print(ml_state_t *mls, buf_t *buf) {
  multilat_result_t *rlt = result_list_top(mls->multilat_lists);
  for ( ; rlt != NULL; rlt = result_list_next(rlt) ) {
    bufprintf(buf, "%d %f %f %f %f %f %f %f %f %f\n", rlt->node, rlt->x, rlt->y, rlt->z, 
	      r2d(-rlt->yaw), r2d(-rlt->roll), r2d(-rlt->pitch), 
	      rlt->real_x, rlt->real_y, rlt->real_z);
  }
}

multilat_result_t *result_list_get_init(ml_state_t *mls, uint32_t node, int init) {
  multilat_result_t *result = result_list_top(mls->multilat_lists);
  for ( ; result != NULL; result = result_list_next(result)) {
     if (result->node == node) 
      goto out;
  }
  if (init) {
    result = g_new0(multilat_result_t, 1);
    result->node = node;
    result->nogt = 1;
    result_list_push(mls->multilat_lists, result);
  }
 out:
  return result;
}



void result_list_initnode(ml_state_t *mls, uint32_t node) {
  result_list_get_init(mls, node, 1);
}


multilat_result_t *result_list_get(ml_state_t *mls, uint32_t node) {
  return result_list_get_init(mls, node, 0);
}


void update_result_list(ml_state_t *mls) {
  multilat_range_t *range = range_list_top(mls->multilat_lists);
  for ( ; range != NULL; range = range_list_next(range)) {
    result_list_initnode(mls, range->chirp_from);
    result_list_initnode(mls, range->data_from);
  }
}


void result_list_make_array(ml_state_t *mls, multilat_result_t ** array, int size) {
  int i = 0;
  multilat_result_t *mrt = result_list_top(mls->multilat_lists);
  for (i = 0; i < size; ++i) {
    array[i] = mrt;
    mrt = result_list_next(mrt);
  }
}

int result_list_array_lookup(ml_state_t* mls, multilat_result_t ** array, uint32_t node) {
  int i = 0;
  int size = result_list_qlen(mls->multilat_lists);
  for (i = 0; i < size; ++i) {
    if (array[i]->node == node) {
      return i;
    }
  }
  return -1;
}


void result_save(ml_state_t* mls) {
  multilat_result_t *mrt = result_list_top(mls->multilat_lists);
  for ( ; mrt != NULL; mrt = result_list_next(mrt)) {
    mrt->saved_x = mrt->x;
    mrt->saved_y = mrt->y;
    mrt->saved_z = mrt->z;
    mrt->saved_yaw = mrt->yaw;
  }
}

void result_load(ml_state_t* mls) {
  multilat_result_t *mrt = result_list_top(mls->multilat_lists);
  for ( ; mrt != NULL; mrt = result_list_next(mrt)) {
    mrt->x = mrt->saved_x;
    mrt->y = mrt->saved_y;
    mrt->z = mrt->saved_z;
    mrt->yaw = mrt->saved_yaw;
  }
}



void add_fuzz_to_results(ml_state_t *mls) {

  multilat_result_t *res = result_list_top(mls->multilat_lists);
  while (res != NULL) {
    if (res->state != RESULT_COORD_ROOT) {
      res->x = res->x + (random() % 300) - 150;
      res->y = res->y + (random() % 300) - 150;
      res->z = res->z + (random() % 300) - 150;
    }
    res = result_list_next(res);
  }

}