diff --git a/beamforming/c/CMakeLists.txt b/beamforming/c/CMakeLists.txt index ccfadf8..c45610b 100644 --- a/beamforming/c/CMakeLists.txt +++ b/beamforming/c/CMakeLists.txt @@ -16,6 +16,7 @@ add_library(beamforming_lib mq.c worker.c dfifo.c + filter.c ) diff --git a/beamforming/c/aps.c b/beamforming/c/aps.c index f4315f7..f857c6b 100644 --- a/beamforming/c/aps.c +++ b/beamforming/c/aps.c @@ -5,7 +5,7 @@ // Description: // ////////////////////////////////////////////////////////////////////// - +#define TRACERPLUS (-5) #include "aps.h" #include "ps.h" #include "ascee_alg.h" @@ -170,56 +170,27 @@ cmat* AvPowerSpectra_addTimeData(AvPowerSpectra* aps, dbgassert(timedata->n_rows > 0,"Invalid time data. " "Should at least have one row"); - const us nsamples = timedata->n_rows; - - /* Split up timedata in blocks of size ~ (FIFO_SIZE_MULT-1)nfft */ - const us max_blocksize = (FIFO_SIZE_MULT-1)*nfft; - - us pos = 0; /* Current position in timedata buffer */ - /* dFifo handle */ dFifo* fifo = aps->fifo; + dFifo_push(fifo,timedata); - do { - us nsamples_part = pos+max_blocksize <= nsamples ? - max_blocksize : nsamples-pos; + /* Temporary storage buffer */ + dmat* buffer = &aps->buffer; - /* Obtain sub matrix */ - dmat timedata_part = dmat_submat(timedata, - pos, /* Startrow */ - 0, /* Startcol */ - nsamples_part, /* n_rows */ - nchannels); /* n_cols */ - - if(dFifo_push(fifo,&timedata_part)!=SUCCESS) { - WARN("Fifo push failed."); - } + /* Pop samples from the fifo while there are still at + * least nfft samples available */ + while (dFifo_size(fifo) >= nfft) { + int popped = dFifo_pop(fifo, + buffer, + aps->overlap); /* Keep 'overlap' + * number of samples + * in the queue */ - /* Temporary storage buffer */ - dmat* buffer = &aps->buffer; + dbgassert((us) popped == nfft,"Bug in dFifo"); + /* Process the block of time data */ + AvPowerSpectra_addBlock(aps,buffer); - /* Pop samples from the fifo while there are still at - * least nfft samples available */ - while (dFifo_size(fifo) >= nfft) { - int popped = dFifo_pop(fifo, - buffer, - aps->overlap); /* Keep 'overlap' - * number of samples - * in the queue */ - - dbgassert((us) popped == nfft,"Bug in dFifo"); - /* Process the block of time data */ - AvPowerSpectra_addBlock(aps,buffer); - - } - - dmat_free(&timedata_part); - - /* Update position */ - pos+=nsamples_part; - - } while (pos < nsamples); - dbgassert(pos == nsamples,"BUG"); + } feTRACE(15); return &aps->ps_storage; diff --git a/beamforming/c/ascee_alg.h b/beamforming/c/ascee_alg.h index 8d1d316..218b49c 100644 --- a/beamforming/c/ascee_alg.h +++ b/beamforming/c/ascee_alg.h @@ -122,6 +122,7 @@ static inline void vc_hadamard(vc* result,const vc* a,const vc* b) { check_overflow_vx(*b); feTRACE(15); } + /** * Compute the element-wise (Hadamard) product of a and b, and store * in result diff --git a/beamforming/c/ascee_math.c b/beamforming/c/ascee_math.c index 9a91799..e8e6853 100644 --- a/beamforming/c/ascee_math.c +++ b/beamforming/c/ascee_math.c @@ -16,7 +16,7 @@ #ifdef ASCEE_DEBUG void print_dmat(const dmat* m) { - feTRACE(50); + fsTRACE(50); size_t row,col; for(row=0;rown_rows;row++){ indent_trace(); @@ -28,10 +28,10 @@ void print_dmat(const dmat* m) { printf("\n"); } - fsTRACE(50); + feTRACE(50); } void print_cmat(const cmat* m) { - feTRACE(50); + fsTRACE(50); size_t row,col; for(row=0;rown_rows;row++){ indent_trace(); diff --git a/beamforming/c/ascee_math.h b/beamforming/c/ascee_math.h index 82de673..a61c454 100644 --- a/beamforming/c/ascee_math.h +++ b/beamforming/c/ascee_math.h @@ -163,8 +163,10 @@ static inline void vc_set(vc* vec,const c value){ */ static inline void dmat_set(dmat* mat,const d value){ dbgassert(mat,NULLPTRDEREF); - for(us col=0;coln_cols;col++) { - d_set(getdmatval(mat,0,col),value,mat->n_rows); + if(likely(mat->n_cols && mat->n_rows)) { + for(us col=0;coln_cols;col++) { + d_set(getdmatval(mat,0,col),value,mat->n_rows); + } } } @@ -177,8 +179,10 @@ static inline void dmat_set(dmat* mat,const d value){ */ static inline void cmat_set(cmat* mat,const c value){ dbgassert(mat,NULLPTRDEREF); - for(us col=0;coln_cols;col++) { - c_set(getcmatval(mat,0,col),value,mat->n_rows); + if(likely(mat->n_cols && mat->n_rows)) { + for(us col=0;coln_cols;col++) { + c_set(getcmatval(mat,0,col),value,mat->n_rows); + } } } @@ -297,6 +301,24 @@ static inline dmat dmat_foreign(const us n_rows, dmat result = {n_rows,n_cols,true,n_rows,data}; return result; } +static inline dmat dmat_foreign_vd(vd* vector) { + dbgassert(vector,NULLPTRDEREF); + dmat result = {vector->size,1,true,vector->size,vector->_data}; + return result; +} +/** + * Create vd from foreign data + * + * @param size Size of the vector + * @param data Pointer to data + * + * @return + */ +static inline vd vd_foreign(const us size,d* data) { + dbgassert(data,NULLPTRDEREF); + vd result = {size,true,data}; + return result; +} /** * Creates a cmat from foreign data. Does not copy the data, but only * initializes the row pointers. Assumes column-major ordering for the diff --git a/beamforming/c/ascee_math_raw.h b/beamforming/c/ascee_math_raw.h index 004089b..56c31bc 100644 --- a/beamforming/c/ascee_math_raw.h +++ b/beamforming/c/ascee_math_raw.h @@ -208,7 +208,7 @@ static inline void cd_copy(c to[],const d from[],const us size) { * @param to : Vector to write to * @param from : Vector to read from */ -static inline void c_copy(c to[],const c from[],us size){ +static inline void c_copy(c to[],const c from[],const us size){ #if ASCEE_USE_BLAS == 1 #if ASCEE_DOUBLE_PRECISION @@ -225,12 +225,13 @@ static inline void c_copy(c to[],const c from[],us size){ /** * Multiply y with fac, and add result to x * - * @param x Array to add to - * @param y Array to add to x - * @param fac Factor with which to multiply y - * @param size Size of the arrays + * @param x[in,out] Array to add to + * @param y[in] Array to add to x + * @param[in] fac Factor with which to multiply y + * @param[in] size Size of the arrays */ -static inline void d_add_to(d x[],const d y[],d fac,us size){ +static inline void d_add_to(d x[],const d y[], + const d fac,const us size){ #if ASCEE_USE_BLAS == 1 #if ASCEE_DOUBLE_PRECISION cblas_daxpy(size,fac,y,1,x,1); @@ -246,12 +247,13 @@ static inline void d_add_to(d x[],const d y[],d fac,us size){ /** * x = x + fac*y * - * @param x Array to add to - * @param y Array to add to x - * @param fac Factor with which to multiply y - * @param size Size of the arrays + * @param[in,out] x Array to add to + * @param[in] y Array to add to x + * @param[in] fac Factor with which to multiply y + * @param[in] size Size of the arrays */ -static inline void c_add_to(c x[],const c y[],c fac,us size){ +static inline void c_add_to(c x[],const c y[], + const c fac,const us size){ fsTRACE(15); #if ASCEE_USE_BLAS == 1 #if ASCEE_DOUBLE_PRECISION diff --git a/beamforming/c/ascee_tracer.h b/beamforming/c/ascee_tracer.h index e14f4ce..1139e32 100644 --- a/beamforming/c/ascee_tracer.h +++ b/beamforming/c/ascee_tracer.h @@ -52,29 +52,29 @@ void indent_trace(); * Produce a debug warning */ #define DBGWARN(a) \ - fprintf(stderr,RED);\ - fprintf(stderr,"%s(%d): ", \ - __FILE__, \ - __LINE__ \ - ); \ - fprintf(stderr,a); \ - fprintf(stderr,RESET "\n"); + printf(RED); \ + printf("%s(%d): ", \ + __FILE__, \ + __LINE__ \ + ); \ + printf(a); \ + printf(RESET "\n"); /** * Produce a runtime warning */ -#define WARN(a) \ - fprintf(stderr,RED);\ - fprintf(stderr,"WARNING: "); \ - fprintf(stderr,a); \ - fprintf(stderr,RESET "\n"); +#define WARN(a) \ + printf(RED); \ + printf("WARNING: "); \ + printf(a); \ + printf(RESET "\n"); /** * Fatal error, abort execution */ -#define FATAL(a) \ - WARN(a); \ - abort(); +#define FATAL(a) \ + WARN(a); \ + abort(); diff --git a/beamforming/c/dfifo.c b/beamforming/c/dfifo.c index aeb5166..47bba29 100644 --- a/beamforming/c/dfifo.c +++ b/beamforming/c/dfifo.c @@ -5,9 +5,8 @@ // Description: // Implementation of the dFifo queue ////////////////////////////////////////////////////////////////////// - +#define TRACERPLUS (-5) #include "dfifo.h" -#define DFIFO_QUEUE_MAX_BLOCKS (50) typedef struct dFifo_s { dmat queue; @@ -19,15 +18,46 @@ us dFifo_size(dFifo* fifo) { dbgassert(fifo,NULLPTRDEREF); dbgassert(fifo->start_row <= fifo->end_row,"BUG"); feTRACE(15); - return fifo->end_row-fifo->start_row; } + +/** + * Change the max size of the dFifo to a new max size specified. Max size + * should be larger than fifo size. Resets start row to 0 + * + * @param fifo + * @param new_size + */ +static void dFifo_change_maxsize(dFifo* fifo,const us new_max_size) { + fsTRACE(30); + dmat old_queue = fifo->queue; + + dbgassert(new_max_size >= dFifo_size(fifo),"BUG"); + const us size = dFifo_size(fifo); + + dmat new_queue = dmat_alloc(new_max_size,old_queue.n_cols); + if(size > 0) { + dmat_copy_rows(&new_queue, + &old_queue, + 0, + fifo->start_row, + size); + } + + dmat_free(&old_queue); + fifo->queue = new_queue; + fifo->end_row -= fifo->start_row; + fifo->start_row = 0; + + feTRACE(30); +} + dFifo* dFifo_create(const us nchannels, - const us max_size) { + const us init_size) { fsTRACE(15); dFifo* fifo = a_malloc(sizeof(dFifo)); - fifo->queue = dmat_alloc(max_size,nchannels); + fifo->queue = dmat_alloc(init_size,nchannels); fifo->start_row = 0; fifo->end_row = 0; feTRACE(15); @@ -39,54 +69,37 @@ void dFifo_free(dFifo* fifo) { a_free(fifo); feTRACE(15); } -int dFifo_push(dFifo* fifo,const dmat* data) { +void dFifo_push(dFifo* fifo,const dmat* data) { fsTRACE(15); dbgassert(fifo && data, NULLPTRDEREF); dbgassert(data->n_cols == fifo->queue.n_cols, "Invalid number of columns in data"); + const us added_size = data->n_rows; + dmat queue = fifo->queue; const us max_size = queue.n_rows; - const us nchannels = queue.n_cols; - us* start_row = &fifo->start_row; us* end_row = &fifo->end_row; - const us added_size = data->n_rows; - const us size_before = dFifo_size(fifo); if(added_size + dFifo_size(fifo) > max_size) { - return FIFO_QUEUE_FULL; + dFifo_change_maxsize(fifo,2*(max_size+added_size)); + + /* Now the stack of this function is not valid anymore. Best + * thing to do is restart the function. */ + dFifo_push(fifo,data); + feTRACE(15); + return; } + else if(*end_row + added_size > max_size) { + dFifo_change_maxsize(fifo,max_size); + /* Now the stack of this function is not valid anymore. Best + * thing to do is restart the function. */ + dFifo_push(fifo,data); + feTRACE(15); + return; - if(*end_row + added_size > max_size) { - if(size_before != 0) { - /* Shift the samples to the front of the queue. TODO: this - * might not be the most optimal implementation (but it is the - * most simple). */ - TRACE(15,"Shift samples back in buffer"); - uVARTRACE(15,size_before); - dmat tmp = dmat_alloc(size_before,nchannels); - TRACE(15,"SFSG"); - dmat_copy_rows(&tmp, - &queue, - 0, - *start_row, - size_before); - TRACE(15,"SFSG"); - dmat_copy_rows(&queue, - &tmp, - 0,0,size_before); - - *end_row -= *start_row; - *start_row = 0; - - dmat_free(&tmp); - } - else { - *start_row = 0; - *end_row = 0; - } } /* Now, copy samples */ @@ -100,7 +113,6 @@ int dFifo_push(dFifo* fifo,const dmat* data) { *end_row += added_size; feTRACE(15); - return SUCCESS; } int dFifo_pop(dFifo* fifo,dmat* data,const us keep) { fsTRACE(15); @@ -111,18 +123,13 @@ int dFifo_pop(dFifo* fifo,dmat* data,const us keep) { " be smaller than requested number of samples"); us* start_row = &fifo->start_row; - us* end_row = &fifo->end_row; - us cur_contents = dFifo_size(fifo); + us cur_size = dFifo_size(fifo); us requested = data->n_rows; - us obtained = requested > cur_contents ? cur_contents : requested; + us obtained = requested > cur_size ? cur_size : requested; dbgassert(obtained > keep,"Number of samples to keep should be" " smaller than requested number of samples"); - uVARTRACE(15,requested); - uVARTRACE(15,obtained); - uVARTRACE(15,*start_row); - uVARTRACE(15,*end_row); dmat_copy_rows(data, &fifo->queue, diff --git a/beamforming/c/dfifo.h b/beamforming/c/dfifo.h index dfd893c..f834fcc 100644 --- a/beamforming/c/dfifo.h +++ b/beamforming/c/dfifo.h @@ -21,11 +21,9 @@ typedef struct dFifo_s dFifo; * @return Pointer to fifo queue (no null ptr) */ dFifo* dFifo_create(const us nchannels, - const us max_size); + const us init_size); -#define FIFO_QUEUE_FULL (-1) - /** * Pushes samples into the fifo. * @@ -33,10 +31,8 @@ dFifo* dFifo_create(const us nchannels, * * @param data data to push. Number of columns should be equal to * nchannels. - * - * @return 0 on success, FIFO_QUEUE_FULL when samples do not fit. */ -int dFifo_push(dFifo* fifo,const dmat* data); +void dFifo_push(dFifo* fifo,const dmat* data); /** * Pop samples from the queue diff --git a/beamforming/c/fft.c b/beamforming/c/fft.c index 99478ee..6f402dc 100644 --- a/beamforming/c/fft.c +++ b/beamforming/c/fft.c @@ -18,8 +18,12 @@ typedef struct Fft_s { * result */ } Fft; -Fft* Fft_alloc(const us nfft) { +Fft* Fft_create(const us nfft) { fsTRACE(15); + if(nfft == 0) { + WARN("nfft > 0"); + return NULL; + } Fft* fft = a_malloc(sizeof(Fft)); if(fft==NULL) { diff --git a/beamforming/c/fft.h b/beamforming/c/fft.h index e4073e7..dcb6bb2 100644 --- a/beamforming/c/fft.h +++ b/beamforming/c/fft.h @@ -24,7 +24,7 @@ typedef struct Fft_s Fft; * * @return Pointer to Fft handle, NULL on error */ -Fft* Fft_alloc(const us nfft); +Fft* Fft_create(const us nfft); /** * Returns the nfft for this Fft instance * @@ -62,9 +62,9 @@ void Fft_fft(const Fft* fft,const dmat* timedata,cmat* result); * * @param[in] fft Fft handle. * @param[in] freqdata Frequency domain input data, to be iFft'th. - * @param[out] result: iFft't data, should have size (nfft). + * @param[out] timedata: iFft't data, should have size (nfft). */ -void Fft_ifft_single(const Fft* fft,const vc* freqdata,vd* result); +void Fft_ifft_single(const Fft* fft,const vc* freqdata,vd* timedata); /** * Perform inverse FFT diff --git a/beamforming/c/ps.c b/beamforming/c/ps.c index aee720d..40daaaa 100644 --- a/beamforming/c/ps.c +++ b/beamforming/c/ps.c @@ -32,7 +32,7 @@ PowerSpectra* PowerSpectra_alloc(const us nfft, } /* ALlocate space */ - Fft* fft = Fft_alloc(nfft); + Fft* fft = Fft_create(nfft); if(fft == NULL) { WARN("Fft allocation failed"); return NULL; diff --git a/beamforming/config.pxi.in b/beamforming/config.pxi.in index 40aa728..fa2c3cf 100644 --- a/beamforming/config.pxi.in +++ b/beamforming/config.pxi.in @@ -10,27 +10,37 @@ IF ASCEE_FLOAT == "double": ctypedef double complex c NUMPY_FLOAT_TYPE = np.float64 NUMPY_COMPLEX_TYPE = np.complex128 + CYTHON_NUMPY_FLOAT_t = np.NPY_FLOAT64 + CYTHON_NUMPY_COMPLEX_t = np.NPY_COMPLEX128 + ELSE: ctypedef float d ctypedef float complex c NUMPY_FLOAT_TYPE = np.float32 - NUMPY_COMPLEX_TYPE = np.complex128 + NUMPY_COMPLEX_TYPE = np.complex64 + CYTHON_NUMPY_FLOAT_t = np.NPY_FLOAT32 + CYTHON_NUMPY_COMPLEX_t = np.NPY_COMPLEX64 ctypedef size_t us cdef extern from "ascee_tracer.h": void setTracerLevel(int) + void TRACE(int,const char*) + void fsTRACE(int) + void feTRACE(int) cdef extern from "ascee_math.h": ctypedef struct dmat: - d* data - d** col_ptrs - us n_cols,n_rows + us n_cols + us n_rows + d* _data + bint _foreign_data ctypedef struct cmat: - c* data - c** col_ptrs - us n_cols,n_rows - + pass + ctypedef struct vd: + pass + ctypedef struct vc: + pass dmat dmat_foreign(us n_rows, us n_cols, d* data) diff --git a/beamforming/wrappers.pyx b/beamforming/wrappers.pyx index 63e90f6..78e32ba 100644 --- a/beamforming/wrappers.pyx +++ b/beamforming/wrappers.pyx @@ -17,7 +17,7 @@ def cls(): cdef extern from "fft.h": ctypedef struct c_Fft "Fft" - c_Fft* Fft_alloc(us nfft) + c_Fft* Fft_create(us nfft) void Fft_free(c_Fft*) void Fft_fft(c_Fft*,dmat * timedate,cmat * res) nogil void Fft_ifft(c_Fft*,cmat * freqdata,dmat* timedata) nogil @@ -30,7 +30,7 @@ cdef class Fft: c_Fft* _fft def __cinit__(self, us nfft): - self._fft = Fft_alloc(nfft) + self._fft = Fft_create(nfft) if self._fft == NULL: raise RuntimeError('Fft allocation failed') @@ -43,7 +43,7 @@ cdef class Fft: cdef us nfft = Fft_nfft(self._fft) cdef us nchannels = timedata.shape[1] assert timedata.shape[0] ==nfft - + result = np.empty((nfft//2+1,nchannels), dtype=NUMPY_COMPLEX_TYPE, order='F') @@ -53,7 +53,7 @@ cdef class Fft: cdef cmat r = cmat_foreign(result.shape[0], result.shape[1], &result_view[0,0]) - + cdef dmat t = dmat_foreign(timedata.shape[0], timedata.shape[1], &timedata[0,0]) @@ -70,14 +70,14 @@ cdef class Fft: cdef us nfft = Fft_nfft(self._fft) cdef us nchannels = freqdata.shape[1] assert freqdata.shape[0] == nfft//2+1 - + # result[:,:] = np.nan+1j*np.nan cdef cmat f = cmat_foreign(freqdata.shape[0], freqdata.shape[1], &freqdata[0,0]) - + timedata = np.empty((nfft,nchannels), dtype=NUMPY_FLOAT_TYPE, order='F') @@ -115,11 +115,11 @@ cdef extern from "ps.h": ctypedef struct c_PowerSpectra "PowerSpectra" c_PowerSpectra* PowerSpectra_alloc(const us nfft, const WindowType wt) - + void PowerSpectra_compute(const c_PowerSpectra* ps, const dmat * timedata, cmat * result) - + void PowerSpectra_free(c_PowerSpectra*) @@ -140,7 +140,7 @@ cdef class PowerSpectra: dmat td cmat result_mat - + td = dmat_foreign(nfft, nchannels, &timedata[0,0]) @@ -155,7 +155,7 @@ cdef class PowerSpectra: result = np.empty((nfft//2+1,nchannels,nchannels), dtype = NUMPY_COMPLEX_TYPE, order='F') - + cdef c[::1,:,:] result_view = result result_mat = cmat_foreign(nfft//2+1, @@ -163,7 +163,7 @@ cdef class PowerSpectra: &result_view[0,0,0]) - + PowerSpectra_compute(self._ps,&td,&result_mat) dmat_free(&td) @@ -186,11 +186,11 @@ cdef extern from "aps.h": cmat* AvPowerSpectra_addTimeData(const c_AvPowerSpectra* ps, const dmat * timedata) - + void AvPowerSpectra_free(c_AvPowerSpectra*) us AvPowerSpectra_getAverages(const c_AvPowerSpectra*); - + cdef class AvPowerSpectra: cdef: c_AvPowerSpectra* aps @@ -215,7 +215,7 @@ cdef class AvPowerSpectra: AvPowerSpectra_free(self.aps) def getAverages(self): return AvPowerSpectra_getAverages(self.aps) - + def addTimeData(self,d[::1,:] timedata): """! Adds time data, returns current result @@ -229,7 +229,7 @@ cdef class AvPowerSpectra: if nsamples < self.nfft: raise RuntimeError('Number of samples should be > nfft') if nchannels != self.nchannels: - raise RuntimeError('Invalid number of channels') + raise RuntimeError('Invalid number of channels') td = dmat_foreign(nsamples, nchannels, diff --git a/test/test_fft.c b/test/test_fft.c index 7ecab9b..e94a482 100644 --- a/test/test_fft.c +++ b/test/test_fft.c @@ -16,7 +16,7 @@ int main() { iVARTRACE(15,getTracerLevel()); - Fft* fft = Fft_alloc(100000); + Fft* fft = Fft_create(100000); /* Fft_fft(fft,NULL,NULL); */