Show multilat_i.h 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.
*
*/
#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
See more files for this project here