tunealg.c

LV24000的单片机DEMO程序

/************************************************************************
*
*   Copyright(c) 2004 ItoM BV
*   All Rights Reserved.
*
*   LV2400x evaluation kit: Algortihm used by LV2400x
*   File name:	TuneAlg..c
*
*************************************************************************/

#include <stdio.h>
#include <stdlib.h> // for abs
#include "common.h"
#include "Lv24Ekit.h"

#ifdef USE_LV2400x
#include "Lv2400xReg.h"	// for QSS_xxx equations
#endif //USE_LV2400x

#ifdef USE_LV2410x
#include "Lv2410xReg.h"	// for QSS_xxx equations
#endif //USE_LV2410x

/*-------------------------------------------------------------------
        Helpers
-------------------------------------------------------------------*/
BYTE SearchInByteArray(BYTE byToFind, BYTE _rom pbyArray[], BYTE byArraySize)
{
	BYTE i;

	for (i=0; i<byArraySize; i++)
	{
		if (byToFind == pbyArray[i])
			return(i);
	}
	return((BYTE)-1); // Not found
} // End SearchInByteArray

/*-------------------------------------------------------------------
        Algorithm routines
-------------------------------------------------------------------*/
/* ************************************************************************************************
 *
 *  Function:   LinTuneDac
 *
 *  Authors:    Hung van Le
 *  Purpose:    Generic routine to tune all OSC registers of the chip (using lineair interpolating)
 *  Input:      
 *			DWORD dwExpFreq: expected frequency in Hz
 *			DWORD dwMeasureTimeUs: measure time in us
 *			FPTR_WRITE_OSC fpWriteOsc: pointer to Write_xxx_Osc routine
 *			WORD wX1, WORD wX2: start X value - see comment
 *			int iOscRes: Osc resolution - see comment
 *			BYTE bPrecision: need precision if 1
 *  Output:     Status as defined in LvErr.h
 *  Comments:
 *		Algorithm:
 *			x1 = point1, measure f1
 *			x2 = point2;
 *			Loop
 *				write x2
 *				measure f2
 *				xe = interpolate(x1,f1,x2,f2,fexp)
 *				fe = measure
 *				check fe against fexp, exit if OK
 *				Move X2,f2 to X1, f1; xe,fe to X2,f2
 *			goto loop
 *
 *		Input wX1, wX2:
 *			use wX1, wX2 for point 1,2
 *
 * ************************************************************************************************
 * Copyright (c) 2004. Semiconductor Ideas to the Market (ItoM) B.V. All rights reserved.
 * ************************************************************************************************ */
BYTE LinTuneDac(WORD wExpFreq, DWORD dwMeasureTimeUs, FPTR_WRITE_OSC fpWriteOsc, WORD wX1, WORD wX2, BYTE bPrecision)
{
	BYTE  byResult;
	WORD  wY1, wY2;
	WORD  wOscValue;
	int   iStep, iCmpRes;
	BYTE  bDone;

	// Init var.
	bDone = FALSE;
	iStep = 0; // Avoid compiler's complain

	// ----- Draw a line to approach the desired point --------
	// Determine point1
	fpWriteOsc(wX1);				// Set X1
	byResult = CountPulse(dwMeasureTimeUs, &wY1);	// Get Y1
	if (byResult != LVLS_NO_ERROR)
		goto return_LinTuneDac;

	while (!bDone)
	{
		byResult = fpWriteOsc(wX2);			// Set X2
		if (byResult != LVLS_NO_ERROR)
			goto return_LinTuneDac;
		byResult = CountPulse(dwMeasureTimeUs, &wY2);	// Get Y2
		if (byResult != LVLS_NO_ERROR)
			goto return_LinTuneDac;
		
		// Check if we have correct frequency
		if ( (iCmpRes=IsFrequencyOk(wY2, wExpFreq, dwMeasureTimeUs)) == 0 )
		{
			bDone = TRUE;		// frequency within margin
		}
		else
		{
			// Interpolate the 2 points for the OscStep
			iStep = InterpolateOscStep(wExpFreq, wX1, wX2, wY1, wY2);
			wOscValue = wX1 + iStep; // this is the new X2
			// Move X2 to X1 when the X2 is nearer the expected frequency
			//if (dwY1>dwExpFreq)
			if ( (abs((int)wY2-(int)wExpFreq)) < (abs((int)wY1-(int)wExpFreq)) )
			{
				wX1 = wX2;
				wY1 = wY2;
			}

			// Calculate the new X2
			wX2 = wOscValue;
			if ( abs(iStep) < 1 )
			{
				// When the slew of the curve is too small, we can not interpolate anymore (step=0)
				// Prepare to step to the frequency
				// Validate wX2
				if ( fpWriteOsc(wX2) != LVLS_NO_ERROR )	
					wX2= wX1;	// Not OK - use the other point

				// Force stepping if not specified by caller
				if ( !bPrecision )
				{
					iStep = 20;		// Step to approach (will be divided by 2)
					bPrecision = 1;		// Force stepping
				}
				break; 
			}

		}
	}

	// Can not tune osc with interpolating - trying step (until step=1)
	if ( bPrecision && (!bDone) )
	{
#define FTO_TOO_BIG		0x01
#define FTO_TOO_SMALL		0x02
#define FTO_APPROACH_UP		0x04
#define FTO_APPROACH_DOWN	0x08
#define FTO_BOTH_DIR		(FTO_TOO_BIG|FTO_TOO_SMALL)
#define FTO_APPROACH_BOTH	(FTO_APPROACH_UP|FTO_APPROACH_DOWN)
		WORD wFlag;
		BYTE byLoopCnt;
		
		iStep /=2;
		if (iStep==0)
			iStep = 1;
		wFlag=0;
		byLoopCnt = 10;
		while (!bDone)
		{
			byResult = fpWriteOsc(wX2);			// Set X2
			if (byResult != LVLS_NO_ERROR)
				goto return_LinTuneDac;
			byResult = CountPulse(dwMeasureTimeUs, &wY2);	// Get Y2
			if (byResult != LVLS_NO_ERROR)
				goto return_LinTuneDac;
			
			// Check if we have correct frequency
			if ( (iCmpRes=IsFrequencyOk(wY2, wExpFreq, dwMeasureTimeUs)) == 0 )
			{
				bDone = TRUE;		// frequency within margin
			}
			else
			{				
				if (iCmpRes <0)
				{
					wFlag |= FTO_TOO_SMALL; // increase OSC)
					iStep = abs(iStep);
					if (iStep == 1)
						wFlag |= FTO_APPROACH_UP;
				}
				else // iCmdRes > 0
				{
					wFlag |= FTO_TOO_BIG;
					iStep = -abs(iStep);
					if (iStep == -1)
						wFlag |= FTO_APPROACH_DOWN;
				}
			}
			if ( (wFlag & FTO_APPROACH_BOTH)==FTO_APPROACH_BOTH) // we did approach with step=1 for both directions
			{
				bDone = TRUE;
			}
			else if ((wFlag & FTO_BOTH_DIR)==FTO_BOTH_DIR)		// we did approach for both directions
			{
				iStep /= 2;			// Keep approaching with half step
				if (iStep==0)
					iStep = 1;		// sign will be corrected by comparing
			}
			wX2 += iStep;
			byLoopCnt--;
			if ( byLoopCnt==0 )
				break;
		} // EndWhile
	} // EndIf !bDone

return_LinTuneDac:
	//return(MakeError(0, 0, LVLS_TUNE_OSCREG_OP, byResult));
	return(byResult);
} // End LinTuneDac 

WORD CalculateCoeff(WORD wFreq) 
{ 
	// Coefficient calculation: Coeff = Constance/f^2 
	// The contance is fixed at (2^32-1) * 2^9 (for range 65 MHz - 130MHz)
	// The calculation is 
	//		  2^32-1
	//		---------- * 2^9
	//		f in 10 kHz
	//		------------------
	//			f in 10 kHz

#define _C1 0xFFFFFFFF		// 2^32-1
#define _C2_SHIFT 9
#define _C2 (1<<_C2_SHIFT)	// 2^9
	DWORD dwTmp;

	// Can not calculate when freq is 0
	if (wFreq == 0)
		return(0);

	dwTmp = _C1/wFreq;
	//dwTmp *= _C2;
	dwTmp <<= _C2_SHIFT;

	return((WORD)(dwTmp/wFreq));
//	return( ((_C1/wFreq) * _C2)/wFreq );
} // End CalculateCoeff

WORD InterpolateY(WORD wX, WORD wX1, WORD wX2, WORD wY1, WORD wY2)    
{
	INT32 iDeltaX;
	INT32 sA,sB, sC;

	iDeltaX = (int)wX1-(int)wX2;
	if (iDeltaX==0)
		return(0);
//	return( ((((int)dwY1-(int)dwY2)*((int)wX-(int)wX1) +(iDeltaX/2))/iDeltaX) + (int)dwY1 );
	sA = (INT32)wY1 - (INT32)wY2;
	sB = sA * ((int)wX - (int)wX1);
	sB += (iDeltaX/2);
	sC = sB/iDeltaX;
	sC += (INT32)wY1;
	return((WORD)sC);
} // End InterpolateY

WORD InterpolateX(WORD wY, WORD wX1, WORD wX2, WORD wY1, WORD wY2)
{
	INT32 iDeltaY;
	INT32 sA,sB, sC;

	iDeltaY = (INT32)wY1 - (INT32)wY2;
	if (iDeltaY==0)
		return(0);
//	return( (WORD)(((((signed long)dwY-(signed long)dwY1)*((int)wX1-(int)wX2) + (iDeltaY/2))/iDeltaY) + (int)wX1) );
	sA = (INT32)wY - (INT32)wY1;
	sB = sA * ((int)wX1 - (int)wX2);
	sB += (iDeltaY/2);
	sC = sB/iDeltaY;
	sC += (int)wX1;
	return((WORD)sC);