static dcache_entry_t * read_cylinder (uint32 fd, uint32 offset, read_callback_funcp_t read_callback) { uint32 nread; uint8 buf[SECTORS_PER_FLOPPY_CYLINDER * 512], *bufp; dcache_entry_t *retval; uint32 new_offset; new_offset = offset / sizeof buf * sizeof buf; nread = read_callback (fd, buf, new_offset, sizeof buf); if (nread != sizeof buf) retval = NULL; else { for (bufp = buf; bufp + DCACHE_BLOCK_SIZE <= buf + sizeof buf; bufp += DCACHE_BLOCK_SIZE, new_offset += DCACHE_BLOCK_SIZE) { if (!dcache_entry_lookup (fd, new_offset)) { uint32 n_written; n_written = dcache_write (fd, bufp, new_offset, DCACHE_BLOCK_SIZE, NULL); if (n_written != DCACHE_BLOCK_SIZE) warning_unexpected (NULL_STRING); } } retval = dcache_entry_lookup (fd, offset); } return retval; }
void udp_port::on_read(int read_length) { read_callback(buffer,read_length); memset(buffer,0x00,sizeof(buffer)); int b = ReadFile ((HANDLE)sock,&buffer,sizeof(buffer),NULL,&ev); if (!b && GetLastError () != ERROR_IO_PENDING) { printf("ReadFile Failed\n"); throw std::exception("next read failed"); } }
/* close a disk file. */ void file_close(int fd) { fde *F = &fd_table[fd]; PF *read_callback; assert(fd >= 0); assert(F->flags.open); if ((read_callback = F->read_handler)) { F->read_handler = NULL; read_callback(-1, F->read_data); } if (F->flags.write_daemon) { #if defined(_SQUID_WIN32_) || defined(_SQUID_OS2_) /* * on some operating systems, you can not delete or rename * open files, so we won't allow delayed close. */ while (!diskWriteIsComplete(fd)) diskHandleWrite(fd, NULL); #else F->flags.close_request = 1; debug(6, 2) ("file_close: FD %d, delaying close\n", fd); return; #endif } /* * Assert there is no write callback. Otherwise we might be * leaking write state data by closing the descriptor */ assert(F->write_handler == NULL); F->flags.closing = 1; #if CALL_FSYNC_BEFORE_CLOSE fsync(fd); #endif debug(6, F->flags.close_request ? 2 : 5) ("file_close: FD %d, really closing\n", fd); fd_close(fd); close(fd); statCounter.syscalls.disk.closes++; }
/*F/////////////////////////////////////////////////////////////////////////////////////// // Names: icvEigenDecomposite_8u32fR // Purpose: The function calculates all decomposition coefficients for input object // using previously calculated eigen objects basis and the mean (averaged) // object // Context: // Parameters: obj - input object // objStep - its step (in bytes) // nEigObjs - number of eigen objects // eigInput - pointer either to array of pointers to eigen objects // or to read callback function (depending on ioFlags) // eigStep - eigen objects step (in bytes) // ioFlags - input/output flags // iserData - pointer to the structure which contains all necessary // data for the callback function // avg - pointer to averaged object // avgStep - its step (in bytes) // size - ROI size of each source object // coeffs - calculated coefficients (output data) // // Returns: icv status // // Notes: see notes for icvCalcEigenObjects_8u32fR function //F*/ static CvStatus CV_STDCALL icvEigenDecomposite_8u32fR( uchar * obj, int objStep, int nEigObjs, void *eigInput, int eigStep, int ioFlags, void *userData, float *avg, int avgStep, CvSize size, float *coeffs ) { int i; if( nEigObjs < 2 ) return CV_BADFACTOR_ERR; if( ioFlags < 0 || ioFlags > 1 ) return CV_BADFACTOR_ERR; if( size.width > objStep || 4 * size.width > eigStep || 4 * size.width > avgStep || size.height < 1 ) return CV_BADSIZE_ERR; if( obj == NULL || eigInput == NULL || coeffs == NULL || avg == NULL ) return CV_NULLPTR_ERR; if( !ioFlags ) for( i = 0; i < nEigObjs; i++ ) if( ((uchar **) eigInput)[i] == NULL ) return CV_NULLPTR_ERR; if( ioFlags ) /* callback */ { float *buffer; CvCallback read_callback = ((CvInput *) & eigInput)->callback; eigStep = 4 * size.width; /* memory allocation */ buffer = (float *) cvAlloc( sizeof( float ) * size.width * size.height ); if( buffer == NULL ) return CV_OUTOFMEM_ERR; for( i = 0; i < nEigObjs; i++ ) { float w; CvStatus r = (CvStatus)read_callback( i, (void *) buffer, userData ); if( r ) { cvFree( &buffer ); return r; } w = icvCalcDecompCoeff_8u32fR( obj, objStep, buffer, eigStep, avg, avgStep, size ); if( w < -1.0e29f ) { cvFree( &buffer ); return CV_NOTDEFINED_ERR; } coeffs[i] = w; } cvFree( &buffer ); } else /* no callback */ for( i = 0; i < nEigObjs; i++ ) { float w = icvCalcDecompCoeff_8u32fR( obj, objStep, ((float **) eigInput)[i], eigStep, avg, avgStep, size ); if( w < -1.0e29f ) return CV_NOTDEFINED_ERR; coeffs[i] = w; } return CV_NO_ERR; }
/*F/////////////////////////////////////////////////////////////////////////////////////// // Name: icvCalcEigenObjects_8u32fR // Purpose: The function calculates an orthonormal eigen basis and a mean (averaged) // object for a group of input objects (images, vectors, etc.). ROI supported. // Context: // Parameters: nObjects - number of source objects // input - pointer either to array of pointers to input objects // or to read callback function (depending on ioFlags) // imgStep - full width of each source object row in bytes // output - pointer either to array of pointers to output eigen objects // or to write callback function (depending on ioFlags) // eigStep - full width of each eigenobject row in bytes // size - ROI size of each source object // ioFlags - input/output flags (see Notes) // ioBufSize - input/output buffer size // userData - pointer to the structure which contains all necessary // data for the callback functions // calcLimit - determines the calculation finish conditions // avg - pointer to averaged object (has the same size as ROI) // avgStep - full width of averaged object row in bytes // eigVals - pointer to corresponding eigenvalues (array of <nObjects> // elements in descending order) // // Returns: CV_NO_ERR or error code // // Notes: 1. input/output data (that is, input objects and eigen ones) may either // be allocated in the RAM or be read from/written to the HDD (or any // other device) by read/write callback functions. It depends on the // value of ioFlags paramater, which may be the following: // CV_EIGOBJ_NO_CALLBACK, or 0; // CV_EIGOBJ_INPUT_CALLBACK; // CV_EIGOBJ_OUTPUT_CALLBACK; // CV_EIGOBJ_BOTH_CALLBACK, or // CV_EIGOBJ_INPUT_CALLBACK | CV_EIGOBJ_OUTPUT_CALLBACK. // The callback functions as well as the user data structure must be // developed by the user. // // 2. If ioBufSize = 0, or it's too large, the function dermines buffer size // itself. // // 3. Depending on calcLimit parameter, calculations are finished either if // eigenfaces number comes up to certain value or the relation of the // current eigenvalue and the largest one comes down to certain value // (or any of the above conditions takes place). The calcLimit->type value // must be CV_TERMCRIT_NUMB, CV_TERMCRIT_EPS or // CV_TERMCRIT_NUMB | CV_TERMCRIT_EPS. The function returns the real // values calcLimit->max_iter and calcLimit->epsilon. // // 4. eigVals may be equal to NULL (if you don't need eigen values in further). // //F*/ static CvStatus CV_STDCALL icvCalcEigenObjects_8u32fR( int nObjects, void* input, int objStep, void* output, int eigStep, CvSize size, int ioFlags, int ioBufSize, void* userData, CvTermCriteria* calcLimit, float* avg, int avgStep, float *eigVals ) { int i, j, n, iev = 0, m1 = nObjects - 1, objStep1 = objStep, eigStep1 = eigStep / 4; CvSize objSize, eigSize, avgSize; float *c = 0; float *ev = 0; float *bf = 0; uchar *buf = 0; void *buffer = 0; float m = 1.0f / (float) nObjects; CvStatus r; if( m1 > calcLimit->max_iter && calcLimit->type != CV_TERMCRIT_EPS ) m1 = calcLimit->max_iter; /* ---- TEST OF PARAMETERS ---- */ if( nObjects < 2 ) return CV_BADFACTOR_ERR; if( ioFlags < 0 || ioFlags > 3 ) return CV_BADFACTOR_ERR; if( input == NULL || output == NULL || avg == NULL ) return CV_NULLPTR_ERR; if( size.width > objStep || 4 * size.width > eigStep || 4 * size.width > avgStep || size.height < 1 ) return CV_BADSIZE_ERR; if( !(ioFlags & CV_EIGOBJ_INPUT_CALLBACK) ) for( i = 0; i < nObjects; i++ ) if( ((uchar **) input)[i] == NULL ) return CV_NULLPTR_ERR; if( !(ioFlags & CV_EIGOBJ_OUTPUT_CALLBACK) ) for( i = 0; i < m1; i++ ) if( ((float **) output)[i] == NULL ) return CV_NULLPTR_ERR; avgStep /= 4; eigStep /= 4; if( objStep == size.width && eigStep == size.width && avgStep == size.width ) { size.width *= size.height; size.height = 1; objStep = objStep1 = eigStep = eigStep1 = avgStep = size.width; } objSize = eigSize = avgSize = size; if( ioFlags & CV_EIGOBJ_INPUT_CALLBACK ) { objSize.width *= objSize.height; objSize.height = 1; objStep = objSize.width; objStep1 = size.width; } if( ioFlags & CV_EIGOBJ_OUTPUT_CALLBACK ) { eigSize.width *= eigSize.height; eigSize.height = 1; eigStep = eigSize.width; eigStep1 = size.width; } n = objSize.height * objSize.width * (ioFlags & CV_EIGOBJ_INPUT_CALLBACK) + 2 * eigSize.height * eigSize.width * (ioFlags & CV_EIGOBJ_OUTPUT_CALLBACK); /* Buffer size determination */ if( ioFlags ) { ioBufSize = MIN( icvDefaultBufferSize(), n ); } /* memory allocation (if necesseay) */ if( ioFlags & CV_EIGOBJ_INPUT_CALLBACK ) { buf = (uchar *) cvAlloc( sizeof( uchar ) * objSize.width ); if( buf == NULL ) return CV_OUTOFMEM_ERR; } if( ioFlags ) { buffer = (void *) cvAlloc( ioBufSize ); if( buffer == NULL ) { if( buf ) cvFree( &buf ); return CV_OUTOFMEM_ERR; } } /* Calculation of averaged object */ bf = avg; for( i = 0; i < avgSize.height; i++, bf += avgStep ) for( j = 0; j < avgSize.width; j++ ) bf[j] = 0.f; for( i = 0; i < nObjects; i++ ) { int k, l; uchar *bu = (ioFlags & CV_EIGOBJ_INPUT_CALLBACK) ? buf : ((uchar **) input)[i]; if( ioFlags & CV_EIGOBJ_INPUT_CALLBACK ) { CvCallback read_callback = ((CvInput *) & input)->callback; r = (CvStatus)read_callback( i, (void *) buf, userData ); if( r ) { if( buffer ) cvFree( &buffer ); if( buf ) cvFree( &buf ); return r; } } bf = avg; for( k = 0; k < avgSize.height; k++, bf += avgStep, bu += objStep1 ) for( l = 0; l < avgSize.width; l++ ) bf[l] += bu[l]; } bf = avg; for( i = 0; i < avgSize.height; i++, bf += avgStep ) for( j = 0; j < avgSize.width; j++ ) bf[j] *= m; /* Calculation of covariance matrix */ c = (float *) cvAlloc( sizeof( float ) * nObjects * nObjects ); if( c == NULL ) { if( buffer ) cvFree( &buffer ); if( buf ) cvFree( &buf ); return CV_OUTOFMEM_ERR; } r = icvCalcCovarMatrixEx_8u32fR( nObjects, input, objStep1, ioFlags, ioBufSize, (uchar *) buffer, userData, avg, 4 * avgStep, size, c ); if( r ) { cvFree( &c ); if( buffer ) cvFree( &buffer ); if( buf ) cvFree( &buf ); return r; } /* Calculation of eigenvalues & eigenvectors */ ev = (float *) cvAlloc( sizeof( float ) * nObjects * nObjects ); if( ev == NULL ) { cvFree( &c ); if( buffer ) cvFree( &buffer ); if( buf ) cvFree( &buf ); return CV_OUTOFMEM_ERR; } if( eigVals == NULL ) { eigVals = (float *) cvAlloc( sizeof( float ) * nObjects ); if( eigVals == NULL ) { cvFree( &c ); cvFree( &ev ); if( buffer ) cvFree( &buffer ); if( buf ) cvFree( &buf ); return CV_OUTOFMEM_ERR; } iev = 1; } r = icvJacobiEigens_32f( c, ev, eigVals, nObjects, 0.0f ); cvFree( &c ); if( r ) { cvFree( &ev ); if( buffer ) cvFree( &buffer ); if( buf ) cvFree( &buf ); if( iev ) cvFree( &eigVals ); return r; } /* Eigen objects number determination */ if( calcLimit->type != CV_TERMCRIT_NUMBER ) { for( i = 0; i < m1; i++ ) if( fabs( eigVals[i] / eigVals[0] ) < calcLimit->epsilon ) break; m1 = calcLimit->max_iter = i; } else m1 = calcLimit->max_iter; calcLimit->epsilon = (float) fabs( eigVals[m1 - 1] / eigVals[0] ); for( i = 0; i < m1; i++ ) eigVals[i] = (float) (1.0 / sqrt( (double)eigVals[i] )); /* ----------------- Calculation of eigenobjects ----------------------- */ if( ioFlags & CV_EIGOBJ_OUTPUT_CALLBACK ) { int nio, ngr, igr; nio = ioBufSize / (4 * eigSize.width); /* number of eigen objects in buffer */ ngr = m1 / nio; /* number of io groups */ if( nObjects % nio ) ngr += 1; for( igr = 0; igr < ngr; igr++ ) { int io, ie, imin = igr * nio, imax = imin + nio; if( imax > m1 ) imax = m1; for(int k = 0; k < eigSize.width * (imax - imin); k++ ) ((float *) buffer)[k] = 0.f; for( io = 0; io < nObjects; io++ ) { uchar *bu = ioFlags & CV_EIGOBJ_INPUT_CALLBACK ? buf : ((uchar **) input)[io]; if( ioFlags & CV_EIGOBJ_INPUT_CALLBACK ) { CvCallback read_callback = ((CvInput *) & input)->callback; r = (CvStatus)read_callback( io, (void *) buf, userData ); if( r ) { cvFree( &ev ); if( iev ) cvFree( &eigVals ); if( buffer ) cvFree( &buffer ); if( buf ) cvFree( &buf ); return r; } } for( ie = imin; ie < imax; ie++ ) { int k, l; uchar *bv = bu; float e = ev[ie * nObjects + io] * eigVals[ie]; float *be = ((float *) buffer) + ((ie - imin) * eigStep); bf = avg; for( k = 0; k < size.height; k++, bv += objStep1, bf += avgStep, be += eigStep1 ) { for( l = 0; l < size.width - 3; l += 4 ) { float f = bf[l]; uchar v = bv[l]; be[l] += e * (v - f); f = bf[l + 1]; v = bv[l + 1]; be[l + 1] += e * (v - f); f = bf[l + 2]; v = bv[l + 2]; be[l + 2] += e * (v - f); f = bf[l + 3]; v = bv[l + 3]; be[l + 3] += e * (v - f); } for( ; l < size.width; l++ ) be[l] += e * (bv[l] - bf[l]); } } } /* io */ for( ie = imin; ie < imax; ie++ ) /* calculated eigen objects writting */ { CvCallback write_callback = ((CvInput *) & output)->callback; float *be = ((float *) buffer) + ((ie - imin) * eigStep); r = (CvStatus)write_callback( ie, (void *) be, userData ); if( r ) { cvFree( &ev ); if( iev ) cvFree( &eigVals ); if( buffer ) cvFree( &buffer ); if( buf ) cvFree( &buf ); return r; } } } /* igr */ } else { int k, p, l; for( i = 0; i < m1; i++ ) /* e.o. annulation */ { float *be = ((float **) output)[i]; for( p = 0; p < eigSize.height; p++, be += eigStep ) for( l = 0; l < eigSize.width; l++ ) be[l] = 0.0f; } for( k = 0; k < nObjects; k++ ) { uchar *bv = (ioFlags & CV_EIGOBJ_INPUT_CALLBACK) ? buf : ((uchar **) input)[k]; if( ioFlags & CV_EIGOBJ_INPUT_CALLBACK ) { CvCallback read_callback = ((CvInput *) & input)->callback; r = (CvStatus)read_callback( k, (void *) buf, userData ); if( r ) { cvFree( &ev ); if( iev ) cvFree( &eigVals ); if( buffer ) cvFree( &buffer ); if( buf ) cvFree( &buf ); return r; } } for( i = 0; i < m1; i++ ) { float v = eigVals[i] * ev[i * nObjects + k]; float *be = ((float **) output)[i]; uchar *bu = bv; bf = avg; for( p = 0; p < size.height; p++, bu += objStep1, bf += avgStep, be += eigStep1 ) { for( l = 0; l < size.width - 3; l += 4 ) { float f = bf[l]; uchar u = bu[l]; be[l] += v * (u - f); f = bf[l + 1]; u = bu[l + 1]; be[l + 1] += v * (u - f); f = bf[l + 2]; u = bu[l + 2]; be[l + 2] += v * (u - f); f = bf[l + 3]; u = bu[l + 3]; be[l + 3] += v * (u - f); } for( ; l < size.width; l++ ) be[l] += v * (bu[l] - bf[l]); } } /* i */ } /* k */ } /* else */ cvFree( &ev ); if( iev ) cvFree( &eigVals ); else for( i = 0; i < m1; i++ ) eigVals[i] = 1.f / (eigVals[i] * eigVals[i]); if( buffer ) cvFree( &buffer ); if( buf ) cvFree( &buf ); return CV_NO_ERR; }
/*F/////////////////////////////////////////////////////////////////////////////////////// // Name: icvCalcCovarMatrixEx_8u32fR // Purpose: The function calculates a covariance matrix for a group of input objects // (images, vectors, etc.). ROI supported. // Context: // Parameters: nObjects - number of source objects // objects - array of pointers to ROIs of the source objects // imgStep - full width of each source object row in bytes // avg - pointer to averaged object // avgStep - full width of averaged object row in bytes // size - ROI size of each source and averaged objects // covarMatrix - covariance matrix (output parameter; must be allocated // before call) // // Returns: CV_NO_ERR or error code // // Notes: //F*/ static CvStatus CV_STDCALL icvCalcCovarMatrixEx_8u32fR( int nObjects, void *input, int objStep1, int ioFlags, int ioBufSize, uchar* buffer, void *userData, float *avg, int avgStep, CvSize size, float *covarMatrix ) { int objStep = objStep1; /* ---- TEST OF PARAMETERS ---- */ if( nObjects < 2 ) return CV_BADFACTOR_ERR; if( ioFlags < 0 || ioFlags > 3 ) return CV_BADFACTOR_ERR; if( ioFlags && ioBufSize < 1024 ) return CV_BADFACTOR_ERR; if( ioFlags && buffer == NULL ) return CV_NULLPTR_ERR; if( input == NULL || avg == NULL || covarMatrix == NULL ) return CV_NULLPTR_ERR; if( size.width > objStep || 4 * size.width > avgStep || size.height < 1 ) return CV_BADSIZE_ERR; avgStep /= 4; if( ioFlags & CV_EIGOBJ_INPUT_CALLBACK ) /* ==== USE INPUT CALLBACK ==== */ { int nio, ngr, igr, n = size.width * size.height, mm = 0; CvCallback read_callback = ((CvInput *) & input)->callback; uchar *buffer2; objStep = n; nio = ioBufSize / n; /* number of objects in buffer */ ngr = nObjects / nio; /* number of io groups */ if( nObjects % nio ) mm = 1; ngr += mm; buffer2 = (uchar *)cvAlloc( sizeof( uchar ) * n ); if( buffer2 == NULL ) return CV_OUTOFMEM_ERR; for( igr = 0; igr < ngr; igr++ ) { int k, l; int io, jo, imin = igr * nio, imax = imin + nio; uchar *bu1 = buffer, *bu2; if( imax > nObjects ) imax = nObjects; /* read igr group */ for( io = imin; io < imax; io++, bu1 += n ) { CvStatus r; r = (CvStatus)read_callback( io, (void *) bu1, userData ); if( r ) return r; } /* diagonal square calc */ bu1 = buffer; for( io = imin; io < imax; io++, bu1 += n ) { bu2 = bu1; for( jo = io; jo < imax; jo++, bu2 += n ) { float w = 0.f; float *fu = avg; int ij = 0; for( k = 0; k < size.height; k++, fu += avgStep ) for( l = 0; l < size.width; l++, ij++ ) { float f = fu[l], u1 = bu1[ij], u2 = bu2[ij]; w += (u1 - f) * (u2 - f); } covarMatrix[io * nObjects + jo] = covarMatrix[jo * nObjects + io] = w; } } /* non-diagonal elements calc */ for( jo = imax; jo < nObjects; jo++ ) { CvStatus r; bu1 = buffer; bu2 = buffer2; /* read jo object */ r = (CvStatus)read_callback( jo, (void *) bu2, userData ); if( r ) return r; for( io = imin; io < imax; io++, bu1 += n ) { float w = 0.f; float *fu = avg; int ij = 0; for( k = 0; k < size.height; k++, fu += avgStep ) { for( l = 0; l < size.width - 3; l += 4, ij += 4 ) { float f = fu[l]; uchar u1 = bu1[ij]; uchar u2 = bu2[ij]; w += (u1 - f) * (u2 - f); f = fu[l + 1]; u1 = bu1[ij + 1]; u2 = bu2[ij + 1]; w += (u1 - f) * (u2 - f); f = fu[l + 2]; u1 = bu1[ij + 2]; u2 = bu2[ij + 2]; w += (u1 - f) * (u2 - f); f = fu[l + 3]; u1 = bu1[ij + 3]; u2 = bu2[ij + 3]; w += (u1 - f) * (u2 - f); } for( ; l < size.width; l++, ij++ ) { float f = fu[l], u1 = bu1[ij], u2 = bu2[ij]; w += (u1 - f) * (u2 - f); } } covarMatrix[io * nObjects + jo] = covarMatrix[jo * nObjects + io] = w; } } } /* igr */ cvFree( &buffer2 ); } /* if() */ else /* ==== NOT USE INPUT CALLBACK ==== */ { int i, j; uchar **objects = (uchar **) (((CvInput *) & input)->data); for( i = 0; i < nObjects; i++ ) { uchar *bu = objects[i]; for( j = i; j < nObjects; j++ ) { int k, l; float w = 0.f; float *a = avg; uchar *bu1 = bu; uchar *bu2 = objects[j]; for( k = 0; k < size.height; k++, bu1 += objStep, bu2 += objStep, a += avgStep ) { for( l = 0; l < size.width - 3; l += 4 ) { float f = a[l]; uchar u1 = bu1[l]; uchar u2 = bu2[l]; w += (u1 - f) * (u2 - f); f = a[l + 1]; u1 = bu1[l + 1]; u2 = bu2[l + 1]; w += (u1 - f) * (u2 - f); f = a[l + 2]; u1 = bu1[l + 2]; u2 = bu2[l + 2]; w += (u1 - f) * (u2 - f); f = a[l + 3]; u1 = bu1[l + 3]; u2 = bu2[l + 3]; w += (u1 - f) * (u2 - f); } for( ; l < size.width; l++ ) { float f = a[l]; uchar u1 = bu1[l]; uchar u2 = bu2[l]; w += (u1 - f) * (u2 - f); } } covarMatrix[i * nObjects + j] = covarMatrix[j * nObjects + i] = w; } } } /* else */ return CV_NO_ERR; }
/*F/////////////////////////////////////////////////////////////////////////////////////// // Names: icvEigenProjection_8u32fR // Purpose: The function calculates object projection to the eigen sub-space (restores // an object) using previously calculated eigen objects basis, mean (averaged) // object and decomposition coefficients of the restored object // Context: // Parameters: nEigObjs - Number of eigen objects // eigens - Array of pointers to eigen objects // eigStep - Eigen objects step (in bytes) // coeffs - Previously calculated decomposition coefficients // avg - Pointer to averaged object // avgStep - Its step (in bytes) // rest - Pointer to restored object // restStep - Its step (in bytes) // size - ROI size of each object // // Returns: CV status // // Notes: //F*/ static CvStatus CV_STDCALL icvEigenProjection_8u32fR( int nEigObjs, void *eigInput, int eigStep, int ioFlags, void *userData, float *coeffs, float *avg, int avgStep, uchar * rest, int restStep, CvSize size ) { int i, j, k; float *buf; float *buffer = NULL; float *b; CvCallback read_callback = ((CvInput *) & eigInput)->callback; if( size.width > avgStep || 4 * size.width > eigStep || size.height < 1 ) return CV_BADSIZE_ERR; if( rest == NULL || eigInput == NULL || avg == NULL || coeffs == NULL ) return CV_NULLPTR_ERR; if( ioFlags < 0 || ioFlags > 1 ) return CV_BADFACTOR_ERR; if( !ioFlags ) for( i = 0; i < nEigObjs; i++ ) if( ((uchar **) eigInput)[i] == NULL ) return CV_NULLPTR_ERR; eigStep /= 4; avgStep /= 4; if( size.width == restStep && size.width == eigStep && size.width == avgStep ) { size.width *= size.height; size.height = 1; restStep = eigStep = avgStep = size.width; } buf = (float *) cvAlloc( sizeof( float ) * size.width * size.height ); if( buf == NULL ) return CV_OUTOFMEM_ERR; b = buf; for( i = 0; i < size.height; i++, avg += avgStep, b += size.width ) for( j = 0; j < size.width; j++ ) b[j] = avg[j]; if( ioFlags ) { buffer = (float *) cvAlloc( sizeof( float ) * size.width * size.height ); if( buffer == NULL ) { cvFree( &buf ); return CV_OUTOFMEM_ERR; } eigStep = size.width; } for( k = 0; k < nEigObjs; k++ ) { float *e = ioFlags ? buffer : ((float **) eigInput)[k]; float c = coeffs[k]; if( ioFlags ) /* read eigen object */ { CvStatus r = (CvStatus)read_callback( k, (void *) buffer, userData ); if( r ) { cvFree( &buf ); cvFree( &buffer ); return r; } } b = buf; for( i = 0; i < size.height; i++, e += eigStep, b += size.width ) { for( j = 0; j < size.width - 3; j += 4 ) { float b0 = c * e[j]; float b1 = c * e[j + 1]; float b2 = c * e[j + 2]; float b3 = c * e[j + 3]; b[j] += b0; b[j + 1] += b1; b[j + 2] += b2; b[j + 3] += b3; } for( ; j < size.width; j++ ) b[j] += c * e[j]; } } b = buf; for( i = 0; i < size.height; i++, avg += avgStep, b += size.width, rest += restStep ) for( j = 0; j < size.width; j++ ) { int w = cvRound( b[j] ); w = !(w & ~255) ? w : w < 0 ? 0 : 255; rest[j] = (uchar) w; } cvFree( &buf ); if( ioFlags ) cvFree( &buffer ); return CV_NO_ERR; }
static THD_FUNCTION(mpu_thread, arg) { (void)arg; chRegSetThreadName("MPU Sampling"); static int16_t raw_accel_gyro_mag_tmp[9]; #if USE_MAGNETOMETER static int mag_cnt = MAG_DIV; #endif static systime_t iteration_timer = 0; static int identical_reads = 0; iteration_timer = chVTGetSystemTime(); for(;;) { if (get_raw_accel_gyro(raw_accel_gyro_mag_tmp)) { int is_identical = 1; for (int i = 0;i < 6;i++) { if (raw_accel_gyro_mag_tmp[i] != raw_accel_gyro_mag_no_offset[i]) { is_identical = 0; break; } } if (is_identical) { identical_reads++; } else { identical_reads = 0; } if (identical_reads >= MAX_IDENTICAL_READS) { failed_reads++; chThdSleepMicroseconds(FAIL_DELAY_US); reset_init_mpu(); iteration_timer = chVTGetSystemTime(); } else { memcpy((uint16_t*)raw_accel_gyro_mag_no_offset, raw_accel_gyro_mag_tmp, sizeof(raw_accel_gyro_mag)); raw_accel_gyro_mag_tmp[3] -= mpu9150_gyro_offsets[0]; raw_accel_gyro_mag_tmp[4] -= mpu9150_gyro_offsets[1]; raw_accel_gyro_mag_tmp[5] -= mpu9150_gyro_offsets[2]; memcpy((uint16_t*)raw_accel_gyro_mag, raw_accel_gyro_mag_tmp, sizeof(raw_accel_gyro_mag)); update_time_diff = chVTGetSystemTime() - last_update_time; last_update_time = chVTGetSystemTime(); if (read_callback) { read_callback(); } #if USE_MAGNETOMETER mag_cnt++; if (mag_cnt >= MAG_DIV) { mag_cnt = 0; mag_updated = 1; int16_t raw_mag_tmp[3]; if (get_raw_mag(raw_mag_tmp)) { memcpy((uint16_t*)raw_accel_gyro_mag_tmp + 6, raw_mag_tmp, sizeof(raw_mag_tmp)); } else { failed_mag_reads++; chThdSleepMicroseconds(FAIL_DELAY_US); reset_init_mpu(); iteration_timer = chVTGetSystemTime(); } } else { mag_updated = 0; } #endif } } else { failed_reads++; chThdSleepMicroseconds(FAIL_DELAY_US); reset_init_mpu(); iteration_timer = chVTGetSystemTime(); } iteration_timer += US2ST(ITERATION_TIME_US); systime_t time_start = chVTGetSystemTime(); if (iteration_timer > time_start) { chThdSleep(iteration_timer - time_start); } else { chThdSleepMicroseconds(MIN_ITERATION_DELAY_US); iteration_timer = chVTGetSystemTime(); } } }
void serial_port::on_read() { read_callback(); start_read(); }