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(result_list,_,results,struct _multilat_result,struct _multilat_stages);
35
36 void result_list_print(ml_state_t *mls, buf_t *buf) {
37 multilat_result_t *rlt = result_list_top(mls->multilat_lists);
38 for ( ; rlt != NULL; rlt = result_list_next(rlt) ) {
39 bufprintf(buf, "%d %f %f %f %f %f %f %f %f %f\n", rlt->node, rlt->x, rlt->y, rlt->z,
40 r2d(-rlt->yaw), r2d(-rlt->roll), r2d(-rlt->pitch),
41 rlt->real_x, rlt->real_y, rlt->real_z);
42 }
43 }
44
45 multilat_result_t *result_list_get_init(ml_state_t *mls, uint32_t node, int init) {
46 multilat_result_t *result = result_list_top(mls->multilat_lists);
47 for ( ; result != NULL; result = result_list_next(result)) {
48 if (result->node == node)
49 goto out;
50 }
51 if (init) {
52 result = g_new0(multilat_result_t, 1);
53 result->node = node;
54 result->nogt = 1;
55 result_list_push(mls->multilat_lists, result);
56 }
57 out:
58 return result;
59 }
60
61
62
63 void result_list_initnode(ml_state_t *mls, uint32_t node) {
64 result_list_get_init(mls, node, 1);
65 }
66
67
68 multilat_result_t *result_list_get(ml_state_t *mls, uint32_t node) {
69 return result_list_get_init(mls, node, 0);
70 }
71
72
73 void update_result_list(ml_state_t *mls) {
74 multilat_range_t *range = range_list_top(mls->multilat_lists);
75 for ( ; range != NULL; range = range_list_next(range)) {
76 result_list_initnode(mls, range->chirp_from);
77 result_list_initnode(mls, range->data_from);
78 }
79 }
80
81
82 void result_list_make_array(ml_state_t *mls, multilat_result_t ** array, int size) {
83 int i = 0;
84 multilat_result_t *mrt = result_list_top(mls->multilat_lists);
85 for (i = 0; i < size; ++i) {
86 array[i] = mrt;
87 mrt = result_list_next(mrt);
88 }
89 }
90
91 int result_list_array_lookup(ml_state_t* mls, multilat_result_t ** array, uint32_t node) {
92 int i = 0;
93 int size = result_list_qlen(mls->multilat_lists);
94 for (i = 0; i < size; ++i) {
95 if (array[i]->node == node) {
96 return i;
97 }
98 }
99 return -1;
100 }
101
102
103 void result_save(ml_state_t* mls) {
104 multilat_result_t *mrt = result_list_top(mls->multilat_lists);
105 for ( ; mrt != NULL; mrt = result_list_next(mrt)) {
106 mrt->saved_x = mrt->x;
107 mrt->saved_y = mrt->y;
108 mrt->saved_z = mrt->z;
109 mrt->saved_yaw = mrt->yaw;
110 }
111 }
112
113 void result_load(ml_state_t* mls) {
114 multilat_result_t *mrt = result_list_top(mls->multilat_lists);
115 for ( ; mrt != NULL; mrt = result_list_next(mrt)) {
116 mrt->x = mrt->saved_x;
117 mrt->y = mrt->saved_y;
118 mrt->z = mrt->saved_z;
119 mrt->yaw = mrt->saved_yaw;
120 }
121 }
122
123
124
125 void add_fuzz_to_results(ml_state_t *mls) {
126
127 multilat_result_t *res = result_list_top(mls->multilat_lists);
128 while (res != NULL) {
129 if (res->state != RESULT_COORD_ROOT) {
130 res->x = res->x + (random() % 300) - 150;
131 res->y = res->y + (random() % 300) - 150;
132 res->z = res->z + (random() % 300) - 150;
133 }
134 res = result_list_next(res);
135 }
136
137 }
138
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.