Return to multilat_i.h 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.
 *
 */

#ifndef __MULTILAT_I_H__
#define __MULTILAT_I_H__

#include <devel/state/cluster_map.h>
#include <devel/state/ssync.h>
#include <libdev/status_client.h>
#include <libdev/g_msg_queue.h>
#include <timesync/sync.h>
#include <emrun/emrun.h>
#include <libmisc/misc.h>

#include <math.h>
#include <pthread.h>
#include <errno.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_pow_int.h>

#define RMS_SYNCED_VALUE 100
#define AR_CHIRP_DEVICE           sim_path("/dev/loc/trigger_chirp")
#define ML_COMMAND                sim_path("/dev/loc/mlat_cmd")
#define ML_SYNC_STATE_DEVICE      sim_path("/dev/loc/sync_state")
#define ML_CONV_STATUS_DEVICE     sim_path("/dev/loc/convergence")
#define ML_ERROR_STATUS_DEVICE    sim_path("/dev/loc/range_errors")
#define ML_COORDS_STATUS_DEVICE   sim_path("/dev/loc/dump_coords")
#define ML_COMPARE_STATUS_DEVICE  sim_path("/dev/loc/residuals")

#define GUESS_MAX_NEIGHBORS 64

typedef double mreal;

/* the next two structs are filled in from multilat_range_t's, and by the guess stage */
typedef struct _guess_neigh {
  uint32_t neighbor; /* this is the guy that sent the chirp */
  int inited;
  mreal distance; 
  mreal theta;     /* this is how I heard the neighbors chirp comming in */
  mreal phi;
  //  coord_guess_t *neighbor_cgt; /* evil... points to neighbor's coord_guess */
} guess_neigh_t;

typedef struct _coord_guess coord_guess_t;
struct _coord_guess {
  uint32_t node; /* this is the node that head all the neighs chirps */
  int total; /* total number of neighs */
  int done_processing; /* if we are done processing this cgt */
  guess_neigh_t neighs[GUESS_MAX_NEIGHBORS]; /* probbaly not more that 10, so static */
  QUEUE_ELEMENT_DECL(_,coord_guess_t);
};


#define RESULT_INITIALIZED 0
#define RESULT_COORD_ROOT 1
#define RESULT_NO_YAW 2
#define RESULT_YAW_COMPLETE 4
#define RESULT_DONE 8


#define RANGE_IN_USE 0
#define RANGE_DROPPED 1

typedef struct _used_range {
  mreal range;
  mreal phi;
  uint32_t used;
  uint32_t aset;
} used_range_t;

#define ML_CONSTRAINT_RANGE 1
#define ML_CONSTRAINT_THETA 2
#define ML_CONSTRAINT_PHI   4

typedef struct _multilat_result multilat_result_t;

/* used for the initial translation from the rages */
typedef struct _multilat_range multilat_range_t;
struct _multilat_range {
  uint32_t chirp_from;
  uint32_t data_from;
  mreal distance;
  mreal theta;
  mreal phi;
  mreal introerr_distance;
  mreal introerr_theta;
  mreal introerr_phi;
  mreal real_distance;
  mreal real_theta;
  mreal real_phi;
  mreal angle_conf;
  mreal conf;
  int state;
  int last_row;
  
  mreal userange;
  int drop_r;  
  int drop_t;
  int drop_p;
  int use_r;  
  int use_t;
  int use_p;
  int use_set;

  multilat_range_t *peer;
  multilat_result_t *result;  /* pointer to result for "DATA FROM" node */
  QUEUE_ELEMENT_DECL(_,multilat_range_t);
};


struct _multilat_result {
  uint32_t node;
  mreal x;
  mreal y;
  mreal z;
  mreal yaw;
  mreal pitch;
  mreal roll;
  int total_heard_me;
  int state;
  int col; /* for translating from a result view to the column of the big matrix */
  mreal saved_x;
  mreal saved_y;
  mreal saved_z;
  mreal saved_yaw;
  mreal real_x;
  mreal real_y;
  mreal real_z;
  mreal real_yaw;
  int nogt;

  /* computing fwd/rev deltas */
  mreal delta;
  int delta_in;

  /* analysis temp vars */
  mreal tx,ty,tz,tyaw,trx,try,trz;
  mreal X[3];
  mreal XG[3];

  QUEUE_ELEMENT_DECL(_,multilat_result_t);
};



/* holds the various queues and stages we use */
typedef struct _multilat_stages {

  /* the final product... fill in as we go */
  QUEUE_DECL(results,multilat_result_t);
  /* the initial translation might be able to scratch this one out */
  QUEUE_DECL(ranges,multilat_range_t);
  /* used for guessing stage... could be up to 100 or so, so not static array */
  QUEUE_DECL(guesses,coord_guess_t);
} multilat_stages_t;


QUEUE_INLINE_INSTANTIATIONS(result_list,_,results,struct _multilat_result,struct _multilat_stages);
QUEUE_INLINE_INSTANTIATIONS(range_list,_,ranges,multilat_range_t,struct _multilat_stages);
QUEUE_INLINE_INSTANTIATIONS(guess_list,_,guesses,struct _coord_guess,struct _multilat_stages);





typedef struct ml_node {
  node_id_t id;
  uint16_t last_RMS;

  uint sync_ok:1;
  uint is_neighbor:1;
  int to_me;
  int range_count;

  int32_t seqno;

  int times_chirped;
  struct timeval goodsince; /* not used yet */

  coord_entry_t entry;
  float coord[3];
  float yaw;
  
  float survey[3];
  
} ml_node_t;



/* want to get rid of these */
#define ML_MAX_NODES 100


struct row_info {
  int c_type;
  multilat_range_t *range;
};

#define MAX_SURVEY_POINTS 3
struct survey_point {
  char *str;
  node_id_t node;
  double coord[3];
  double angle;

  double est_coord[3];
  double est_angle;
  int est_used;
};

typedef struct _ml_state {

  /* events */
  cluster_map_t *cluster_map;
  ssync_sub_t *range_table_event;
  status_client_context_t *sync_params_event;
  status_context_t *ml_sync_state;

  /* nodes */
  // needs to use queue stuff
  ml_node_t nodes[ML_MAX_NODES];

  /* range snapshot and multilat thread stuff */
  pthread_t multilat_thread;
  pthread_mutex_t range_snapshot_mutex;
  pthread_cond_t range_snapshot_cond;
  multilat_stages_t *multilat_lists; /* store all the lists used for multilat here */

  /* the table to be processed */
  range_entry_table_t *latest_table;
  int latest_table_count;

  /* the last table we received.. */
  range_entry_table_t *last_table;
  int last_table_count;

  /* mapping info from matrix to ranges */
  struct row_info *row_info;
  int row_count;

  int useangles;

  int global_run;

  float studthresh; /* studentized residual threshold */

  mreal guess_rms;

  /* turns off logging */
  int logs_off;

  // #ifdef STANDALONE_MULTILAT
  status_context_t *mtrigger;
  char *filename;
  char *gt_filename;
  char *outputprefix;
  FILE *fakedata;
  int anode1;
  mreal x1;
  mreal y1;
  mreal z1;
  mreal a1;
  int anode2;
  mreal x2;
  mreal y2;
  mreal z2;
  mreal a2;
  int noreject;
  int addfuzz;
  int input_in_degrees;
  int include_conf;
  //#endif

  char *flood_interface;
  lu_context_t *flood_event;
  int chirp_count;

  /* survey points for mapping */
  struct survey_point survey[MAX_SURVEY_POINTS];
  double rot;
  double scale;
  double trans[3];
  int master_mode;
  int failed_convergence;
  int last_update_time;
  int result_element_count;
  int range_element_count;
  
  /* results outputs for main thread */
  msg_queue_t *results_queue;
  status_context_t *errors_status;
  status_context_t *converge_status;
  status_context_t *compare_status;
  status_context_t *coords_status;
  buf_t *current_errors;
  buf_t *current_studbuf;
  buf_t *current_coords;
  g_event_t *range_table_holdoff;
} ml_state_t;



void initialize_guess_stage(ml_state_t *mls);
int guess_coords(ml_state_t *mls);

void multilat_thread_init(ml_state_t *ml);


void update_result_list(ml_state_t *mls);
void result_list_initnode(ml_state_t *mls, uint32_t node);
multilat_result_t *result_list_get(ml_state_t *mls, uint32_t node);
void result_list_print(ml_state_t *mls, buf_t *buf);
void result_list_make_array(ml_state_t *mls, multilat_result_t ** array, int size);
int result_list_array_lookup(ml_state_t *mls, multilat_result_t **array, uint32_t node);
void result_save(ml_state_t *mls);
void result_load(ml_state_t *mls);
void add_fuzz_to_results(ml_state_t *mls);

mreal finish_cm();
void add_cm(mreal theta, mreal weight);
void init_cm();


#define LOG_ENTRY(args...) \
{ FILE *f = fopen("/home/data/mlatlog", "a+"); \
if (f == NULL) elog(LOG_WARNING, "cant open log file, %m"); \
else { struct timeval tv; gettimeofday(&tv, NULL); \
fprintf(f, "%ld.%06ld: ", tv.tv_sec, tv.tv_usec); \
fprintf(f, args); fprintf(f, "\n"); fclose(f); } }


#endif