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 <libmisc/misc.h>
32 #include <math.h>
33 #include <libnl/nl.h>
34 #include <libmisc/misc.h>
35 #include <timesync/sync.h>
36 #include <devel/state/ssync.h>
37 #include <link/link.h>
38 #include <libdev/g_msg_queue.h>
39 #include <libdev/status_dev.h>
40 #include <pthread.h>
41 #include <sensors/vxp.h>
42 #include <emrun/fault_logger.h>
43
44 #ifndef __AR_H__
45 #define __AR_H__
46
47 /* change for 24 bit */
48 #define SAMPLE_TYPE int16_t
49 #define CHANNEL_COUNT 4
50
51 #define AR_CHIRP_DEVICE sim_path("/dev/loc/trigger_chirp")
52 #define AR_SEQNO_DEVICE sim_path("/dev/loc/curr_seqno")
53 #define AR_INDEX_DEVICE sim_path("/dev/loc/curr_index")
54 #define AR_LOCAL_RANGES_DEVICE sim_path("/dev/loc/local_ranges")
55 #define AR_QUEUE_DEVICE sim_path("/dev/loc/queue")
56
57 #define DEV_ENSBOX_POWER sim_path("/dev/ensbox/power")
58 #define BOX_VERSION_FILE sim_path("/etc/box_version")
59
60 #define CHIRP_CHIPS 1024
61 #define MOD_FACTOR 8
62 #define RANGE_FACTOR 4 /* this determines the max detect range. 2 = ~80m */
63 #define EXTRACT_LEN (CHIRP_CHIPS*MOD_FACTOR*RANGE_FACTOR)
64 #define PRE_EXTRACT 100
65
66 /* noise estimation and distribution */
67 #define NORM_NOISEEST 500
68 #define NOISE_FLOOR 6.0
69
70 #define USE_NEW_ARRAY
71
72 /* some constants */
73 #define SPEED_OF_SOUND (340.0) //m/s
74 #define SAMPLE_RATE (48000) //samples/s
75 #define DYN_SAMPLE_RATE (_ar.curr_status.sample_rate ? _ar.curr_status.sample_rate : SAMPLE_RATE)
76 #define AR_FORMAT SND_PCM_FORMAT_S16_LE
77 #define HIGH_PASS_F 2000 //hz
78 #ifdef USE_NEW_ARRAY
79 # define SELF_RANGE_OFFSET 85 //mm
80 #else
81 # define SELF_RANGE_OFFSET 20 //mm
82 #endif
83 #define SELF_RANGE_MIN_CONF 40 //factor
84 #define SELF_RANGE_MIN_AVG 1000 //
85
86 #define SELF_FACTOR 4.0 // accelerate self ranging
87
88 #define MAX_CONVERSION_ERROR 15.0
89
90 /* exhaustive angle test parameters */
91 #define THETA_COUNT 360
92 #define THETA(x) (2.0*M_PI*((float)x)/((float)THETA_COUNT))
93
94 #define PHI_COUNT 180
95 #define PHI(x) (M_PI*((float)x)/((float)PHI_COUNT)-(M_PI/2.0))
96
97 /* options */
98 #define CORRECT_OFFSETS
99
100 /* peak detection adaptive filter parameters */
101 #define TRACE_MAX 10
102 #define ALPHA 0.01
103 #define STARTUP_WINDOW PRE_EXTRACT
104 #define STD_DEV_FACTOR 12.0
105 #define SELFRANGE_STD_DEV_FACTOR 9.0
106 #define THRESH_FACTOR 0.50
107 #define SWING_FUZZ_FACTOR 0.9
108
109 /* DOA2 parameters */
110 #define CLIP_LENGTH 32
111 #define CLIP_RESAMPLE 8
112
113 /* chirp status values */
114 #define CHIRP_STATUS_WAIT 1
115 #define CHIRP_STATUS_PROCESSING 2
116 #define CHIRP_STATUS_DONE 3
117 #define CHIRP_STATUS_FAILED 4
118
119 /* options for LSFIT */
120 #define LSFIT_RESID_MAX 10.0
121 #define CORRECT_NEAR
122 #define DO_SPEED
123 #undef WEIGHT_AGAINST_DERIV
124
125 /* options for the excess correction algorithm
126 *
127 * CORRECT_EXCESS is a start at calibrating the array for more exact
128 * positioning of the microphones. a future system should apply per-array
129 * calibration data in a similar fashion. CORRECT_EXCESS provides a
130 * correction to account for the obstruction of the signal by the self--ranging
131 * speaker. this excess is angle--dependent; mutliplied by the sin(elevation).
132 * a future system would benefit from placing the self--range speaker to the
133 * side where it does not obstruct the signal, but coupled to the microphone.
134 * it would also be good to use smaller mounts for the microphones that don't
135 * obstruct so much.
136 *
137 * when disabling CORRECT_EXCESS, enable DO_SPEED
138 */
139
140 #define CH0_SPK_EXCESS 0.015
141 #undef CORRECT_EXCESS
142 #undef WEIGHT_CH0
143 #undef DIRECT_EXCESS_CORR
144
145
146 typedef struct chirp_record chirp_record_t;
147 typedef struct chirp_result chirp_result_t;
148 typedef struct range_neighbor range_neighbor_t;
149
150 typedef struct {
151 char *flood_interface;
152 char *sensor_interface;
153 char *sound_dev;
154 char *platform;
155 int gpio_channel;
156 uint32_t card_index;
157
158 /* current temp */
159 float curr_temperature; /* 0-30 */
160 float curr_RH; /* 0-100 */
161
162 /* debug mode */
163 char *dump_files_ctrl_string;
164 uint32_t use_old_doa:1;
165 uint32_t no_delta_corr:1;
166 uint32_t use_first_arrival:1;
167 uint32_t use_first_swing:1;
168 uint32_t simple_selfrange:1;
169
170 /* network events */
171 lu_context_t *flood_event;
172 lu_context_t *flood_trigger_event;
173
174 /* thread state */
175 pthread_t thread;
176 msg_queue_t *result_event;
177 active_fault_t *selfrange_fault;
178
179 /* chirp list */
180 pthread_mutex_t chirp_list_mutex;
181 pthread_cond_t chirp_list_cond;
182 QUEUE_DECL(chirps,chirp_record_t);
183 status_context_t *local_queue;
184
185 /* chirp state */
186 uint32_t chirp_seqno;
187 struct timeval last_chirp_time;
188 status_context_t *chirp_cmd;
189 status_context_t *seqno_status;
190 status_context_t *index_status;
191 active_fault_t *selfrange_late;
192
193 /* neighbor list */
194 QUEUE_DECL(neighbors, range_neighbor_t);
195 status_context_t *local_ranges;
196 int published_count;
197
198 /* audio server status */
199 status_client_context_t *audio_status;
200 audiod_status_t curr_status;
201
202 int chirp_len;
203
204 /* which array to use? */
205 int array_type;
206 char *array_spec;
207
208 /* log file */
209 char *ar_log_file;
210 char *ar_log_raw;
211 int chirp_index;
212 int send_index;
213 int raw_as_text;
214 int log_timestamps;
215
216 /* next chirp to be sent */
217 g_event_t *amp_at_time;
218 g_event_t *chirp_refractory;
219 g_event_t *chirp_delay;
220 g_event_t *computation_holdoff;
221 int next_seed;
222 int next_mod;
223 char *next_note;
224 int16_t *next_chirp;
225 int next_nocomp;
226
227 /* stats */
228 int fail_local_delay;
229 int fail_self_range_noconf;
230 int fail_no_snr;
231 int fail_future;
232 int fail_no_convert;
233 int fail_convert_error;
234 int warn_uncorrectable_offset;
235 int fail_other;
236 int trigger_fail_no_convert;
237 int succeeded;
238 } ar_state_t;
239
240
241 struct chirp_record {
242 node_id_t source;
243 float timing_error;
244 int64_t start_sample;
245 range_notify_pkt_t notify;
246 float sample_fraction;
247 float observed_len;
248 float temperature;
249 int priority;
250 int status;
251 buf_t *stamplog;
252 SAMPLE_TYPE raw_data[EXTRACT_LEN][4];
253 QUEUE_ELEMENT_DECL(_,chirp_record_t);
254 };
255
256
257 struct range_neighbor {
258 node_id_t node;
259 range_entry_t published;
260 chirp_result_t *best;
261
262 /* result list */
263 QUEUE_DECL(results,chirp_result_t);
264
265 QUEUE_ELEMENT_DECL(_,range_neighbor_t);
266 };
267
268 struct chirp_result {
269 range_notify_pkt_t notify;
270 range_entry_t entry;
271 int32_t uncombined_range;
272 float max_value;
273 int64_t start_sample;
274 float sample_fraction;
275 float temperature;
276
277 uint16_t status;
278 uint16_t curr_angle:1;
279 uint16_t used:1;
280 uint16_t best:1;
281 uint16_t pad:5;
282 uint16_t angle_group:8;
283
284 float mode_value;
285 float angle_value;
286 float mode;
287 QUEUE_ELEMENT_DECL(_,chirp_result_t);
288 };
289
290
291 QUEUE_INLINE_INSTANTIATIONS(ar_chirp_list,_,chirps,chirp_record_t,ar_state_t);
292 QUEUE_INLINE_INSTANTIATIONS(ar_neighbor_list,_,neighbors,range_neighbor_t,ar_state_t);
293 QUEUE_INLINE_INSTANTIATIONS(ar_result_list,_,results,chirp_result_t,range_neighbor_t);
294
295
296 #define ARRAY_TYPE_ORIG 0
297 #define ARRAY_TYPE_8CM 1
298 #define ARRAY_TYPE_12CM 2
299
300 /* global state var */
301 extern ar_state_t _ar;
302
303 /* ar_doa.c */
304 int ar_find_doa3 (float range, float lags[4][4], float lags_conf[4][4], float array[4][3], float *theta, float *phi,
305 float *speed, float *fit);
306
307 /* ar_pn.c */
308 /* rate_adjust is peer_rate / our_rate
309 * this generates a waveform that matches their rate */
310 void ar_pn_generate_chirp(int16_t *wav, size_t count, int seed, int modulation, float rate_adjust);
311
312 /* ar_send.c */
313 int ar_send_init();
314 int ar_send_trigger(ar_state_t *ar, int seed, int modulation, char *notation, int nocomp);
315 int ar_send_schedule_chirp(ar_state_t *ar, struct timeval *tv, int seed, int delay, int count);
316 void ar_send_invalidate(ar_state_t *ar);
317
318 /* ar_emit.c */
319 void ar_amplifier_enable(ar_state_t *ar, int enable);
320 void ar_emit_current_chirp(ar_state_t *ar, int loop);
321
322 /* ar_net.c */
323 void ar_net_init(ar_state_t *ar);
324 int ar_handle_result(msg_queue_opts_t *opts, buf_t *result);
325 void ar_record_chirp(ar_state_t *ar, node_id_t source, range_notify_pkt_t *notify, int data_len);
326
327 /* ar_table.c */
328 void ar_table_init(ar_state_t *ar);
329 void ar_update_published(ar_state_t *ar, chirp_result_t *result);
330
331 /* ar_dsp.c */
332 void ar_thread_init(ar_state_t *ar);
333 float samples_to_usec(float samples);
334 float usec_to_samples(float usec);
335 int usec_to_mm(float temp, float rh, int usec);
336 int mm_to_usec(float temp, float rh, int mm);
337 void ar_compute_offsets(float theta, float phi, float array[4][3],
338 float offsets[4]);
339 void ar_find_max(int *max_index, float *max,
340 float alpha, int thresh, float std_dev_factor,
341 float *signal, size_t points, float *observed_std_dev);
342 void ar_complex_vector_free(complex *vectors[]);
343 void ar_float_vector_free(float *vectors[]);
344 void ar_process_chirp(ar_state_t *ar, chirp_record_t *chirp, int chirp_len,
345 chirp_result_t *ret_result, int16_t *pn_override, int16_t **ret_bpsk);
346 int ar_update_array_spec(ar_state_t *ar, char *spec);
347
348 /* ar_main.c */
349 void ar_shutdown(void *data);
350
351
352 /*
353 * logging support
354 */
355
356 extern int report_to_stdout;
357 extern char *out_file;
358 extern buf_t *report_line;
359
360 #define REPORT(args...) \
361 do { if (out_file) { \
362 if (report_line == NULL) report_line = buf_new(); \
363 bufprintf(report_line, args); \
364 } } while (0)
365
366 static inline
367 void REPORT_DONE()
368 {
369 if (out_file && report_line) {
370 REPORT("\n");
371 int fd;
372 if (report_to_stdout) fd = STDOUT_FILENO;
373 else fd = open(out_file, O_WRONLY|O_APPEND|O_CREAT, 0666);
374 if (fd < 0)
375 elog(LOG_WARNING, "can't write to log file %s: %m", out_file);
376 else {
377 if (write(fd, report_line->buf, report_line->len) != report_line->len) {
378 elog(LOG_WARNING, "write to log file %s failed: %m", out_file);
379 }
380 if (!report_to_stdout) close(fd);
381 }
382 buf_free(report_line);
383 report_line=NULL;
384 }
385 }
386
387 static inline
388 void REPORT_ERROR(node_id_t source, range_notify_pkt_t *notify, char *msg)
389 {
390 if (out_file) {
391 buf_t *buf = buf_new();
392 bufprintf(buf, "ERR %d %u %d %d %s\n", source, notify->seqno,
393 notify->seed, notify->modulation, msg);
394 int fd;
395 if (report_to_stdout) fd = STDOUT_FILENO;
396 else fd = open(out_file, O_WRONLY|O_APPEND|O_CREAT, 0666);
397 if (fd < 0) elog(LOG_WARNING, "can't write to log file %s: %m", out_file);
398 else {
399 if (write(fd, buf->buf, buf->len) != buf->len) {
400 elog(LOG_WARNING, "write to log file %s failed: %m", out_file);
401 }
402 if (!report_to_stdout) close(fd);
403 }
404 buf_free(buf);
405 }
406
407 if (report_line) buf_free(report_line);
408 report_line=NULL;
409 }
410
411
412 /* original doa algorithm.. */
413 int ar_find_doa(ar_state_t *ar, float array[4][3], float *correls[], complex *fft_input[],
414 complex *fft_ref, float *correl_filtered, size_t chirp_len,
415 float *theta, float *phi, float *SNR,
416 int32_t *max_ind, float *max, float *conf,
417 int32_t *uncombined_range, char **reason);
418
419 #endif
420
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.