1 /*
2 *
3 * Copyright (c) 2005 The Regents of the University of California. All
4 * rights reserved.
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 *
10 * - Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer.
12 *
13 * - Neither the name of the University nor the names of its
14 * contributors may be used to endorse or promote products derived
15 * from this software without specific prior written permission.
16 *
17 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
18 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
19 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
20 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
21 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
22 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
23 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
24 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
25 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 *
29 */
30
31 #ifndef __MULTILAT_I_H__
32 #define __MULTILAT_I_H__
33
34 #include <devel/state/cluster_map.h>
35 #include <devel/state/ssync.h>
36 #include <libdev/status_client.h>
37 #include <libdev/g_msg_queue.h>
38 #include <timesync/sync.h>
39 #include <emrun/emrun.h>
40 #include <libmisc/misc.h>
41
42 #include <math.h>
43 #include <pthread.h>
44 #include <errno.h>
45
46 #include <gsl/gsl_errno.h>
47 #include <gsl/gsl_math.h>
48 #include <gsl/gsl_linalg.h>
49 #include <gsl/gsl_pow_int.h>
50
51 #define RMS_SYNCED_VALUE 100
52 #define AR_CHIRP_DEVICE sim_path("/dev/loc/trigger_chirp")
53 #define ML_COMMAND sim_path("/dev/loc/mlat_cmd")
54 #define ML_SYNC_STATE_DEVICE sim_path("/dev/loc/sync_state")
55 #define ML_CONV_STATUS_DEVICE sim_path("/dev/loc/convergence")
56 #define ML_ERROR_STATUS_DEVICE sim_path("/dev/loc/range_errors")
57 #define ML_COORDS_STATUS_DEVICE sim_path("/dev/loc/dump_coords")
58 #define ML_COMPARE_STATUS_DEVICE sim_path("/dev/loc/residuals")
59
60 #define GUESS_MAX_NEIGHBORS 64
61
62 typedef double mreal;
63
64 /* the next two structs are filled in from multilat_range_t's, and by the guess stage */
65 typedef struct _guess_neigh {
66 uint32_t neighbor; /* this is the guy that sent the chirp */
67 int inited;
68 mreal distance;
69 mreal theta; /* this is how I heard the neighbors chirp comming in */
70 mreal phi;
71 // coord_guess_t *neighbor_cgt; /* evil... points to neighbor's coord_guess */
72 } guess_neigh_t;
73
74 typedef struct _coord_guess coord_guess_t;
75 struct _coord_guess {
76 uint32_t node; /* this is the node that head all the neighs chirps */
77 int total; /* total number of neighs */
78 int done_processing; /* if we are done processing this cgt */
79 guess_neigh_t neighs[GUESS_MAX_NEIGHBORS]; /* probbaly not more that 10, so static */
80 QUEUE_ELEMENT_DECL(_,coord_guess_t);
81 };
82
83
84 #define RESULT_INITIALIZED 0
85 #define RESULT_COORD_ROOT 1
86 #define RESULT_NO_YAW 2
87 #define RESULT_YAW_COMPLETE 4
88 #define RESULT_DONE 8
89
90
91 #define RANGE_IN_USE 0
92 #define RANGE_DROPPED 1
93
94 typedef struct _used_range {
95 mreal range;
96 mreal phi;
97 uint32_t used;
98 uint32_t aset;
99 } used_range_t;
100
101 #define ML_CONSTRAINT_RANGE 1
102 #define ML_CONSTRAINT_THETA 2
103 #define ML_CONSTRAINT_PHI 4
104
105 typedef struct _multilat_result multilat_result_t;
106
107 /* used for the initial translation from the rages */
108 typedef struct _multilat_range multilat_range_t;
109 struct _multilat_range {
110 uint32_t chirp_from;
111 uint32_t data_from;
112 mreal distance;
113 mreal theta;
114 mreal phi;
115 mreal introerr_distance;
116 mreal introerr_theta;
117 mreal introerr_phi;
118 mreal real_distance;
119 mreal real_theta;
120 mreal real_phi;
121 mreal angle_conf;
122 mreal conf;
123 int state;
124 int last_row;
125
126 mreal userange;
127 int drop_r;
128 int drop_t;
129 int drop_p;
130 int use_r;
131 int use_t;
132 int use_p;
133 int use_set;
134
135 multilat_range_t *peer;
136 multilat_result_t *result; /* pointer to result for "DATA FROM" node */
137 QUEUE_ELEMENT_DECL(_,multilat_range_t);
138 };
139
140
141 struct _multilat_result {
142 uint32_t node;
143 mreal x;
144 mreal y;
145 mreal z;
146 mreal yaw;
147 mreal pitch;
148 mreal roll;
149 int total_heard_me;
150 int state;
151 int col; /* for translating from a result view to the column of the big matrix */
152 mreal saved_x;
153 mreal saved_y;
154 mreal saved_z;
155 mreal saved_yaw;
156 mreal real_x;
157 mreal real_y;
158 mreal real_z;
159 mreal real_yaw;
160 int nogt;
161
162 /* computing fwd/rev deltas */
163 mreal delta;
164 int delta_in;
165
166 /* analysis temp vars */
167 mreal tx,ty,tz,tyaw,trx,try,trz;
168 mreal X[3];
169 mreal XG[3];
170
171 QUEUE_ELEMENT_DECL(_,multilat_result_t);
172 };
173
174
175
176 /* holds the various queues and stages we use */
177 typedef struct _multilat_stages {
178
179 /* the final product... fill in as we go */
180 QUEUE_DECL(results,multilat_result_t);
181 /* the initial translation might be able to scratch this one out */
182 QUEUE_DECL(ranges,multilat_range_t);
183 /* used for guessing stage... could be up to 100 or so, so not static array */
184 QUEUE_DECL(guesses,coord_guess_t);
185 } multilat_stages_t;
186
187
188 QUEUE_INLINE_INSTANTIATIONS(result_list,_,results,struct _multilat_result,struct _multilat_stages);
189 QUEUE_INLINE_INSTANTIATIONS(range_list,_,ranges,multilat_range_t,struct _multilat_stages);
190 QUEUE_INLINE_INSTANTIATIONS(guess_list,_,guesses,struct _coord_guess,struct _multilat_stages);
191
192
193
194
195
196 typedef struct ml_node {
197 node_id_t id;
198 uint16_t last_RMS;
199
200 uint sync_ok:1;
201 uint is_neighbor:1;
202 int to_me;
203 int range_count;
204
205 int32_t seqno;
206
207 int times_chirped;
208 struct timeval goodsince; /* not used yet */
209
210 coord_entry_t entry;
211 float coord[3];
212 float yaw;
213
214 float survey[3];
215
216 } ml_node_t;
217
218
219
220 /* want to get rid of these */
221 #define ML_MAX_NODES 100
222
223
224 struct row_info {
225 int c_type;
226 multilat_range_t *range;
227 };
228
229 #define MAX_SURVEY_POINTS 3
230 struct survey_point {
231 char *str;
232 node_id_t node;
233 double coord[3];
234 double angle;
235
236 double est_coord[3];
237 double est_angle;
238 int est_used;
239 };
240
241 typedef struct _ml_state {
242
243 /* events */
244 cluster_map_t *cluster_map;
245 ssync_sub_t *range_table_event;
246 status_client_context_t *sync_params_event;
247 status_context_t *ml_sync_state;
248
249 /* nodes */
250 // needs to use queue stuff
251 ml_node_t nodes[ML_MAX_NODES];
252
253 /* range snapshot and multilat thread stuff */
254 pthread_t multilat_thread;
255 pthread_mutex_t range_snapshot_mutex;
256 pthread_cond_t range_snapshot_cond;
257 multilat_stages_t *multilat_lists; /* store all the lists used for multilat here */
258
259 /* the table to be processed */
260 range_entry_table_t *latest_table;
261 int latest_table_count;
262
263 /* the last table we received.. */
264 range_entry_table_t *last_table;
265 int last_table_count;
266
267 /* mapping info from matrix to ranges */
268 struct row_info *row_info;
269 int row_count;
270
271 int useangles;
272
273 int global_run;
274
275 float studthresh; /* studentized residual threshold */
276
277 mreal guess_rms;
278
279 /* turns off logging */
280 int logs_off;
281
282 // #ifdef STANDALONE_MULTILAT
283 status_context_t *mtrigger;
284 char *filename;
285 char *gt_filename;
286 char *outputprefix;
287 FILE *fakedata;
288 int anode1;
289 mreal x1;
290 mreal y1;
291 mreal z1;
292 mreal a1;
293 int anode2;
294 mreal x2;
295 mreal y2;
296 mreal z2;
297 mreal a2;
298 int noreject;
299 int addfuzz;
300 int input_in_degrees;
301 int include_conf;
302 //#endif
303
304 char *flood_interface;
305 lu_context_t *flood_event;
306 int chirp_count;
307
308 /* survey points for mapping */
309 struct survey_point survey[MAX_SURVEY_POINTS];
310 double rot;
311 double scale;
312 double trans[3];
313 int master_mode;
314 int failed_convergence;
315 int last_update_time;
316 int result_element_count;
317 int range_element_count;
318
319 /* results outputs for main thread */
320 msg_queue_t *results_queue;
321 status_context_t *errors_status;
322 status_context_t *converge_status;
323 status_context_t *compare_status;
324 status_context_t *coords_status;
325 buf_t *current_errors;
326 buf_t *current_studbuf;
327 buf_t *current_coords;
328 g_event_t *range_table_holdoff;
329 } ml_state_t;
330
331
332
333 void initialize_guess_stage(ml_state_t *mls);
334 int guess_coords(ml_state_t *mls);
335
336 void multilat_thread_init(ml_state_t *ml);
337
338
339 void update_result_list(ml_state_t *mls);
340 void result_list_initnode(ml_state_t *mls, uint32_t node);
341 multilat_result_t *result_list_get(ml_state_t *mls, uint32_t node);
342 void result_list_print(ml_state_t *mls, buf_t *buf);
343 void result_list_make_array(ml_state_t *mls, multilat_result_t ** array, int size);
344 int result_list_array_lookup(ml_state_t *mls, multilat_result_t **array, uint32_t node);
345 void result_save(ml_state_t *mls);
346 void result_load(ml_state_t *mls);
347 void add_fuzz_to_results(ml_state_t *mls);
348
349 mreal finish_cm();
350 void add_cm(mreal theta, mreal weight);
351 void init_cm();
352
353
354 #define LOG_ENTRY(args...) \
355 { FILE *f = fopen("/home/data/mlatlog", "a+"); \
356 if (f == NULL) elog(LOG_WARNING, "cant open log file, %m"); \
357 else { struct timeval tv; gettimeofday(&tv, NULL); \
358 fprintf(f, "%ld.%06ld: ", tv.tv_sec, tv.tv_usec); \
359 fprintf(f, args); fprintf(f, "\n"); fclose(f); } }
360
361
362 #endif
363
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.