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 #include <ar.h>
32
33
34 QUEUE_FUNCTION_INSTANTIATIONS(ar_chirp_list,_,chirps,chirp_record_t,ar_state_t);
35 QUEUE_FUNCTION_INSTANTIATIONS(ar_neighbor_list,_,neighbors,range_neighbor_t,ar_state_t);
36 QUEUE_FUNCTION_INSTANTIATIONS(ar_result_list,_,results,chirp_result_t,range_neighbor_t);
37
38
39 /* lookup/create */
40 range_neighbor_t *ar_neighbor_lookup(ar_state_t *ar, node_id_t id, int create)
41 {
42 range_neighbor_t *ptr;
43 for (ptr=ar_neighbor_list_top(ar); ptr; ptr=ar_neighbor_list_next(ptr)) {
44 if (ptr->node == id)
45 return ptr;
46 }
47
48 if (create) {
49 ptr = g_new0(range_neighbor_t, 1);
50 ptr->node = id;
51 ar_neighbor_list_push(ar, ptr);
52 }
53
54 return ptr;
55 }
56
57
58 /*
59 * Local range status output
60 */
61
62 static
63 void ar_entry_to_buf(buf_t *buf, range_entry_t *entry)
64 {
65 bufprintf(buf, "Range: %.3f[%.1f] %.1f azi, %.1f zen [%.1f], seqno=%d",
66 entry->distance / 1000.0,
67 entry->conf / 10.0,
68 entry->theta / 10.0,
69 entry->phi / 10.0,
70 entry->a_conf / 10.0,
71 entry->seqno % 256);
72 }
73
74 static
75 int ar_local_ranges_print(status_context_t *ctx, buf_t *buf)
76 {
77 ar_state_t *ar = (ar_state_t *)sd_data(ctx);
78 bufprintf(buf,
79 "Local Ranges:\n"
80 "-------------\n");
81
82 range_neighbor_t *n;
83 for (n=ar_neighbor_list_top(ar); n; n=ar_neighbor_list_next(n)) {
84 chirp_result_t *ptr;
85 bufprintf(buf, "Neighbor %s: %sPublished\n",
86 print_if_id(n->node), n->best ? "" : "Not ");
87
88 if (n->best && (n->node != my_node_id)) {
89 bufprintf(buf, " * ");
90 ar_entry_to_buf(buf, &(n->published));
91 bufprintf(buf, "\n");
92 }
93
94 for (ptr=ar_result_list_top(n); ptr; ptr=ar_result_list_next(ptr)) {
95 bufprintf(buf, " %c%d ", ptr->best ? 'B' : ' ', ptr->angle_group);
96 ar_entry_to_buf(buf, &(ptr->entry));
97 bufprintf(buf, " uncomb=%.3f status=%d\n",
98 (float)ptr->uncombined_range / 1000.0, ptr->status);
99 if (ptr->mode > 0) {
100 bufprintf(buf, " dist-mode=%.3f[%.1f], angle-avg=%.1f\n",
101 ptr->mode_value, ptr->mode, ptr->angle_value);
102 }
103 }
104
105 bufprintf(buf, "\n");
106 }
107
108 return STATUS_MSG_COMPLETE;
109 }
110
111 /*
112 * Publication interface to statesync
113 */
114
115
116 void ar_republish(ar_state_t *ar)
117 {
118 g_status_dev_notify(ar->local_ranges);
119
120 range_neighbor_t *n;
121 buf_t *table = buf_new();
122
123 ar->published_count = 0;
124 for (n=ar_neighbor_list_top(ar); n; n=ar_neighbor_list_next(n)) {
125 if (n->best && (n->published.conf != 0 || n->node == my_node_id)) {
126 ar->published_count++;
127 range_entry_table_t entry = {
128 range_entry: n->published
129 };
130 bufcpy(table, &entry, sizeof(entry));
131 }
132 }
133
134 /* push table */
135 flow_id_t fid = {
136 src: my_node_id,
137 dst: LINK_BROADCAST,
138 max_hops: 10
139 };
140 if (range_entry_pub(SSYNC_MULTIHOP_PREFIX, (range_entry_table_t *)table->buf,
141 ar->published_count, &fid) < 0) {
142 elog(LOG_WARNING, "Failed to push local ranges to state sync: %m");
143 }
144 buf_free(table);
145 }
146
147
148 #define AR_ANGLE_SLIP 10 /* degrees */
149 #define AR_RANGE_SLIP 100 /* mm */
150
151 void ar_update_published(ar_state_t *ar, chirp_result_t *result)
152 {
153 chirp_result_t *ptr;
154 chirp_result_t *j;
155 chirp_result_t *k;
156
157 /* look up / create */
158 range_neighbor_t *n = ar_neighbor_lookup(ar, result->entry.source, 1);
159 n->best = NULL;
160
161 int inval_all = (result->entry.source == my_node_id);
162
163 /* toss any info from previous sequence numbers */
164 for (ptr=ar_result_list_top(n); ptr; ) {
165 chirp_result_t *tmp = ptr;
166 ptr=ar_result_list_next(ptr);
167
168 if (tmp->entry.seqno != result->entry.seqno || inval_all) {
169 elog(LOG_NOTICE, "Dropping result info for node %s, old seqno %d != %d",
170 print_if_id(result->entry.source), result->entry.seqno, tmp->entry.seqno);
171 ar_result_list_remove(n, tmp);
172 free(tmp);
173 }
174
175 else {
176 tmp->used = 0;
177 tmp->curr_angle = 0;
178 tmp->mode = 0;
179 tmp->mode_value = 0;
180 tmp->best = 0;
181 tmp->angle_group = 0;
182
183 /* skip over no conf data */
184 if (tmp->entry.conf == 0) tmp->used = 1;
185 }
186 }
187
188 /* push new entry */
189 ar_result_list_push(n, result);
190
191 /*
192 * recompute results
193 */
194
195 /* process each angle class */
196 int angle_group = 0;
197 for (ptr=ar_result_list_top(n); ptr; ptr=ar_result_list_next(ptr)) {
198 if (ptr->used == 0) {
199
200 /* locate average of all angles in this class */
201 float angle = ((float)ptr->entry.theta / 10.0);
202 float angle_sum = angle;
203 int count = 1;
204 angle_group++;
205 ptr->angle_group = angle_group;
206 ptr->curr_angle = 1;
207
208 for (j=ar_result_list_next(ptr); j; j=ar_result_list_next(j))
209 if (j->used == 0) {
210 float this_angle = ((float)j->entry.theta / 10.0);
211
212 /* correct for wrap */
213 if ((this_angle - angle) > (360-AR_ANGLE_SLIP)) {
214 this_angle -= 360.0;
215 }
216 else if ((angle - this_angle) > (360-AR_ANGLE_SLIP)) {
217 this_angle += 360.0;
218 }
219
220 float angle_diff = (abs)(this_angle - angle);
221 if (angle_diff < AR_ANGLE_SLIP) {
222 angle_sum += this_angle;
223 count++;
224 j->curr_angle = 1;
225 j->angle_group = angle_group;
226 }
227 }
228
229 /* find approximate mode */
230 for (j=ptr; j; j=ar_result_list_next(j))
231 if (j->curr_angle) {
232
233 for (k=ptr; k; k=ar_result_list_next(k))
234 if (k->curr_angle) {
235 if (abs(k->entry.distance - j->entry.distance) < AR_RANGE_SLIP) {
236 /* $$$ could weight by conf */
237 j->mode++;
238 j->mode_value += k->entry.distance;
239 }
240 }
241 }
242
243 /* find min mode */
244 float min_value = -1;
245 k = NULL;
246 for (j=ptr; j; j=ar_result_list_next(j))
247 if (j->curr_angle && (j->mode > 0)) {
248 j->mode_value = j->mode_value / j->mode;
249 if (min_value < 0 || min_value > j->mode_value) {
250 k = j;
251 min_value = j->mode_value;
252 }
253 }
254 if (k) {
255 k->best = 1;
256 k->angle_value = (float)angle_sum / (float)count;
257 }
258
259 /* clear angles to used */
260 for (j=ptr; j; j=ar_result_list_next(j))
261 if (j->curr_angle) {
262 j->curr_angle = 0;
263 j->used = 1;
264 }
265 }
266 }
267
268 /* find best choice overall */
269 float min_value = -1;
270 k = NULL;
271 for (ptr=ar_result_list_top(n); ptr; ptr=ar_result_list_next(ptr)) {
272 if (ptr->best) {
273 if (min_value < 0 || min_value > ptr->mode_value) {
274 k = ptr;
275 min_value = ptr->mode_value;
276 }
277 }
278 }
279
280 /* update the published info */
281 memset(&(n->published), 0, sizeof(n->published));
282 if (k) {
283 n->published.source = n->node;
284 n->published.distance = k->mode_value;
285 if (k->angle_value < 0)
286 k->angle_value += 360.0;
287 if (k->angle_value >= 360.0)
288 k->angle_value -= 360.0;
289 n->published.theta = k->angle_value * 10;
290 n->published.phi = k->entry.phi;
291 n->published.a_conf = k->entry.a_conf;
292 n->published.conf = k->entry.conf;
293 n->published.seqno = k->entry.seqno;
294 n->best = k;
295 }
296
297 /* publish!! */
298 ar_republish(ar);
299 }
300
301
302 void ar_table_init(ar_state_t *ar)
303 {
304 /* create local ranges status dev */
305 status_dev_opts_t opts2 = {
306 device: {
307 devname: AR_LOCAL_RANGES_DEVICE,
308 device_info: ar
309 },
310 printable: ar_local_ranges_print
311 };
312
313 if (g_status_dev(&opts2, &(ar->local_ranges)) < 0) {
314 elog(LOG_CRIT, "Failed to create local ranges device: %m");
315 }
316 }
317
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.