processdoppler.c from EmStar at Krugle
Show processdoppler.c syntax highlighted
/*
*
* Copyright (c) 2006 The Regents of the University of California. All
* rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* - Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* - Neither the name of the University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
* PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
* OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
#include <devel/loc/ar/ar.h>
#include <sndfile.h>
/*
* processdoppler
*
*/
/*
*/
void usage(char *name)
{
misc_print_usage
(name,
"",
" chirplog: name of chirp log\n"
" synclog: name of sync log\n"
" audio: synced audio data file name\n"
" map: map file name\n"
);
exit(1);
}
#define MAX_ARR 1000
#define MAX_MICS 30
#define MAX_BEAMS 10
#define MAX_FEED 10
#define RCV_COUNT 14
struct chirpentry {
int samples;
int chips;
int code;
float velocity;
float confidence;
float x,y,theta;
float range_est[2][RCV_COUNT];
float range_conf[2][RCV_COUNT];
};
int chirplog_count;
struct chirpentry chirp_log[MAX_ARR];
struct audiosync {
int samples;
int mark;
};
int audio_count;
struct audiosync audio_log[MAX_ARR];
struct syncentry {
double timestamp;
int mark;
int sensorid;
int velocity;
int type;
int samples;
};
int sync_count;
struct syncentry sync_log[MAX_ARR];
#define SYNC 0
#define EVENT 1
#define SPEED 2
struct sensorinfo {
float *channel;
float pos[3];
int is_mic;
int is_feedback;
complex *fft;
complex *fft_correl;
float *correl;
float max;
float mean;
float std_dev;
float max_conf;
int max_index;
};
struct sensorinfo mics[MAX_MICS];
SF_INFO info = {};
struct beaminfo {
int in_use;
float pos[3];
};
struct beaminfo beams[MAX_BEAMS];
int feedback_count=0;
int feedback[MAX_FEED];
int first_common_mark[2];
int last_common_mark[2];
double clock_offset, clock_scale;
int sec2samp(double seconds) {
return seconds * clock_scale - clock_offset;
}
double samp2sec(int samples) {
return (samples + clock_offset) / clock_scale;
}
float mps2coeff(float mps) {
return (SPEED_OF_SOUND-mps)/SPEED_OF_SOUND;
}
/*
some helper functions:
vehiclespeed(sample) returns estimate of vehicle speed in m/s at that sample
vehiclelocation(sample) returns estimate of vehicle location at that sample
*/
float vehiclespeed(int samples)
{
int i;
for (i=0; i<sync_count; i++) {
if (sync_log[i].type == SPEED &&
sync_log[i].samples > samples) {
return sync_log[i].velocity * 0.278;
}
}
return 0;
}
float *generate_chirp(int seed, int length, int *len, int range, float doppler)
{
int chirp_len = length*MOD_FACTOR;
int16_t *next_chirp = g_new0(int16_t, chirp_len);
float *next_chirpf = g_new0(float, range);
if (range < chirp_len) {
printf("## *** chirp len too large: range=%d, len=%d\n",
range, chirp_len);
exit(1);
}
ar_pn_generate_chirp(next_chirp, chirp_len, seed, 0, doppler);
int i;
for (i=0; i<chirp_len; i++) {
next_chirpf[i] = next_chirp[i] / 32768.0;
}
free(next_chirp);
*len = chirp_len;
return next_chirpf;
}
void compute_stats(int i, int range, int chirplen)
{
int j;
float mean = 0;
float variance = 0;
float std_dev = 0;
float max = -1;
int max_index = -1;
/* find max */
for (j=0; j<range; j++) {
if (fabsf(mics[i].correl[j]) > max) {
max = fabsf(mics[i].correl[j]);
max_index = j;
}
}
int start = max_index - chirplen;
int end = max_index + chirplen;
if (end > range) end = range;
if (start < 0) start = 0;
/* compute mean */
for (j=start; j<end; j++)
mean += fabsf(mics[i].correl[j]);
mean /= (float)range;
/* compute variance */
for (j=start; j<end; j++)
variance += sqrf(mean - fabsf(mics[i].correl[j]));
variance /= (float)range;
std_dev = sqrtf(variance);
mics[i].max_index = max_index;
mics[i].max = max;
mics[i].max_conf = (max - mean) / std_dev;
mics[i].std_dev = std_dev;
mics[i].mean = mean;
}
void ar_find_max(int *max_index, float *max,
float alpha, int thresh, float std_dev_factor,
float *signal, size_t points, float *observed_std_dev)
{
int trace_peak = 0;
float mean = 0;
float variance = 0;
int j;
float std_dev = 0;
float max_conf = 0;
*max_index = -1;
*max = 0;
if (observed_std_dev) *observed_std_dev = 0;
/* compute mean */
for (j=0; j<thresh; j++)
mean += fabsf(signal[j]);
mean /= (float)thresh;
/* compute variance */
for (j=0; j<thresh; j++)
variance += sqrf(mean - fabsf(signal[j]));
variance /= (float)thresh;
std_dev = sqrtf(variance);
//printf("## Initial mean/stddev: %f/%f (thresh=%d)\n", mean, std_dev, thresh);
/* continue */
for (j=thresh; j<points; j++) {
float mag = fabsf(signal[j]);
float mag_conf = (mag-mean) / std_dev;
if (mag_conf > max_conf)
max_conf = mag_conf;
/* keep the first peak */
if (trace_peak && mag < *max) {
//elog(LOG_DEBUG(5), "done.. ");
goto done;
}
/* if this is a peak */
if (mag_conf > std_dev_factor) {
/* start/continue tracing */
trace_peak++;
/* record std_dev of first hit */
if (*max_index < 0) {
if (observed_std_dev) *observed_std_dev = std_dev;
}
/* record if this is a max */
if (*max_index < 0 || *max < mag) {
*max_index = j;
*max = mag;
//elog(LOG_WARNING, "tracing, index %d sig=%f m/s=%f/%f", j, signal[j], mean, std_dev);
}
/* or done.. */
else
if (trace_peak > TRACE_MAX) goto done;
}
else {
/* if we hit a peak and we're back here, wait N more samples for max.. */
if (trace_peak) {
if (trace_peak > TRACE_MAX) goto done;
trace_peak++;
}
/* update the filter */
float new_mean = (1.0 - alpha) * mean + alpha * fabsf(signal[j]);
float new_var = (1.0 - alpha) * variance + alpha * sqrf(fabsf(signal[j]) - new_mean);
mean = new_mean;
variance = new_var;
std_dev = sqrtf(variance);
}
}
done:
//REPORT(" %d %.2f ", *max_index, max_conf);
//elog(LOG_WARNING, " Max conf is %.2f (stddev=%.2f)", max_conf, std_dev);
return;
}
void fd_correl(complex *corr, float *corr_td, complex *a, complex *b, size_t points)
{
memmove(corr, a, sizeof(complex)*points/2);
nl_convolve(corr, b, points);
//nl_mult_scalar(corr, 1.0/(float)(points), points/2);
if (corr_td) {
/* inverse */
nl_ifft(corr, corr_td, points);
}
}
void free_chirps()
{
int i;
for (i=0; i<MAX_MICS; i++) {
if (mics[i].fft) free(mics[i].fft);
mics[i].fft = NULL;
if (mics[i].fft_correl) free(mics[i].fft_correl);
mics[i].fft_correl = NULL;
if (mics[i].correl) free(mics[i].correl);
mics[i].correl = NULL;
}
}
void compute_chirp(int i, int chirp_index, complex *fft_ref, int range)
{
int j;
mics[i].fft = g_new(complex,range);
mics[i].fft_correl = g_new(complex,range);
mics[i].correl = g_new(float,range);
nl_fft(mics[i].channel + chirp_log[chirp_index].samples,
mics[i].fft, range);
/* 2khz high pass filter */
for (j=0; j<(range/2 * (2000 / 24000.0)); j++) {
mics[i].fft[j].real = 0;
mics[i].fft[j].imag = 0;
}
/* convolve and inverse */
fd_correl(mics[i].fft_correl, mics[i].correl,
fft_ref, mics[i].fft, range);
}
void compute_ranges(int chirp_index)
{
int i;
int emitter;
for (emitter=0; emitter<2; emitter++) {
/* generate reference */
int length;
int range = 1 << 14;
float *ref = generate_chirp(chirp_log[chirp_index].code+emitter,
chirp_log[chirp_index].chips,
&length, range, mps2coeff(chirp_log[chirp_index].velocity));
/* fft the reference signal */
complex *fft_ref = g_new(complex,range);
nl_fft(ref, fft_ref, range);
/* fft and correlate mics */
for (i=0; i<MAX_MICS; i++) {
if (mics[i].is_mic) {
compute_chirp(i, chirp_index, fft_ref, range);
compute_stats(i, range, length);
chirp_log[chirp_index].range_est[emitter][i] = mics[i].max_index;
chirp_log[chirp_index].range_conf[emitter][i] = mics[i].max_conf;
}
}
free_chirps();
free(fft_ref);
free(ref);
}
}
#if 0
printf("STATS: mean %f stddev %f max %f ind %d conf %f\n",
mics[i].mean, mics[i].std_dev, mics[i].max,
mics[i].max_index, mics[i].max_conf);
#endif
#if 0
float max;
float std_dev;
int max_index;
int first_index = -1;
#define ALPHA 0.01
#define STARTUP_WINDOW PRE_EXTRACT
#define STD_DEV_FACTOR 12.0
/* find maxes */
ar_find_max(&(max_index), &(max),
ALPHA, STARTUP_WINDOW, STD_DEV_FACTOR,
mics[i].correl, range, &(std_dev));
if (std_dev > 0) {
printf("## Found peak of %f[%f] at %d, channel %d\n",
max, max / std_dev, max_index, i);
if (mics[i].is_mic)
printf("%d %d %d %f %f\n",
emitter, i, max_index, max, max / std_dev);
if (first_index < 0 || first_index > max_index) {
first_index = max_index;
}
}
}
}
/* dump the result */
FILE *dump = fopen("/tmp/correl", "w");
for (i=0; i<range; i++) {
fprintf(dump, "%d %f %f ", i, ref[i], fft_ref[i].real);
for (j=0; j<MAX_MICS; j++) {
if (mics[j].is_mic || mics[j].is_feedback) {// if (mics[j].is_mic) {
fprintf(dump,
"%f %f ",
mics[j].channel[i+chirp_log[chirp_index].samples],
mics[j].correl[i]);
}
}
fprintf(dump, "\n");
}
fclose(dump);
}
/* memory leak.. */
free_chirps();
exit(1);
}
#endif
float doppler_test(int chirp_index, float mps, int *max_channel)
{
int emitter,i;
float max_conf = 0;
int max_ind = -1;
int max_emit = -1;
for (emitter=0; emitter<2; emitter++) {
/* only do one emitter */
if (*max_channel >= 0)
emitter = *max_channel / RCV_COUNT;
/* generate reference */
int length;
int range = 1 << 14;
float *ref = generate_chirp(chirp_log[chirp_index].code+emitter,
chirp_log[chirp_index].chips,
&length, range, mps2coeff(mps));
/* fft the reference signal */
complex *fft_ref = g_new(complex,range);
nl_fft(ref, fft_ref, range);
/* fft and correlate mics */
for (i=0; i<MAX_MICS; i++) {
/* only do one channel ? */
if (*max_channel >= 0)
i = *max_channel % RCV_COUNT;
if (mics[i].is_mic) {
compute_chirp(i, chirp_index, fft_ref, range);
compute_stats(i, range, length);
if (mics[i].max_conf > max_conf) {
max_conf = mics[i].max_conf;
max_ind = i;
max_emit = emitter;
}
}
if (*max_channel >= 0) {
goto done;
}
}
done:
free(ref);
free(fft_ref);
free_chirps();
if (*max_channel >= 0)
break;
}
if (*max_channel < 0)
*max_channel = max_ind + RCV_COUNT * max_emit;
return max_conf;
}
#define TRIAL_GRAIN 0.2
void estimate_doppler(float *curr_est, float *range, int *max_channel,
float *max_corr, float *mean_corr, float *std_dev,
int index)
{
int trial_count = (*range / TRIAL_GRAIN);
float trialsfwd[trial_count];
float trialsrev[trial_count];
int maxfwd[trial_count];
int maxrev[trial_count];
memset(trialsfwd, 0, sizeof(trialsfwd));
memset(trialsrev, 0, sizeof(trialsrev));
int i;
printf("#");
for (i=0; i<trial_count; i++) {
printf("trial %d/%d.. ", i, trial_count);
fflush(stdout);
maxfwd[i] = *max_channel;
trialsfwd[i] = doppler_test(index, *curr_est + i*TRIAL_GRAIN, &(maxfwd[i]));
maxrev[i] = *max_channel;
trialsrev[i] = doppler_test(index, *curr_est - i*TRIAL_GRAIN, &(maxrev[i]));
}
printf("\n");
/* find max */
float maxcorr = 0;
float maxv = 0;
int maxind = 0;
for (i=0; i<trial_count; i++) {
if (trialsfwd[i] > maxcorr) {
maxcorr = trialsfwd[i];
maxv = *curr_est + i*TRIAL_GRAIN;
maxind = i;
}
if (trialsrev[i] > maxcorr) {
maxcorr = trialsrev[i];
maxv = *curr_est - i*TRIAL_GRAIN;
maxind = -i;
}
}
int count = 0;
float meancorr = 0;
float varcorr = 0;
for (i=1-trial_count; i<trial_count; i++) {
if (abs(i-maxind) > 0) {
if (i<0)
meancorr += trialsrev[-i];
else
meancorr += trialsfwd[i];
count++;
}
}
meancorr /= (float)count;
for (i=1-trial_count; i<trial_count; i++) {
if (abs(i-maxind) > 0) {
if (i<0)
varcorr += sqrf(meancorr - trialsrev[-i]);
else
varcorr += sqrf(meancorr - trialsfwd[i]);
}
}
varcorr /= (float)count;
*max_corr = maxcorr;
*mean_corr = meancorr;
*std_dev = sqrtf(varcorr);
static float sgn = +1;
float conf = (maxcorr - meancorr) / *std_dev;
//if (conf > 2.5)
if (conf > 3.5) {
*curr_est = maxv;
*range = 4;
if (maxind < 0)
*max_channel = maxrev[-maxind];
else
*max_channel = maxrev[maxind];
sgn = (*curr_est > 0) ? +1 : -1;
}
//else if (conf > 2) {
else if (conf > 3.0) {
if (*max_channel >= 0)
*range = 1.2;
*max_channel = -1;
}
else {
*range = 3;
*curr_est = vehiclespeed(chirp_log[index].samples) * sgn;
sgn = -1.0 * sgn;
*max_channel = -1;
}
#if 0
for (i=trial_count-1; i>=0; i--)
printf("%f %f\n", *curr_est - i*TRIAL_GRAIN, trialsrev[i]);
for (i=0; i<trial_count; i++)
printf("%f %f\n", *curr_est + i*TRIAL_GRAIN, trialsfwd[i]);
#endif
}
int main(int argc, char **argv)
{
/* generic init */
misc_init(&argc, argv, CVSTAG);
char *audio_fn;
char *chirp_fn;
char *sync_fn;
char *map_fn;
chirp_fn = misc_parse_out_option(&argc, argv, "chirplog", 0);
audio_fn = misc_parse_out_option(&argc, argv, "audio", 0);
sync_fn = misc_parse_out_option(&argc, argv, "synclog", 0);
map_fn = misc_parse_out_option(&argc, argv, "map", 0);
if (chirp_fn == NULL || sync_fn == NULL ||
audio_fn == NULL || map_fn == NULL) {
fprintf(stderr, "missing filename(s)\n");
exit(1);
}
if (misc_args_remain(&argc, argv))
usage(argv[0]);
char fname[128];
FILE *f;
char buf[128];
/* load the chirp log */
strcpy(fname, chirp_fn);
strcat(fname, ".log");
f = fopen(fname, "r");
if (f == NULL) {
fprintf(stderr, "unable to open file '%s': %m\n", fname);
exit(1);
}
chirplog_count = 0;
while (NULL != fgets(buf, sizeof(buf), f)) {
if (buf[0] == '#') continue;
int status =
sscanf(buf, "%d %d %d",
&(chirp_log[chirplog_count].samples),
&(chirp_log[chirplog_count].chips),
&(chirp_log[chirplog_count].code));
if (status == 3) {
printf("# chirplog: sample,chips,code: %d %d %d\n",
(chirp_log[chirplog_count].samples),
(chirp_log[chirplog_count].chips),
(chirp_log[chirplog_count].code));
chirplog_count++;
}
}
fclose(f);
/* load the audio sync log */
strcpy(fname, audio_fn);
strcat(fname, ".sync");
f = fopen(fname, "r");
if (f == NULL) {
fprintf(stderr, "unable to open audio log file '%s': %m\n", fname);
exit(1);
}
audio_count = 0;
while (NULL != fgets(buf, sizeof(buf), f)) {
if (buf[0] == '#') continue;
int status =
sscanf(buf, "%d %d",
&(audio_log[audio_count].mark),
&(audio_log[audio_count].samples));
if (status == 2) {
printf("# audio log samples,mark %d %d\n",
(audio_log[audio_count].samples),
(audio_log[audio_count].mark));
audio_count++;
}
}
fclose(f);
printf("## found %d audio log entries\n", audio_count);
/* load the sync log */
strcpy(fname, sync_fn);
//strcat(fname, ".sync");
f = fopen(fname, "r");
if (f == NULL) {
fprintf(stderr, "unable to open sync log file '%s': %m\n", fname);
exit(1);
}
sync_count = 0;
while (NULL != fgets(buf, sizeof(buf), f)) {
if (buf[0] == '#') continue;
char type[20];
int status =
sscanf(buf, "%lf %d %d %d %s",
&(sync_log[sync_count].timestamp),
&(sync_log[sync_count].mark),
&(sync_log[sync_count].sensorid),
&(sync_log[sync_count].velocity),
type);
if (sync_log[sync_count].mark < 0)
sync_log[sync_count].mark += 256;
if (strcmp(type, "sync") == 0) {
sync_log[sync_count].type = SYNC;
printf("# sync entry timestamp,mark %lf %d\n",
(sync_log[sync_count].timestamp),
(sync_log[sync_count].mark));
}
else if (strcmp(type, "event") == 0) {
sync_log[sync_count].type = EVENT;
}
else if (strcmp(type, "speed") == 0) {
sync_log[sync_count].type = SPEED;
}
else {
printf("#unknown entry '%s' in sync log\n", buf);
continue;
}
if (status == 5)
sync_count++;
}
fclose(f);
printf("## found %d sync entries\n", sync_count);
/* load the map */
strcpy(fname, map_fn);
f = fopen(fname, "r");
if (f == NULL) {
fprintf(stderr, "unable to open map file '%s': %m\n", fname);
exit(1);
}
memset(beams, 0, sizeof(beams));
while (NULL != fgets(buf, sizeof(buf), f)) {
if (buf[0] == '#') continue;
char type[20];
int index;
float pos[3];
int status =
sscanf(buf, "%s %d %f %f %f",
type, &index, &pos[0], &pos[1], &pos[2]);
if (status == 5) {
if (strcmp(type, "mic") == 0) {
if (index < MAX_MICS) {
mics[index].is_mic = 1;
memmove(mics[index].pos, pos, sizeof(pos));
printf("# mic mapped: %d %f %f %f\n",
index, pos[0], pos[1], pos[2]);
}
else {
printf("error.. mic index %d\n", index);
exit(1);
}
}
else if (strcmp(type, "feedback") == 0) {
if (index < MAX_MICS) {
mics[index].is_feedback = 1;
memmove(mics[index].pos, pos, sizeof(pos));
feedback[feedback_count] = index;
feedback_count++;
printf("# feedback channel mapped: %d %f %f %f\n",
index, pos[0], pos[1], pos[2]);
}
else {
printf("error.. mic index %d\n", index);
exit(1);
}
}
else if (strcmp(type, "beam") == 0) {
if (index < MAX_BEAMS) {
memmove(beams[index].pos, pos, sizeof(pos));
beams[index].in_use = 1;
}
else {
printf("error.. beam index %d\n", index);
exit(1);
}
}
else {
printf("#unknown entry '%s' in sync log\n", buf);
continue;
}
}
}
fclose(f);
/* load audio data */
strcpy(fname, audio_fn);
strcat(fname, ".wav");
SNDFILE *sf = sf_open(fname, SFM_READ, &info);
int i;
for (i=0; i<info.channels; i++) {
mics[i].channel = malloc(sizeof(float)*info.frames);
}
float tmp[4096][info.channels];
int counter = 0;
while (counter < info.frames) {
int num = sf_readf_float(sf, (float*)tmp, 4096);
for (i=0; i<num; i++) {
int j;
for (j=0; j<info.channels; j++) {
mics[j].channel[counter] = tmp[i][j];
}
counter++;
}
if (num < 0) {
printf("# read error reading audio data? %m \n");
}
}
sf_close(sf);
/*
* ok, all files are read.
* now locate chirp sequence offset:
* start of first chirp in feedback
*/
int chirp_index = 0;
for (i=0; i<info.frames; i++) {
if (fabs(mics[feedback[0]].channel[i]) > 0.25) {
printf("# found chirp at sequence %d\n", i);
if (chirp_index >= chirplog_count) {
printf("## **** would run off end of chirp log!\n");
exit(1);
}
chirp_log[chirp_index].samples = i;
i += (chirp_log[chirp_index].chips + 5) * MOD_FACTOR;
chirp_index++;
}
}
if (chirp_index == 0) {
printf("# no chirp found in channel %d!! aborting\n", feedback[0]);
for (i=0; i<MAX_MICS; i++) {
if (mics[i].channel) {
int j;
printf("channel %d: ", i);
for (j=0; j<10; j++) {
printf("%f ", mics[i].channel[j]);
}
printf("\n");
}
}
exit(1);
}
/* match first, last mark in audio_log to mark in sync_log */
int j;
for (i=0; i<audio_count; i++) {
for (j=0; j<sync_count; j++) {
if (audio_log[i].mark == sync_log[j].mark) {
first_common_mark[0] = i;
first_common_mark[1] = j;
printf("## found first common mark: %lf == %d (index %d,%d)\n",
sync_log[j].timestamp, audio_log[i].samples,
j,i);
goto second;
}
}
}
printf("## NO MARKS IN COMMON\n");
exit(1);
second:
for (i=audio_count-1; i>=0; i--) {
for (j=0; j<sync_count; j++) {
if (audio_log[i].mark == sync_log[j].mark) {
last_common_mark[0] = i;
last_common_mark[1] = j;
printf("## found last common mark: %lf == %d (index %d,%d)\n",
sync_log[j].timestamp, audio_log[i].samples,
j,i);
goto common_found;
}
}
}
common_found:
/* compute mapping from timestamp <--> sample */
if (first_common_mark[0] == last_common_mark[0]) {
printf("## warning... only one common mark found.. \n");
clock_scale = 48000;
}
else {
clock_scale =
(audio_log[last_common_mark[0]].samples -
audio_log[first_common_mark[0]].samples) /
(sync_log[last_common_mark[1]].timestamp -
sync_log[first_common_mark[1]].timestamp);
printf("## estimated sample rate: %lf\n", clock_scale);
}
clock_offset = sync_log[first_common_mark[1]].timestamp * clock_scale -
audio_log[first_common_mark[0]].samples;
printf("## estimated offset value: %lf\n", clock_offset);
/* fill in sample numbers for all entries in sync_log. */
for (i=0; i<sync_count; i++) {
sync_log[i].samples = sec2samp(sync_log[i].timestamp);
}
/* prune chirps to those we have data for */
printf("## Last full chirp is index %d.. dropping remaining %d chirps\n",
chirp_index-1, chirplog_count-chirp_index);
chirplog_count = chirp_index;
/* OK, now we can start correlations */
float curr_est = 0;
float range = 1.0;
float max_corr,mean_corr,std_dev;
int max_channel = -1;
for (i=0; i<chirplog_count; i++) {
estimate_doppler(&curr_est, &range, &max_channel,
&max_corr, &mean_corr, &std_dev,
i);
chirp_log[i].velocity = curr_est;
chirp_log[i].confidence = (max_corr - mean_corr) / std_dev;
compute_ranges(i);
printf("%lf %f %f %d %f start %s\n",
samp2sec(chirp_log[i].samples),
curr_est, max_corr, chirp_log[i].chips,
chirp_log[i].confidence,
(chirp_log[i].confidence < 3.5) ? "noconf" : "");
int j;
int k;
for (k=0; k<2; k++) {
for (j=0; j<RCV_COUNT; j++) {
printf("%lf %d %d %f %f\n",
samp2sec(chirp_log[i].samples),
k, j, chirp_log[i].range_est[k][j],
chirp_log[i].range_conf[k][j]);
}
}
}
return 0;
}
See more files for this project here