//КИХ фильтр работающий по DEV_PeriodSamples количеству выборок. Коэффициенты предварительно рассчитываются в calcFiltCoef
//errCode 0x3302, 0x3303
int filterInt(int sample, FiltInt *filt)
{
volatile int index=0;
fifo_int_push(&(filt->samples), sample); //signalShiftInt(filt->samples , sample, DEV_PeriodSamples);
if (getErr()!=0)
{
setErr_str("filterInt");
return 0;
}
filt->result =0;
//sample <<=16;
//index = DEV_PeriodSamples;
for (index = 0; index < DEV_PeriodSamples; index++) //TODO: переписать под CMSIS в Keil
//while(index)
{
//filt->result += filt->filtCoef[index] * (int)sample; //filt->samples[DEV_PeriodSamples - 1 - index];
filt->result += filt->filtCoef[index] * fifo_int_oldnumber(&(filt->samples), DEV_PeriodSamples - 1 - index); //filt->samples[DEV_PeriodSamples - 1 - index];
if (getErr()!=0)
{
setErr_str("filterInt");
return 0;
}
//index --;
}
filt->result *= 2;
filt->result /= DEV_PeriodSamples;//Множитель для алгоритма Фурье
filt->result >>=32;
return filt->result;
}
int VFAdaptive::findWorstSphere(Voronoi3D *vor, const MedialTester &mt, const SEBase *eval, const Sphere *filterSphere, float *resultErr, int *numMedial, bool includeCover){
int worstI = -1;
float errSofar = -1;
int numInternal = 0;
// find the worst approximated point
int numVerts = vor->vertices.getSize();
for (int i = 0; i < numVerts; i++){
Voronoi3D::Vertex *v = &vor->vertices.index(i);
if (filterSphere)
if (!filterSphere->overlap(v->s))
continue;
if (v->flag == VOR_FLAG_INTERNAL || (includeCover && v->flag == VOR_FLAG_COVER)){
numInternal++;
float err = getErr(v, eval, &mt, vor);
if (err > errSofar){
errSofar = err;
worstI = i;
}
}
}
if (resultErr)
*resultErr = errSofar;
if (numMedial)
*numMedial = numInternal;
return worstI;
}
double TransformErrFunction::evaluateReprojErrInt()
{
double reprojErr = 0;
for(TransformSet::const_iterator ppTrans = transformSet_->begin(); ppTrans != transformSet_->end(); ppTrans++)
{
int j = ppTrans->first.im1Id();
int k = ppTrans->first.im2Id();
if(j != alignTo_ && k != alignTo_ && (j > minIdToRefine_ || k > minIdToRefine_))
{
IdPair j0(j, alignTo_);
IdPair k0(k, alignTo_);
const Transform * Tj0 = (*transformSet_)[j0]->transform();
const Transform * Tk0 = (*transformSet_)[k0]->transform();
const Transform * Tjk = ppTrans->second->transform();
int inlierCount = ppTrans->second->correspondences().size();
Transform * Tj0_from_jk = Tk0->accumulate(Tjk);
reprojErr += getErr(Tj0, Tj0_from_jk, inlierCount);
delete Tj0_from_jk;
}
/*else if(j != alignTo_ ) //Here ppTrans->first.second == alignTo_. Don't calc reproj err for identity!
{
const Transform * Tj0 = ppTrans->second->transform();
reprojErr += getErr(Tj0);
}*/
}
return reprojErr * scaleCost_; //scaleCost_ makes errors initially about 1
}
void CheckOpenGLError(const char* stmt, const char* fname, int line)
{
GLenum err = glGetError();
if (err != GL_NO_ERROR)
{
printf("OpenGL error %s:\n%s:%i\n%s\n", getErr(err), fname, line, stmt);
abort();
}
}
// Generate the rational approximation x^(pnum/pden)
void AlgRemez::generateApprox()
{
char *fname = "generateApprox()";
Float time = -dclock();
iter = 0;
spread = 1.0e37;
if (approx_type == RATIONAL_APPROX_ZERO_POLE) {
n--;
neq--;
}
initialGuess();
stpini(step);
while (spread > tolerance) { //iterate until convergance
if (iter++%100==0)
VRB.Result(cname,fname,"Iteration %d, spread %e delta %e\n", iter-1,(Float)spread,(Float)delta);
equations();
if (delta < tolerance)
ERR.General(cname, fname,"Delta too small, try increasing precision\n");
search(step);
}
int sign;
Float error = (Float)getErr(mm[0],&sign);
VRB.Result(cname,fname,"Converged at %d iterations, error = %e\n",
iter,error);
//!< Once the approximation has been generated, calculate the roots
if(!root()) ERR.General(cname,fname,"Root finding failed\n");
if (approx_type == RATIONAL_APPROX_ZERO_POLE) {
roots[n] = (bigfloat)0.0;
n++;
neq++;
}
//!< Now find the partial fraction expansions
if (remez_arg->field_type == BOSON) {
getPFE(remez_arg->residue, remez_arg->pole, &(remez_arg->norm));
getIPFE(remez_arg->residue_inv, remez_arg->pole_inv, &(remez_arg->norm_inv));
} else {
getIPFE(remez_arg->residue, remez_arg->pole, &(remez_arg->norm));
getPFE(remez_arg->residue_inv, remez_arg->pole_inv, &(remez_arg->norm_inv));
}
remez_arg->error = error;
time += dclock();
print_time(cname,fname,time);
}
/**
* return: >0 OK, else failed,will close socket.
*/
int doNewClientIn(int sockfd){
struct _client *thisClient = newClient(sockfd);
if(thisClient == 0 ){
close(sockfd);
return getErr();
}
//thisClient->sendCommand(thisClient,CMD_GET_ID,0,0);
clientGetIdFromBoard(thisClient);
return sockfd;
}
//взять последний элемент из fifo
//errCode 0x3302, 0x3303
unsigned int fifo_uint_pull(Fifo_uint *fifo)
{
checkFifoPos(fifo->size, (fifo->head+fifo->size-1) & (fifo->size-1));
if (getErr() !=0)
{
setErr_str("fifo_uint_pull");
return 0;
}
return fifo->buf[(fifo->head+fifo->size-1) & (fifo->size-1)];
}
//взять элемент старше последнего на number
//errCode 0x3302, 0x3303
unsigned int fifo_uint_oldnumber(Fifo_uint *fifo, int num)
{
int pos = (fifo->head-num+fifo->size-1) & (fifo->size-1);
checkFifoPos(fifo->size, pos);
if (getErr() != 0)
{
setErr_str("fifo_uint_oldnumber");
return 0.0;
}
return fifo->buf[pos];
}
//взять последний элемент из fifo
//errCode 0x3302, 0x3303
void fifo_float_pull_(Fifo_float *fifo, float * res)
{
//static float res;
int pos = (fifo->head+fifo->size-1) & (fifo->size-1);
checkFifoPos(fifo->size, pos);
if (getErr() !=0)
{
setErr_str("fifo_float_pull");
//return 0.0;
}
*res = fifo->buf[pos];
}
//поместить элемент в fifo
//errCode 0x3302, 0x3303
void fifo_float_push(Fifo_float * fifo, float value)
{
checkFifoPos(fifo->size, fifo->head);
if (getErr() !=0)
{
setErr_str("fifo_float_push");
return;
}
fifo->buf[fifo->head] = value;
fifo->head++;
fifo->head = fifo->head & (fifo->size - 1);
}
double PDECond::Passo(){
for(int i=1; i<X-1;i++){
for(int j=1; j<Y-1;j++){
if(M == mSOR)
succ[i][j] = SOR(i,j);
else if(M == mGaussSeidel)
succ[i][j] = GaussSeidel(i,j);
else
succ[i][j] = Jacobi(i,j);
}
}
return getErr();//ritorna l'errore la precisione e` decisa dall'utente
}
//взять элемент старше последнего на number
//errCode 0x3302, 0x3303
float fifo_float_oldnumber(Fifo_float *fifo, int num)
{
//(head - num -1)&(size-1) поскольку head может быть меньше чем num
int pos = (fifo->head-num+fifo->size-1) & (fifo->size-1);
//int pos = (fifo->head-num-1) & (fifo->size-1);
checkFifoPos(fifo->size, pos);
if (getErr() != 0)
{
setErr_str("fifo_float_oldnumber");
return 0.0;
}
return fifo->buf[pos];
}
//КИХ фильтр работающий по DEV_PeriodSamples количеству выборок. Коэффициенты предварительно рассчитываются в calcFiltCoef
//errCode 0x3302, 0x3303
float filter(int sample, Filt *filt)
{
int index=0;
fifo_int_push(&(filt->samples), sample); //signalShiftInt(filt->samples , sample, DEV_PeriodSamples);
if (getErr()!=0)
{
setErr_str("filter");
return 0.0;
}
filt->result =0;
for (index = 0; index < DEV_PeriodSamples; index++) //TODO: переписать под CMSIS в Keil
{
filt->result += filt->filtCoef[index] * fifo_int_oldnumber(&(filt->samples), DEV_PeriodSamples - 1 - index); //filt->samples[DEV_PeriodSamples - 1 - index];
if (getErr()!=0)
{
setErr_str("filter");
return 0.0;
}
}
filt->result *= 2.0 / DEV_PeriodSamples;//Множитель для алгоритма Фурье
return filt->result;
}
//Заполняет fifo нулями и устанавливает его начальное положение
//errCode 0x3301
void fifo_initialize_float(Fifo_float *fifo)
{
int index;
checkFifoSize(fifo->size);
if (getErr() !=0)
{
setErr_str("fifo_initialize_float");
return;
}
for (index = 0; index < fifo->size; index++)
{
fifo->buf[index] = 0.0f;
}
fifo->head=0;
}
//Инциализирует структуру для расчета частоты
//errCode 0x3301
FreqF FreqFInit()
{
FreqF fr;
fr.dTdtbuf.buf = dTdtbuf;
fr.dTdtbuf.size = BUFSIZE_LO;
fifo_initialize_float(&fr.dTdtbuf);
fr.freqBuf.buf = freqBuf;
fr.freqBuf.size = BUFSIZE_LO;
fifo_initialize_float(&fr.freqBuf);
fr.periodBuf.buf = periodBuf;
fr.periodBuf.size = BUFSIZE_LO;
fifo_initialize_float(&fr.periodBuf);
fr.sampleBufer.buf = sampleBufer;
fr.sampleBufer.size = BUFSIZE_HI;
fifo_initialize_float(&fr.sampleBufer);
fr.timeBuf.buf = timeBuf;
fr.timeBuf.size = BUFSIZE_LO;
fifo_initialize_float(&fr.timeBuf);
fr.filter.samples.buf = filtSamplesBuffer;
fr.filter.samples.size = BUFSIZE_HI;
fifo_initialize_int(&fr.filter.samples);
fr.dfdt=0;
fr.frequency=DEV_WorkFreq;
fr.campleCounter=0;
fr.dTdt=0;
fr.T=(DEV_SampleFreq)/(DEV_WorkFreq);
fr.sampleFreq = DEV_SampleFreq;
fr.nomFreq = DEV_WorkFreq;
FiltInitDSP(&fr.filter);
//FiltInit(&fr.filter);
fr.isCalc = false;
if (getErr() != 0)
setErr_str("FreqFInit");
return fr;
}
int DevSPI::send( const uint8_t *ds, int ns )
{
if( ds == nullptr || ns < 1 ) {
return 0;
}
nss_pre_cond();
last_rc = HAL_SPI_Transmit( spi, (uint8_t*)ds, ns, maxWait );
nss_post_cond();
if ( last_rc != HAL_OK ) {
last_err = getErr();
return 0;
}
return ns;
}
int DevSPI::recv( uint8_t *dd, int nd )
{
if( dd == nullptr || nd < 1 ) {
return 0;
}
nss_pre_cond();
last_rc = HAL_SPI_Receive( spi, (uint8_t*)(dd), nd, maxWait );
nss_post_cond();
if ( last_rc != HAL_OK ) {
last_err = getErr();
return 0;
}
return nd;
}
// send 1 bytes more ?
int DevSPI::duplex( const uint8_t *ds, uint8_t *dd, int nd )
{
if( ds == nullptr || dd == nullptr || nd == 0 ) {
return 0;
}
nss_pre_cond();
last_rc = HAL_SPI_TransmitReceive( spi, (uint8_t*)ds, dd, nd, maxWait );
if( last_rc == HAL_OK ) {
last_rc = HAL_SPI_Receive( spi, (uint8_t*)(dd), nd, maxWait );
}
nss_post_cond();
if ( last_rc != HAL_OK ) {
last_err = getErr();
return 0;
}
return nd;
}
int DevSPI::send2( const uint8_t *ds1, int ns1, const uint8_t *ds2, int ns2 )
{
if( ds1 == nullptr || ns1 < 1 || ds2 == nullptr || ns2 < 1 ) {
return 0;
}
nss_pre_cond();
last_rc = HAL_SPI_Transmit( spi, (uint8_t*)ds1, ns1, maxWait );
if( last_rc == HAL_OK ) {
last_rc = HAL_SPI_Transmit( spi, (uint8_t*)(ds2), ns2, maxWait );
}
nss_post_cond();
if ( last_rc != HAL_OK ) {
last_err = getErr();
return 0;
}
return ns1 + ns2;
}
//Расчет скорости изменения чатоты. Используется FLOAT.
//df/dt расчитывается оп формуле df/dt = (f(n) - f(n-4) + f(n-1) - f(n-5)) / (t(n) - t(n-4) + t(n-1) - t(n-5)).
//На выходе = df/dt в Гц/c .
//errCode 0x3302, 0x3303
float calcDfDtF(FreqF* fr)
{
float dfdt, df, dtime=0;
float f0 = 0,f1=0,f4=0,f5=0;
float t0, t1, t4, t5;
f0 = fifo_float_pull(&fr->freqBuf);
f1 = fifo_float_oldnumber(&fr->freqBuf, 1);
f4 = fifo_float_oldnumber(&fr->freqBuf, 4);
f5 = fifo_float_oldnumber(&fr->freqBuf, 5);
//df = fr->freqBuf[19] - fr->freqBuf[15] + fr->freqBuf[18] - fr->freqBuf[14];
df = f0 - f4 + f1 - f5;// fifo_float_pull(&fr->freqBuf) - fifo_float_oldnumber(&fr->freqBuf, 4) + fifo_float_oldnumber(&fr->freqBuf, 1) - fifo_float_oldnumber(&fr->freqBuf, 5);
if (getErr() != 0)
{
setErr_str("calcDfDtF");
return 0.0;
}
if (0 == df)
{
fr->dfdt = 0;
}
else
{
//dtime = fr->timeBuf[19] - fr->timeBuf[15] + fr->timeBuf[18] - fr->timeBuf[14];
t0 = fifo_float_pull(&fr->timeBuf);
t1 = fifo_float_oldnumber(&fr->timeBuf, 1);
t4 = fifo_float_oldnumber(&fr->timeBuf, 4);
t5 = fifo_float_oldnumber(&fr->timeBuf, 5);
//if(fifo_float_pull(&fr->timeBuf) > fifo_float_oldnumber(&fr->timeBuf, 4))
//{
// dtime += fifo_float_pull(&fr->timeBuf) - fifo_float_oldnumber(&fr->timeBuf, 4);
//}
//else//если буфер выборок переполнился между этими двумя переходами
//{
// dtime += (float)0xFFFF - fifo_float_oldnumber(&fr->timeBuf, 4) + fifo_float_pull(&fr->timeBuf);
//}
//if(fifo_float_oldnumber(&fr->timeBuf, 1) > fifo_float_oldnumber(&fr->timeBuf, 5))
//{
// dtime += fifo_float_oldnumber(&fr->timeBuf, 1) - fifo_float_oldnumber(&fr->timeBuf, 5);
//}
//else//если буфер выборок переполнился между этими двумя переходами
//{
// dtime += (float)0xFFFF - fifo_float_oldnumber(&fr->timeBuf, 5) + fifo_float_oldnumber(&fr->timeBuf, 1);
//}
if(t0 > t4)
{
dtime += t0 - t4;
}
else//если буфер выборок переполнился между этими двумя переходами
{
dtime += (float)0xFFFF - t4 + t0;
}
if(t1 > t5)
{
dtime += t1 - t5;
}
else//если буфер выборок переполнился между этими двумя переходами
{
dtime += (float)0xFFFF - t5 + t1;
}
if (getErr() != 0)
{
setErr_str("calcDfDtF");
return 0.0;
}
if (0 != dtime)
{
dfdt = df *DEV_SampleFreq / dtime;
#ifndef MSVC
if (fabs(dfdt)<=15.0f)//если df/dt > 15 Гц/с - велика вероятность, что это периходной процесс (скачек фазы)
#else
if (abs(dfdt)<=15.0f)//если df/dt > 15 Гц/с - велика вероятность, что это периходной процесс (скачек фазы)
#endif
fr->dfdt = dfdt;
}
else
{
setErr_code_str(0x4101, "calcDfDtF");
fr->dfdt = 0.0f;
}
}
return fr->dfdt;
}
void VFAdaptive::adaptiveSample(Voronoi3D *vor, const MedialTester &mt, const SurfaceRep *coverRep, float maxErr, int maxSam, int maxSph, int minSph, const Sphere *filterSphere, const SEBase *eval, bool countOnlyCoverSpheres, int maxLoops){
if (maxErr < 0.0f)
return;
// find the worst approximated point
float errSofar;
findWorstSphere(vor, mt, eval, filterSphere, &errSofar, NULL, false);
OUTPUTINFO("Initial Error Sofar : %f\n", errSofar);
// label the vertices
int numCover = mt.processMedial(vor, coverRep, filterSphere, countOnlyCoverSpheres);
// work out maximum loops alowed
if (maxLoops <= 0){
if (maxSam > 0)
maxLoops = maxSam*3;
else if (maxSph)
maxLoops = maxSph*3;
}
// find the starting error
int loop = 0;
while (true){
loop++;
// find the worst approximated point
int numInt = 0;
float worstD = -1;
int worstI = findWorstSphere(vor, mt, eval, filterSphere, &worstD, &numInt, true);
if (countOnlyCoverSpheres)
numInt = numCover;
if (worstI < 0){
// clear the ban flags and try again
int numVert = vor->vertices.getSize();
for (int i = 0; i < numVert; i++){
Voronoi3D::Vertex *vert = &vor->vertices.index(i);
if (filterSphere && !vert->s.overlap(*filterSphere))
continue;
vor->resetFlag(vert);
}
// update medial/coverage spheres
numCover = mt.processMedial(vor, coverRep, filterSphere);
worstI = findWorstSphere(vor, mt, eval, filterSphere, &worstD, &numInt, true);
if (worstI < 0)
break; // nothing more we can do
}
// enable this to output POV files
//saveSpheres(vor, mt, worstI);
if ((loop %25) ==0)
OUTPUTINFO("NumSpheres : %4d \tErr : %10.6f \terrSofar : %10.6f\n", numInt, worstD, errSofar);
if (errSofar < 0) // first iteration to have some spheres
errSofar = worstD;
// is it worth continuing ??
if (worstD < maxErr && numInt > minSph)
break;
else if ((maxSam > 0 && vor->formingPoints.getSize()-9 >= maxSam*2) ||
(maxSph > 0 && numInt >= maxSph*2) ||
(maxLoops > 0 && loop > maxLoops)){
// normally we would wait until we get back down to the correct error
// but this CAN mean the algorithm will NEVER terminate
// so guard against that happening
// remove BAD cover spheres i.e. those with error > minError sofar
// otherwise this will cause major problems for Merge/Burst
int numVerts = vor->vertices.getSize();
for (int i = 0; i < numVerts; i++){
Voronoi3D::Vertex *v = &vor->vertices.index(i);
if (v->flag == VOR_FLAG_COVER){
float err = getErr(v, eval, &mt, vor);
if (err > errSofar)
v->flag = VOR_FLAG_EXTERNAL;
}
}
break;
}
else if (worstD <= errSofar || worstD <= EPSILON_LARGE){
if (worstD > EPSILON_LARGE)
errSofar = worstD;
else
errSofar = EPSILON_LARGE;
if ((maxSam > 0 && vor->formingPoints.getSize()-9 >= maxSam) ||
(maxSph > 0 && numInt >= maxSph) ||
(maxLoops > 0 && loop > maxLoops))
break;
}
// flag the vertex so that we cannot consider it in the future
// just in case the vertex isn't removed
Voronoi3D::Vertex *v = &vor->vertices.index(worstI);
mt.blockMedial(v);
// get the closest surface point to the vertex
Vector3D n;
Point3D pClose;
mt.closestPointNormal(&pClose, &n, v);
// check if the new point will improve the approximation
if (pClose.distanceSQR(v->s.c) > v->s.r*v->s.r)
OUTPUTINFO("WARNING : the closest point is further away than it should be\n");
// add the point to the Voronoi diagram
vor->insertPoint(pClose, n, worstI);
// save off the results of adding the new sphere
// uncomment this to save POV files
//saveSpheres(vor, mt, -1);
// update medial/coverage spheres
numCover = mt.processMedial(vor, coverRep, filterSphere);
}
}
bool CEgErr::noErr() {
return getErr() == cNoErr;
}
void CEgErr::GetErrStr( UtilStr& outStr ) {
long err;
bool handled = false;
if ( mOSErr ) {
err = mOSErr;
OSErrMsg( outStr ); }
else {
err = getErr();
switch ( err ) {
case cNoErr:
outStr.Append( "No error." );
break;
case cBadPLVersion:
case cBadExamVersion:
outStr.Append( "This file was made with a different version of Examgen or is damaged and cannot be opened." );
break;
case cCorrupted:
outStr.Append( "This file appears to be corrupt." );
break;
case cFileNotFound:
outStr.Append( "File not found." );
break;
case cEOFErr:
outStr.Append( "End of file reached." );
break;
case cEOSErr:
outStr.Append( "End of file/stream reached." );
break;
case cBitmapCorrupted:
outStr.Append( "The bitmap information is corrupt." );
break;
case cBitmapTooDeep:
outStr.Append( "The bitmap must be 256 or less colors." );
break;
case cBitmapNotMono:
outStr.Append( "The bitmap must be monochrome." );
break;
case cBadBitmapType:
outStr.Append( "The file is not a BMP file." );
break;
case cRLENotSupported:
outStr.Append( "Compressed BMPs are not supported." );
break;
default:
outStr.Append( "Internal error." );
break;
}
}
outStr.Append( " (" );
outStr.Append( err );
outStr.Append( ')' );
}
// Generate the rational approximation x^(pnum/pden)
double AlgRemez::generateApprox(int num_degree, int den_degree,
unsigned long pnum, unsigned long pden,
int a_len, double *a_param, int *a_pow)
{
char *fname = "generateApprox(int, unsigned long, unsigned long)";
printf("Degree of the approximation is (%d,%d)\n", num_degree, den_degree);
printf("Approximating the function x^(%d/%d)\n", pnum, pden);
// Reallocate arrays, since degree has changed
if (num_degree != n || den_degree != d) allocate(num_degree,den_degree);
if (a_len > SUM_MAX) {
printf("Error: a_length > SUM_MAX");
exit(0);
}
step = new bigfloat[num_degree+den_degree+2];
a_length = a_len;
for (int j=0; j<a_len; j++) {
a[j]= a_param[j];
a_power[j] = a_pow[j];
}
power_num = pnum;
power_den = pden;
spread = 1.0e37;
iter = 0;
n = num_degree;
d = den_degree;
neq = n + d + 1;
initialGuess();
stpini(step);
while (spread > tolerance) { //iterate until convergance
if (iter++%100==0)
printf("Iteration %d, spread %e delta %e\n",
iter-1,(double)spread,(double)delta);
equations();
if (delta < tolerance) {
printf("Delta too small, try increasing precision\n");
exit(0);
}
search(step);
}
int sign;
double error = (double)getErr(mm[0],&sign);
printf("Converged at %d iterations, error = %e\n",iter,error);
// Once the approximation has been generated, calculate the roots
if(!root()) {
printf("Root finding failed\n");
} else {
foundRoots = 1;
}
delete [] step;
// Return the maximum error in the approximation
return error;
}
// Search for error maxima and minima
void AlgRemez::search(bigfloat *step) {
bigfloat a, q, xm, ym, xn, yn, xx0, xx1;
int i, j, meq, emsign, ensign, steps;
meq = neq + 1;
bigfloat *yy = new bigfloat[meq];
bigfloat eclose = 1.0e30;
bigfloat farther = 0l;
j = 1;
xx0 = apstrt;
for (i = 0; i < meq; i++) {
steps = 0;
xx1 = xx[i]; // Next zero
if (i == meq-1) xx1 = apend;
xm = mm[i];
ym = getErr(xm,&emsign);
q = step[i];
xn = xm + q;
if (xn < xx0 || xn >= xx1) { // Cannot skip over adjacent boundaries
q = -q;
xn = xm;
yn = ym;
ensign = emsign;
} else {
yn = getErr(xn,&ensign);
if (yn < ym) {
q = -q;
xn = xm;
yn = ym;
ensign = emsign;
}
}
while(yn >= ym) { // March until error becomes smaller.
if (++steps > 10) break;
ym = yn;
xm = xn;
emsign = ensign;
a = xm + q;
if (a == xm || a <= xx0 || a >= xx1) break;// Must not skip over the zeros either side.
xn = a;
yn = getErr(xn,&ensign);
}
mm[i] = xm; // Position of maximum
yy[i] = ym; // Value of maximum
if (eclose > ym) eclose = ym;
if (farther < ym) farther = ym;
xx0 = xx1; // Walk to next zero.
} // end of search loop
q = (farther - eclose); // Decrease step size if error spread increased
if (eclose != 0.0) q /= eclose; // Relative error spread
if (q >= spread) delta *= 0.5; // Spread is increasing; decrease step size
spread = q;
for (i = 0; i < neq; i++) {
q = yy[i+1];
if (q != 0.0) q = yy[i] / q - (bigfloat)1l;
else q = 0.0625;
if (q > (bigfloat)0.25) q = 0.25;
q *= mm[i+1] - mm[i];
step[i] = q * delta;
}
step[neq] = step[neq-1];
for (i = 0; i < neq; i++) { // Insert new locations for the zeros.
xm = xx[i] - step[i];
if (xm <= apstrt) continue;
if (xm >= apend) continue;
if (xm <= mm[i]) xm = (bigfloat)0.5 * (mm[i] + xx[i]);
if (xm >= mm[i+1]) xm = (bigfloat)0.5 * (mm[i+1] + xx[i]);
xx[i] = xm;
}
delete [] yy;
}
//Рассчитывает период и частоту сигнала по переходам через ноль. Используется FLOAT. С применением фильтра.
//Расчет периода ведется по среднему расстоянию между переходами через ноль из "-" в "+".
//Для расчета используется среднее время за 3 предыдущих периода. T = (t0-t3)/3.
//Функция вычисляет период сигнала в выборках.
//Вход:
//int sample - значение новой выборки;
//FreqF_intFlt *fr - структура для хранения информации о частоте;
//Выход:
//float res - занчение частоы основной гармоники сигнала в Гц.
//
//Усреднение по трем периодам дает запаздывание результата измерения на 1.5 периода текущей частоты + запаздывание фильтра на 0.5. Например, при наличии df/dt != 0 результатом
//будет частота не в данный момент времени (переход чрез 0), а в момент времени отстающий от текущего на 2 периода назад.
//При f = 50 Гц и df/dt = 5 Гц/с ~ 0.3/0.06, замер частоты будет запаздывать на 0.3/2 + 0.05 = 0.2 Гц, где 0.3 - изменение частоты за 3 периода, 0.06 - примерная длительность 3 периодов.
//errCode 0x3302, 0x3303, 0x3101
float periodCalcF_fltInt(int sample, FreqF_intFlt *fr)
{
float time, time3;
volatile int64_t sampleFlt=0;
float period=0;
float passDt=0;
float dt=0.0;
float frTmp=0;
static int freqBlockCounter;
float f1=0, f2=0, df=0, t1, dt_;
fr->isCalc = false;
filterInt(sample, &fr->filter);
if (getErr() != 0)
{
setErr_str("periodCalcF_flt");
return 0.0f;
}
sampleFlt = fr->filter.result;
fifo_float_push(&fr->sampleBufer, (float)sampleFlt);
if (fifo_float_oldnumber(&fr->sampleBufer, 1) < 0 && (float)sampleFlt >=0 )
{
passDt = nulpassF((float)sample, fifo_float_oldnumber(&fr->sampleBufer, 1));//вычисляем доли выборок при переходе черз 0 и кладем в буфер
if (getErr()!=0)
{
setErr_str("periodCalcF_flt");
return 0.0f;
}
//signalShiftF(fr->timeBuf, fr->campleCounter + passDt);//кладем в буфер время до текущего перехода через ноль
fifo_float_push(&fr->timeBuf, fr->campleCounter + passDt);
time = fifo_float_pull(&fr->timeBuf);
time3 = fifo_float_oldnumber(&fr->timeBuf, 3);
if(time > time3)
{
dt = time - time3;
}
else//если буфер выборок переполнился между этими двумя переходами
{
dt= (float)0xFFFF - time3 + time;
}
if ((float)0.0f == dt)
{
setErr_code_str(0x3101, "periodCalcF_flt");
return 0.0f;
}
period = dt / 3.0f;
//signalShiftF( fr->periodBuf, period);
fifo_float_push(&fr->periodBuf, period);
frTmp = 3.0f * DEV_SampleFreq / dt;
//signalShiftF( fr->freqBuf, fr->frequency);
fifo_float_push(&fr->freqBuf, frTmp);
fr->isCalc = true;//поднимаем флаг появления нового значения частоты
//Оцениваем текущее значение df/dt
f1= fifo_float_pull(&fr->freqBuf);
f2 = fifo_float_oldnumber(&fr->freqBuf,1);
df = (f1 - f2 );
t1= fifo_float_pull(&fr->periodBuf);
//t2 = fifo_float_oldnumber(&fr->periodBuf,1);
dt_ = t1/DEV_SampleFreq;
//если df/dt > 15 Гц/с - велика вероятность, что это переходной процесс (скачек фазы)
#ifndef MSVC
if (fabs(df/dt_) > 15)
#else
if (abs(df/dt_) > 15)
#endif
{
freqBlockCounter=1;
}
if (freqBlockCounter>0)
{
if (freqBlockCounter<4)//При переходном процессе пропускаем 4 периода с моемента его начала. 3 периода расчет частоты + 1 период фильтр
freqBlockCounter++;
else
freqBlockCounter=0;
}
if (0 == freqBlockCounter)
{
if (time3 == 0)//первые 3 периода частоту не считаем.
{
fr->T = DEV_PeriodSamples;
fr->frequency = fr->nomFreq;
freqBlockCounter = 0;
}
else
{
fr->frequency = frTmp;
fr->T = period;
// calcDfDtF(fr);
}
}
}
fr->campleCounter++;
if (fr->campleCounter == 0)
fr->campleCounter = 1;
if (getErr() !=0)
{
setErr_str("periodCalcF_flt");
}
if (fr->T == 0)
{
fr->T = DEV_PeriodSamples;
fr->frequency = fr->nomFreq;
}
return fr->T;
}
