From 3a8f28630608dec5afea00a1dd9609f76d3f5e51 Mon Sep 17 00:00:00 2001 From: Pascal Brisset Date: Mon, 5 Sep 2005 22:02:27 +0000 Subject: [PATCH] *** empty log message *** --- sw/ground_segment/multimon/demodml.c | 30 ++++--- sw/ground_segment/multimon/pprzlib.c | 113 ++++++++++----------------- sw/ground_segment/multimon/pprzlib.h | 1 + sw/tools/fp_syntax.ml | 12 +-- 4 files changed, 64 insertions(+), 92 deletions(-) diff --git a/sw/ground_segment/multimon/demodml.c b/sw/ground_segment/multimon/demodml.c index 94875f701b..5bed9e3c86 100644 --- a/sw/ground_segment/multimon/demodml.c +++ b/sw/ground_segment/multimon/demodml.c @@ -14,17 +14,21 @@ ml_demod_init(value dev) { value ml_demod_get_data(value unit) { struct data *data = pprz_demod_read_data(); - int i; - - CAMLparam0(); - CAMLlocal3 (result,l, r); - result = alloc(2, 0); - l = alloc_string(data->len_left); - for(i = 0; i < data->len_left; i++) Byte(l, i) = data->data_left[i]; - r = alloc_string(data->len_right); - for(i = 0; i < data->len_right; i++) Byte(r, i) = data->data_right[i]; - - Store_field(result, 0, l); - Store_field(result, 1, r); - CAMLreturn (result); + if (data) { + int i; + + CAMLparam0(); + CAMLlocal3 (result,l, r); + result = alloc(2, 0); + l = alloc_string(data->len_left); + for(i = 0; i < data->len_left; i++) Byte(l, i) = data->data_left[i]; + r = alloc_string(data->len_right); + for(i = 0; i < data->len_right; i++) Byte(r, i) = data->data_right[i]; + + Store_field(result, 0, l); + Store_field(result, 1, r); + CAMLreturn (result); + } else { + failwith("End of file"); + } } diff --git a/sw/ground_segment/multimon/pprzlib.c b/sw/ground_segment/multimon/pprzlib.c index 297fce1e8b..caa9f83cfe 100644 --- a/sw/ground_segment/multimon/pprzlib.c +++ b/sw/ground_segment/multimon/pprzlib.c @@ -225,7 +225,6 @@ afsk48p_demod(struct demod_state *s, float *buffer, int length, char* data) } } -int fmt = 0; int fd; union { short s[8192]; @@ -250,16 +249,8 @@ input_init(const char *ifname) { exit (10); } if (sndparam != AFMT_S16_LE) { - fmt = 1; - sndparam = AFMT_U8; - if (ioctl(fd, SNDCTL_DSP_SETFMT, &sndparam) == -1) { - perror("ioctl: SNDCTL_DSP_SETFMT"); - exit (10); - } - if (sndparam != AFMT_U8) { - perror("ioctl: SNDCTL_DSP_SETFMT"); - exit (10); - } + perror("ioctl: AFMT_S16_LE"); + exit (10); } stereo = TRUE; sndparam = 1; /* we want 2 channels */ @@ -300,80 +291,56 @@ char data_left[OUTPUT_BUF_LEN+1]; char data_right[OUTPUT_BUF_LEN+1]; struct data data_received = {data_left, 0, data_right, 0}; unsigned int fbuf_cnt = 0; +float fbuf[2][16384]; struct data* pprz_demod_read_data(void) { unsigned int overlap = CORRLEN; int i; - unsigned char *bp; short *sp; - float fbuf[2][16384]; - if (fmt) { - perror("FIXME: stereo&fmt missing"); - exit(1); - - - i = read(fd, bp = b.b, sizeof(b.b)); - if (i < 0 && errno != EAGAIN) { - perror("read"); - exit(4); - } - if (i > 0) { - for (; i >= sizeof(b.b[0]); i -= sizeof(b.b[0]), sp++) - fbuf[LEFT][fbuf_cnt++] = ((int)(*bp)-0x80) * (1.0/128.0); - if (i) - fprintf(stderr, "warning: noninteger number of samples read\n"); - if (fbuf_cnt > overlap) { - glob_data_received_len = 0; - afsk48p_demod(&dem_st[LEFT], fbuf[LEFT], fbuf_cnt-overlap, data_received.data_left); - data_received.len_left = glob_data_received_len; - memmove(fbuf[LEFT], fbuf[LEFT]+fbuf_cnt-overlap, overlap*sizeof(fbuf[LEFT][0])); - fbuf_cnt = overlap; - } - } - } else { - i = read(fd, sp = b.s, sizeof(b.s)); - if (i < 0 && errno != EAGAIN) { - perror("read"); - exit(4); - } - if (i > 0) { - if (stereo) { - for (; i >= sizeof(b.s[0]); i -= (sizeof(b.s[0])*2), sp+=2) { - fbuf[LEFT][fbuf_cnt] = (*sp) * (1.0/32768.0); - fbuf[RIGHT][fbuf_cnt++] = (*(sp+1)) * (1.0/32768.0); - } - } else { - for (; i >= sizeof(b.s[0]); i -= sizeof(b.s[0]), sp++) - fbuf[LEFT][fbuf_cnt++] = (*sp) * (1.0/32768.0); - } - if (i) - fprintf(stderr, "warning: noninteger number of samples read\n"); - if (fbuf_cnt > overlap) { - - glob_data_received_len = 0; - afsk48p_demod(&dem_st[LEFT], fbuf[LEFT], fbuf_cnt-overlap, data_received.data_left); - - data_received.len_left = glob_data_received_len; - memmove(fbuf[LEFT], fbuf[LEFT]+fbuf_cnt-overlap, overlap*sizeof(fbuf[LEFT][0])); - if (stereo) { - glob_data_received_len = 0; - afsk48p_demod(&dem_st[RIGHT], fbuf[RIGHT], fbuf_cnt-overlap, data_received.data_right); - data_received.len_right = glob_data_received_len; - memmove(fbuf[RIGHT], fbuf[RIGHT]+fbuf_cnt-overlap, overlap*sizeof(fbuf[RIGHT][0])); - } - - fbuf_cnt = overlap; - } - } + i = read(fd, sp = b.s, sizeof(b.s)); + if (i < 0 && errno != EAGAIN) { + perror("read"); + exit(4); } - return &data_received; + if (i > 0) { + data_received.len_left = 0; + data_received.len_right = 0; + if (stereo) { + for (; i >= sizeof(b.s[0]); i -= (sizeof(b.s[0])*2), sp+=2) { + fbuf[LEFT][fbuf_cnt] = (*sp) * (1.0/32768.0); + fbuf[RIGHT][fbuf_cnt++] = (*(sp+1)) * (1.0/32768.0); + } + } else { + for (; i >= sizeof(b.s[0]); i -= sizeof(b.s[0]), sp++) + fbuf[LEFT][fbuf_cnt++] = (*sp) * (1.0/32768.0); + } + if (i) + fprintf(stderr, "warning: noninteger number of samples read\n"); + if (fbuf_cnt > overlap) { + + glob_data_received_len = 0; + afsk48p_demod(&dem_st[LEFT], fbuf[LEFT], fbuf_cnt-overlap, data_received.data_left); + + data_received.len_left = glob_data_received_len; + memmove(fbuf[LEFT], fbuf[LEFT]+fbuf_cnt-overlap, overlap*sizeof(fbuf[LEFT][0])); + if (stereo) { + glob_data_received_len = 0; + afsk48p_demod(&dem_st[RIGHT], fbuf[RIGHT], fbuf_cnt-overlap, data_received.data_right); + data_received.len_right = glob_data_received_len; + memmove(fbuf[RIGHT], fbuf[RIGHT]+fbuf_cnt-overlap, overlap*sizeof(fbuf[RIGHT][0])); + } + fbuf_cnt = overlap; + } + return &data_received; + } else + return NULL; } int pprz_demod_init(char *dev) { - memset(&dem_st, 0, sizeof(dem_st)); + memset(dem_st, 0, 2*sizeof(struct demod_state)); afsk48p_init(&dem_st[LEFT]); afsk48p_init(&dem_st[RIGHT]); return input_init(dev); diff --git a/sw/ground_segment/multimon/pprzlib.h b/sw/ground_segment/multimon/pprzlib.h index 9f9e988837..f90c6b8951 100644 --- a/sw/ground_segment/multimon/pprzlib.h +++ b/sw/ground_segment/multimon/pprzlib.h @@ -2,3 +2,4 @@ int pprz_demod_init(char *dev); struct data { char* data_left; int len_left; char* data_right; int len_right;}; struct data* pprz_demod_read_data(void); +/** Returns NULL if no characters are available on the device */ diff --git a/sw/tools/fp_syntax.ml b/sw/tools/fp_syntax.ml index f9ffc0a442..2b4083bbb2 100644 --- a/sw/tools/fp_syntax.ml +++ b/sw/tools/fp_syntax.ml @@ -39,27 +39,27 @@ let functions = [ "Qdr"; "And"; "Or"; - "RcRoll"; + "RcRoll"; "RcEvent1"; "RcEvent2"; - "RadOfDeg"] (*@ binary_operators @ unary_operators*) + "RadOfDeg"] (* Valid identifiers *) let variables = [ "launch"; "estimator_z"; "estimator_flight_time"; - "estimator_hspeed_mod"; + "estimator_hspeed_mod"; "estimator_theta"; - "circle_count"; + "circle_count"; "vsupply"; "stage_time"; - "stage_time_ds"; + "stage_time_ds"; "block_time"; "SECURITY_ALT"; "GROUND_ALT"; "TRUE"; - "FALSE"; + "FALSE"; "QFU" ]