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
32 #include "multilat_i.h"
33
34 QUEUE_FUNCTION_INSTANTIATIONS(guess_list,_,guesses,struct _coord_guess,struct _multilat_stages);
35
36
37
38
39
40 void coord_guess_add_or_update_neighbor(coord_guess_t *cgt, multilat_range_t *mrt) {
41 /* check if have the neighbor first */
42 int i = 0;
43 for (i = 0; i < GUESS_MAX_NEIGHBORS; ++i) {
44 if (cgt->neighs[i].inited == 0) {
45 goto new;
46 } else if (cgt->neighs[i].neighbor == mrt->chirp_from) {
47 elog(LOG_ERR,"This should not be happening... duplicate in range list! (i=%d) %d == %d",
48 i, cgt->neighs[i].neighbor, mrt->chirp_from);
49 if (cgt->neighs[i].distance > mrt->distance)
50 goto up_distance;
51 else
52 goto out;
53 }
54 }
55 new:
56 cgt->neighs[i].inited = 1;
57 cgt->total = cgt->total + 1;
58 up_distance:
59 cgt->neighs[i].distance = mrt->distance;
60 out:
61 cgt->neighs[i].neighbor = mrt->chirp_from;
62 cgt->neighs[i].theta = mrt->theta;
63 cgt->neighs[i].phi = mrt->phi;
64 return;
65 }
66
67
68 int coord_guess_get_neighbor(coord_guess_t *cgt, uint32_t node) {
69 int i = 0;
70 for (i = 0; i < cgt->total; ++i) {
71 if (cgt->neighs[i].neighbor == node) {
72 return i;
73 }
74 }
75 return -1;
76 }
77
78
79 /* lookup a coord_guess in the list, and figure out if */
80 coord_guess_t *coord_guess_get_init(ml_state_t *mls, uint32_t node, int init) {
81 coord_guess_t *cgt = guess_list_top(mls->multilat_lists);
82 for ( ; cgt != NULL; cgt = guess_list_next(cgt)) {
83 if (cgt->node == node) {
84 goto out;
85 }
86 }
87 if (init) {
88 cgt = g_new0(coord_guess_t, 1);
89 cgt->node = node;
90 guess_list_push(mls->multilat_lists, cgt);
91 }
92 out:
93 return cgt;
94 }
95
96 void corrd_guess_initnode(ml_state_t *mls, uint32_t node) {
97 coord_guess_get_init(mls, node, 1);
98 }
99
100 coord_guess_t *coord_guess_get(ml_state_t *mls, uint32_t node) {
101 return coord_guess_get_init(mls, node, 0);
102 }
103
104
105 /* used to sort the cgt_array on total number of neighbors */
106 int sort_cgt_array_on_neighs(const void *a, const void *b) {
107 return ((coord_guess_t *) b)->total - ((coord_guess_t *) a)->total;
108
109 // if (((coord_guess_t *) a)->total > ((coord_guess_t *) b)->total)
110 // return 1;
111 //else if (((coord_guess_t *) a)->total > ((coord_guess_t *) b)->total)
112 // return -1;
113 // else if (((coord_guess_t *) a)->total == ((coord_guess_t *) b)->total)
114 //return 0;
115 }
116
117
118
119
120 /* public functions */
121
122 void initialize_guess_stage(ml_state_t *mls) {
123
124 multilat_range_t *mrt = range_list_top(mls->multilat_lists);
125
126 for ( ; mrt != NULL; mrt = range_list_next(mrt)) {
127 coord_guess_t *cgt = NULL;
128 cgt = coord_guess_get_init(mls, mrt->data_from, 1);
129 coord_guess_add_or_update_neighbor(cgt, mrt);
130 }
131
132 /* now keep only the short distances between two nodes */
133 #if 0
134 coord_guess_t *cgt = guess_list_top(mls->multilat_lists);
135 coord_guess_t *cgt_temp = NULL;
136 for ( ; cgt != NULL; cgt = guess_list_next(cgt)) {
137 int i = 0;
138 for ( i = 0; i < cgt->total; ++i ) {
139 cgt_temp = coord_guess_find(mls, cgt->neigh[i].neighbor, 0);
140 int loc = coord_guess_has_neighbor(cgt_temp, cgt->node);
141 if (loc != -1) {
142 /* set the distance to the shorter of the two */
143 if (cgt->neigh[i].distance <= cgt_temp->neigh[loc].distance) {
144 cgt_temp->neigh[loc].distance = cgt->neigh[i].distance;
145 } else {
146 cgt->neigh[i].distance = cgt_temp->neigh[loc].distance;
147 }
148 // cgt_temp->neigh[loc].neighbor_cgt = cgt; /* carefull */
149 }
150 // cgt->neigh[i].neighbor_cgt = cgt_temp; /* carefull! */
151 }
152 }
153 #endif
154
155 }
156
157 #if 0
158 void fix_no_hear(ml_state_t *mls, uint32_t node) {
159
160 coord_guess_t *cgt = coord_guess_get(mls, node);
161 multilat_result_t *my_res = result_list_get(mls, node);
162 int i = 0;
163 mreal x = 0, y = 0, z = 0;
164 for (i = 0; i < cgt->total; ++i) {
165 multilat_result_t *neigh_coords = result_list_get(mls, cgt->neighs[i]);
166 x = sinf(cgt->neighs[i].theta) -
167
168 }
169
170
171
172 }
173 #endif
174 /* VERY INEFFICIENT... but clear for now */
175 void calculate_coords_and_yaw(ml_state_t *mls, coord_guess_t *cgt) {
176
177 multilat_result_t *listener_result = result_list_get(mls, cgt->node);
178
179 if (listener_result == NULL) {
180 elog(LOG_WARNING, "unable to look up result for node %d", cgt->node);
181 return;
182 }
183
184 if (listener_result->total_heard_me == 0) {
185 elog(LOG_WARNING, "no-one has heard me.. giving up");
186 return;
187 }
188
189 // 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);
190
191 /* first find coords and yaw of each of the neighs */
192 int i = 0;
193 for (i = 0; i < cgt->total; ++i) {
194 multilat_result_t *chirper_result = result_list_get(mls, cgt->neighs[i].neighbor);
195
196 if (chirper_result->state == RESULT_COORD_ROOT) {
197 continue;
198 }
199
200 #ifdef USEZ
201 /* distance */
202 chirper_result->x += cgt->neighs[i].distance
203 * cosf(listener_result->yaw + cgt->neighs[i].theta)
204 * cosf(cgt->neighs[i].phi)
205 + listener_result->x / (listener_result->total_heard_me + 0.0);
206
207 chirper_result->y += cgt->neighs[i].distance
208 * sinf(listener_result->yaw + cgt->neighs[i].theta)
209 * cosf(cgt->neighs[i].phi)
210 + listener_result->y / (listener_result->total_heard_me + 0.0);
211
212 chirper_result->z += cgt->neighs[i].distance
213 * sinf(cgt->neighs[i].phi)
214 + listener_result->z / (listener_result->total_heard_me + 0.0);
215 #else
216 /* distance */
217 chirper_result->x += cgt->neighs[i].distance
218 * cosf(listener_result->yaw + cgt->neighs[i].theta)
219 + listener_result->x / (listener_result->total_heard_me + 0.0);
220
221 chirper_result->y += cgt->neighs[i].distance
222 * sinf(listener_result->yaw + cgt->neighs[i].theta)
223 + listener_result->y / (listener_result->total_heard_me + 0.0);
224 #endif
225
226 chirper_result->total_heard_me = chirper_result->total_heard_me + 1;
227 #if 0
228 elog(LOG_WARNING, "& coords of node %d are: ", cgt->neighs[i].neighbor);
229 elog(LOG_WARNING, "--- x = %f = dist %f * cosf(orr %f + theta %f) + cur x %f",
230 chirper_result->x / chirper_result->total_heard_me,
231 cgt->neighs[i].distance, listener_result->yaw,cgt->neighs[i].theta,
232 listener_result->x / (listener_result->total_heard_me + 0.0));
233 elog(LOG_WARNING, "--- y = %f = dist %f * sinf(orr %f + theta %f) + cur y %f",
234 chirper_result->y / chirper_result->total_heard_me,
235 cgt->neighs[i].distance, listener_result->yaw,cgt->neighs[i].theta,
236 listener_result->y / (listener_result->total_heard_me + 0.0));
237 #endif
238
239 /* yaw */
240 coord_guess_t *chirper_cgt = coord_guess_get(mls, cgt->neighs[i].neighbor);
241 if (chirper_cgt == NULL) {
242 elog(LOG_CRIT, "oops!!! null cgt for neighbor %d -- skipping", i);
243 continue;
244 }
245 int chirper_neigh_offset_listener = coord_guess_get_neighbor(chirper_cgt, cgt->node);
246
247 if (chirper_neigh_offset_listener == -1 && chirper_result->state == RESULT_INITIALIZED) {
248 chirper_result->state = RESULT_NO_YAW;
249 } else if (chirper_result->state == RESULT_INITIALIZED ||
250 chirper_result->state == RESULT_NO_YAW) {
251 chirper_result->yaw =
252 misc_normalize_angle_rad(listener_result->yaw +
253 cgt->neighs[i].theta + M_PI
254 - chirper_cgt->neighs[chirper_neigh_offset_listener].theta);
255 #if 0
256 elog(LOG_WARNING, "--- orr = %f = orr %f + thetac %f + pi - thetal %f",
257 chirper_result->yaw, listener_result->yaw, cgt->neighs[i].theta,
258 chirper_cgt->neighs[chirper_neigh_offset_listener].theta);
259 #endif
260 chirper_result->state = RESULT_YAW_COMPLETE;
261 }
262 #if 1
263 elog(LOG_WARNING, "Guessing node %d from neigh %d: %f, %f, %f theta: %f status %d",
264 chirper_result->node, cgt->node,
265 chirper_result->x, chirper_result->y,
266 chirper_result->z, chirper_result->yaw, chirper_result->state);
267 #endif
268 }
269 cgt->done_processing = 1;
270 /* then call recursivly on cgt's that are not 'done' */
271 for (i = 0; i < cgt->total; ++i) {
272 coord_guess_t *chirper_cgt = coord_guess_get(mls, cgt->neighs[i].neighbor);
273 if (chirper_cgt == NULL) {
274 elog(LOG_CRIT, "oops2, n %d", i);
275 continue;
276 }
277 if (!chirper_cgt->done_processing) {
278 calculate_coords_and_yaw(mls, chirper_cgt);
279 }
280 }
281 }
282
283
284 int guess_coords(ml_state_t *mls) {
285
286 /* first sort on total number of neighbors */
287 int i = 0;
288 int arr_size = guess_list_qlen(mls->multilat_lists);
289
290 if (arr_size < 3) return -1;
291
292 coord_guess_t *cgt = guess_list_top(mls->multilat_lists);
293
294 coord_guess_t *(cgt_arrayview[arr_size]);
295 int first = -1;
296 int second = -1;
297 /* build an array out of the list so we can sort and then iterate */
298 for ( i = 0; i < arr_size; ++i) {
299 cgt_arrayview[i] = cgt;
300 if (cgt->node == mls->anode1) {
301 // elog(LOG_WARNING, "fixed node %i at %i", mls->anode1, i);
302 multilat_result_t *temp_res = result_list_get(mls, mls->anode1);
303 temp_res->state = RESULT_COORD_ROOT;
304 temp_res->total_heard_me = 1;
305 temp_res->x = mls->x1;
306 temp_res->y = mls->y1;
307 temp_res->z = mls->z1;
308 temp_res->yaw = mls->a1;
309 cgt->done_processing = 1;
310 first = i;
311 }
312 if (cgt->node == mls->anode2) {
313 // elog(LOG_WARNING, "fixed node %i at %i", mls->anode2, i);
314 multilat_result_t *temp_res = result_list_get(mls, mls->anode2);
315 temp_res->state = RESULT_COORD_ROOT;
316 temp_res->total_heard_me = 1;
317 temp_res->x = mls->x2;
318 temp_res->y = mls->y2;
319 temp_res->z = mls->z2;
320 temp_res->yaw = mls->a2;
321 cgt->done_processing = 1;
322 second = i;
323 }
324 cgt = guess_list_next(cgt);
325 }
326 if (first == -1 && second == -1) {
327 qsort(cgt_arrayview, arr_size, sizeof(coord_guess_t *),
328 sort_cgt_array_on_neighs);
329 }
330
331 /* mark the first node at 0,0,0 */
332 if (first == -1 && second == -1) {
333 elog(LOG_WARNING, "Marking %i as 0,0,0", cgt_arrayview[0]->node);
334 multilat_result_t *temp_res = result_list_get(mls, cgt_arrayview[0]->node);
335 temp_res->state = RESULT_COORD_ROOT;
336 temp_res->total_heard_me = 1;
337 cgt_arrayview[0]->done_processing = 1;
338 }
339
340 /* now SOHCAHTOA until you drop */
341 if (first != -1) {
342 elog(LOG_WARNING, "Wroking on fixed node %i", cgt_arrayview[first]->node);
343 calculate_coords_and_yaw(mls,cgt_arrayview[first]);
344 }
345 if (second != -1) {
346 elog(LOG_WARNING, "Wroking on fixed node %i", cgt_arrayview[second]->node);
347 calculate_coords_and_yaw(mls,cgt_arrayview[second]);
348 }
349 if (first == -1 && second == -1) {
350 calculate_coords_and_yaw(mls,cgt_arrayview[0]);
351 }
352
353 for ( i = 0 ; i < arr_size; ++i) {
354 elog(LOG_WARNING, "\n\nTop level processing %d\n", cgt_arrayview[i]->node);
355 if (!cgt_arrayview[i]->done_processing) {
356 calculate_coords_and_yaw(mls, cgt_arrayview[i]);
357 } else {
358 elog(LOG_WARNING, "skipping\n");
359 }
360 }
361
362 /* now average out all the resulting positions */
363 multilat_result_t *res = result_list_top(mls->multilat_lists);
364 for ( ; res != NULL; res = result_list_next(res)) {
365 if (res->total_heard_me != 0) {
366 res->x = res->x / (res->total_heard_me + 0.0);
367 res->y = res->y / (res->total_heard_me + 0.0);
368 res->z = res->z / (res->total_heard_me + 0.0);
369 } else {
370 elog(LOG_WARNING, "No one has heard from node %d", res->node);
371 // fix_no_hear(mls, res->node);
372 res->x = 0;
373 res->y = 0;
374 res->z = 0;
375 res->yaw = 0;
376 /*
377 res->x = 700;
378 res->y = 5500;
379 res->z = 0;
380 res->yaw = M_PI;
381 */
382 }
383 res->col = -1;
384 }
385
386 return 1;
387
388 }
389
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.