path: root/drivers/misc/mediatek/lens/common/lc898122af/OisCmd.c

//********************************************************************************
//
//		<< LC898122 Evaluation Soft >>
//	    Program Name	: OisCmd.c
//		Design			: Y.Yamada
//		History			: First edition						2009.07.31 Y.Tashita
//********************************************************************************
//**************************
//	Include Header File		
//**************************
#define		OISCMD

//#include	"Main.h"
//#include	"Cmd.h"
#include	"Ois.h"
#include	"OisDef.h"

//**************************
//	Local Function Prottype	
//**************************
void			MesFil( unsigned char ) ;					// Measure Filter Setting
unsigned long	GinMes( unsigned char ) ;					// Measure Result Getting
void			GyrCon( unsigned char ) ;					// Gyro Filter Control
short			GenMes( unsigned short, unsigned char ) ;	// General Measure
void 			StbOnn( void ) ;							// Servo ON Slope mode

void			SetSineWave(   unsigned char , unsigned char );
void			StartSineWave( void );
void			StopSineWave(  void );

void			SetMeasFil(  unsigned char );
void			ClrMeasFil( void );
unsigned char	TstActMov( unsigned char );



//**************************
//	define					
//**************************
#define		MES_XG1			0								// LXG1 Measure Mode
#define		MES_XG2			1								// LXG2 Measure Mode

#define		HALL_ADJ		0
#define		LOOPGAIN		1
#define		THROUGH			2
#define		NOISE			3

// Measure Mode

#ifdef H1COEF_CHANGER
 #ifdef	CORRECT_1DEG
  #define		MAXLMT		0x40600000				// 3.5
  #define		MINLMT		0x400CCCCD				// 2.2
  #define		CHGCOEF		0xBA0D89D9				// 
  #define		MINLMT_MOV	0x00000000				// 0.0
  #define		CHGCOEF_MOV	0xB8A49249
 #else
  #define		MAXLMT		0x40000000				// 2.0
  #define		MINLMT		0x3F8CCCCD				// 1.1
  #define		CHGCOEF		0xBA4C71C7				// 
  #define		MINLMT_MOV	0x00000000				// 0.0
  #define		CHGCOEF_MOV	0xB9700000
 #endif
#endif

//**************************
//	Global Variable			
//**************************
 unsigned short	UsStpSiz	= 0 ;							// Bias Step Size
 unsigned short	UsErrBia, UsErrOfs ;



//**************************
//	Const					
//**************************
// gxzoom Setting Value
#define		ZOOMTBL	16
const unsigned long	ClGyxZom[ ZOOMTBL ]	= {
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000
	} ;

// gyzoom Setting Value
const unsigned long	ClGyyZom[ ZOOMTBL ]	= {
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000,
		0x3F800000
	} ;

// DI Coefficient Setting Value
#define		COEFTBL	7
const unsigned long	ClDiCof[ COEFTBL ]	= {
		DIFIL_S2,		/* 0 */
		DIFIL_S2,		/* 1 */
		DIFIL_S2,		/* 2 */
		DIFIL_S2,		/* 3 */
		DIFIL_S2,		/* 4 */
		DIFIL_S2,		/* 5 */
		DIFIL_S2		/* 6 */
	} ;
	


//********************************************************************************
// Function Name 	: MesFil
// Retun Value		: NON
// Argment Value	: Measure Filter Mode
// Explanation		: Measure Filter Setting Function
// History			: First edition 						2009.07.31  Y.Tashita
//********************************************************************************
void	MesFil( unsigned char	UcMesMod )
{
	if( !UcMesMod ) {								// Hall Bias&Offset Adjust
		// Measure Filter1 Setting
		RamWrite32A( mes1aa, 0x3CA175C0 ) ;		// 0x10F0	LPF150Hz
		RamWrite32A( mes1ab, 0x3CA175C0 ) ;		// 0x10F1
		RamWrite32A( mes1ac, 0x3F75E8C0 ) ;		// 0x10F2
		RamWrite32A( mes1ad, 0x00000000 ) ;		// 0x10F3
		RamWrite32A( mes1ae, 0x00000000 ) ;		// 0x10F4
		RamWrite32A( mes1ba, 0x3F800000 ) ;		// 0x10F5	Through
		RamWrite32A( mes1bb, 0x00000000 ) ;		// 0x10F6
		RamWrite32A( mes1bc, 0x00000000 ) ;		// 0x10F7
		RamWrite32A( mes1bd, 0x00000000 ) ;		// 0x10F8
		RamWrite32A( mes1be, 0x00000000 ) ;		// 0x10F9
		
		// Measure Filter2 Setting
		RamWrite32A( mes2aa, 0x3CA175C0 ) ;		// 0x11F0	LPF150Hz
		RamWrite32A( mes2ab, 0x3CA175C0 ) ;		// 0x11F1
		RamWrite32A( mes2ac, 0x3F75E8C0 ) ;		// 0x11F2
		RamWrite32A( mes2ad, 0x00000000 ) ;		// 0x11F3
		RamWrite32A( mes2ae, 0x00000000 ) ;		// 0x11F4
		RamWrite32A( mes2ba, 0x3F800000 ) ;		// 0x11F5	Through
		RamWrite32A( mes2bb, 0x00000000 ) ;		// 0x11F6
		RamWrite32A( mes2bc, 0x00000000 ) ;		// 0x11F7
		RamWrite32A( mes2bd, 0x00000000 ) ;		// 0x11F8
		RamWrite32A( mes2be, 0x00000000 ) ;		// 0x11F9
		
	} else if( UcMesMod == LOOPGAIN ) {				// Loop Gain Adjust
		// Measure Filter1 Setting
		RamWrite32A( mes1aa, 0x3DF21080 ) ;		// 0x10F0	LPF1000Hz
		RamWrite32A( mes1ab, 0x3DF21080 ) ;		// 0x10F1
		RamWrite32A( mes1ac, 0x3F437BC0 ) ;		// 0x10F2
		RamWrite32A( mes1ad, 0x00000000 ) ;		// 0x10F3
		RamWrite32A( mes1ae, 0x00000000 ) ;		// 0x10F4
		RamWrite32A( mes1ba, 0x3F7EF980 ) ;		// 0x10F5	HPF30Hz
		RamWrite32A( mes1bb, 0xBF7EF980 ) ;		// 0x10F6
		RamWrite32A( mes1bc, 0x3F7DF300 ) ;		// 0x10F7
		RamWrite32A( mes1bd, 0x00000000 ) ;		// 0x10F8
		RamWrite32A( mes1be, 0x00000000 ) ;		// 0x10F9
		
		// Measure Filter2 Setting
		RamWrite32A( mes2aa, 0x3DF21080 ) ;		// 0x11F0	LPF1000Hz
		RamWrite32A( mes2ab, 0x3DF21080 ) ;		// 0x11F1
		RamWrite32A( mes2ac, 0x3F437BC0 ) ;		// 0x11F2
		RamWrite32A( mes2ad, 0x00000000 ) ;		// 0x11F3
		RamWrite32A( mes2ae, 0x00000000 ) ;		// 0x11F4
		RamWrite32A( mes2ba, 0x3F7EF980 ) ;		// 0x11F5	HPF30Hz
		RamWrite32A( mes2bb, 0xBF7EF980 ) ;		// 0x11F6
		RamWrite32A( mes2bc, 0x3F7DF300 ) ;		// 0x11F7
		RamWrite32A( mes2bd, 0x00000000 ) ;		// 0x11F8
		RamWrite32A( mes2be, 0x00000000 ) ;		// 0x11F9
		
	} else if( UcMesMod == THROUGH ) {				// for Through
		// Measure Filter1 Setting
		RamWrite32A( mes1aa, 0x3F800000 ) ;		// 0x10F0	Through
		RamWrite32A( mes1ab, 0x00000000 ) ;		// 0x10F1
		RamWrite32A( mes1ac, 0x00000000 ) ;		// 0x10F2
		RamWrite32A( mes1ad, 0x00000000 ) ;		// 0x10F3
		RamWrite32A( mes1ae, 0x00000000 ) ;		// 0x10F4
		RamWrite32A( mes1ba, 0x3F800000 ) ;		// 0x10F5	Through
		RamWrite32A( mes1bb, 0x00000000 ) ;		// 0x10F6
		RamWrite32A( mes1bc, 0x00000000 ) ;		// 0x10F7
		RamWrite32A( mes1bd, 0x00000000 ) ;		// 0x10F8
		RamWrite32A( mes1be, 0x00000000 ) ;		// 0x10F9
		
		// Measure Filter2 Setting
		RamWrite32A( mes2aa, 0x3F800000 ) ;		// 0x11F0	Through
		RamWrite32A( mes2ab, 0x00000000 ) ;		// 0x11F1
		RamWrite32A( mes2ac, 0x00000000 ) ;		// 0x11F2
		RamWrite32A( mes2ad, 0x00000000 ) ;		// 0x11F3
		RamWrite32A( mes2ae, 0x00000000 ) ;		// 0x11F4
		RamWrite32A( mes2ba, 0x3F800000 ) ;		// 0x11F5	Through
		RamWrite32A( mes2bb, 0x00000000 ) ;		// 0x11F6
		RamWrite32A( mes2bc, 0x00000000 ) ;		// 0x11F7
		RamWrite32A( mes2bd, 0x00000000 ) ;		// 0x11F8
		RamWrite32A( mes2be, 0x00000000 ) ;		// 0x11F9
		
	} else if( UcMesMod == NOISE ) {				// SINE WAVE TEST for NOISE
		// Measure Filter1 Setting
		RamWrite32A( mes1aa, 0x3CA175C0 ) ;		// 0x10F0	LPF150Hz
		RamWrite32A( mes1ab, 0x3CA175C0 ) ;		// 0x10F1
		RamWrite32A( mes1ac, 0x3F75E8C0 ) ;		// 0x10F2
		RamWrite32A( mes1ad, 0x00000000 ) ;		// 0x10F3
		RamWrite32A( mes1ae, 0x00000000 ) ;		// 0x10F4
		RamWrite32A( mes1ba, 0x3CA175C0 ) ;		// 0x10F5	LPF150Hz
		RamWrite32A( mes1bb, 0x3CA175C0 ) ;		// 0x10F6
		RamWrite32A( mes1bc, 0x3F75E8C0 ) ;		// 0x10F7
		RamWrite32A( mes1bd, 0x00000000 ) ;		// 0x10F8
		RamWrite32A( mes1be, 0x00000000 ) ;		// 0x10F9
		
		// Measure Filter2 Setting
		RamWrite32A( mes2aa, 0x3CA175C0 ) ;		// 0x11F0	LPF150Hz
		RamWrite32A( mes2ab, 0x3CA175C0 ) ;		// 0x11F1
		RamWrite32A( mes2ac, 0x3F75E8C0 ) ;		// 0x11F2
		RamWrite32A( mes2ad, 0x00000000 ) ;		// 0x11F3
		RamWrite32A( mes2ae, 0x00000000 ) ;		// 0x11F4
		RamWrite32A( mes2ba, 0x3CA175C0 ) ;		// 0x11F5	LPF150Hz
		RamWrite32A( mes2bb, 0x3CA175C0 ) ;		// 0x11F6
		RamWrite32A( mes2bc, 0x3F75E8C0 ) ;		// 0x11F7
		RamWrite32A( mes2bd, 0x00000000 ) ;		// 0x11F8
		RamWrite32A( mes2be, 0x00000000 ) ;		// 0x11F9
	}
}



//********************************************************************************
// Function Name 	: SrvCon
// Retun Value		: NON
// Argment Value	: X or Y Select, Servo ON/OFF
// Explanation		: Servo ON,OFF Function
// History			: First edition 						2013.01.09 Y.Shigeoka
//********************************************************************************
void	SrvCon( unsigned char	UcDirSel, unsigned char	UcSwcCon )
{
	if( UcSwcCon ) {
		if( !UcDirSel ) {						// X Direction
			RegWriteA( WH_EQSWX , 0x03 ) ;			// 0x0170
			RamWrite32A( sxggf, 0x00000000 ) ;		// 0x10B5
		} else {								// Y Direction
			RegWriteA( WH_EQSWY , 0x03 ) ;			// 0x0171
			RamWrite32A( syggf, 0x00000000 ) ;		// 0x11B5
		}
	} else {
		if( !UcDirSel ) {						// X Direction
			RegWriteA( WH_EQSWX , 0x02 ) ;			// 0x0170
			RamWrite32A( SXLMT, 0x00000000 ) ;		// 0x1477
		} else {								// Y Direction
			RegWriteA( WH_EQSWY , 0x02 ) ;			// 0x0171
			RamWrite32A( SYLMT, 0x00000000 ) ;		// 0x14F7
		}
	}
}






//********************************************************************************
// Function Name 	: RtnCen
// Retun Value		: Command Status
// Argment Value	: Command Parameter
// Explanation		: Return to center Command Function
// History			: First edition 						2013.01.15 Y.Shigeoka
//********************************************************************************
unsigned char	RtnCen( unsigned char	UcCmdPar )
{
	unsigned char	UcCmdSts ;

	UcCmdSts	= EXE_END ;

	GyrCon( OFF ) ;											// Gyro OFF

	if( !UcCmdPar ) {										// X,Y Centering

		StbOnn() ;											// Slope Mode
		
	} else if( UcCmdPar == 0x01 ) {							// X Centering Only

		SrvCon( X_DIR, ON ) ;								// X only Servo ON
		SrvCon( Y_DIR, OFF ) ;
	} else if( UcCmdPar == 0x02 ) {							// Y Centering Only

		SrvCon( X_DIR, OFF ) ;								// Y only Servo ON
		SrvCon( Y_DIR, ON ) ;
	}

	return( UcCmdSts ) ;
}



//********************************************************************************
// Function Name 	: GyrCon
// Retun Value		: NON
// Argment Value	: Gyro Filter ON or OFF
// Explanation		: Gyro Filter Control Function
// History			: First edition 						2013.01.15 Y.Shigeoka
//********************************************************************************
void	GyrCon( unsigned char	UcGyrCon )
{
	// Return HPF Setting
	RegWriteA( WG_SHTON, 0x00 ) ;									// 0x0107
	
	if( UcGyrCon == ON ) {												// Gyro ON

		
#ifdef	GAIN_CONT
		/* Gain3 Register */
//		AutoGainControlSw( ON ) ;											/* Auto Gain Control Mode ON */
#endif
		ClrGyr( 0x000E , CLR_FRAM1 );		// Gyro Delay RAM Clear

		RamWrite32A( sxggf, 0x3F800000 ) ;	// 0x10B5
		RamWrite32A( syggf, 0x3F800000 ) ;	// 0x11B5
		
	} else if( UcGyrCon == SPC ) {										// Gyro ON for LINE

		
#ifdef	GAIN_CONT
		/* Gain3 Register */
//		AutoGainControlSw( ON ) ;											/* Auto Gain Control Mode ON */
#endif

		RamWrite32A( sxggf, 0x3F800000 ) ;	// 0x10B5
		RamWrite32A( syggf, 0x3F800000 ) ;	// 0x11B5
		

	} else {															// Gyro OFF
		
		RamWrite32A( sxggf, 0x00000000 ) ;	// 0x10B5
		RamWrite32A( syggf, 0x00000000 ) ;	// 0x11B5
		

#ifdef	GAIN_CONT
		/* Gain3 Register */
//		AutoGainControlSw( OFF ) ;											/* Auto Gain Control Mode OFF */
#endif
	}
}



//********************************************************************************
// Function Name 	: OisEna
// Retun Value		: NON
// Argment Value	: Command Parameter
// Explanation		: OIS Enable Control Function
// History			: First edition 						2013.01.15 Y.Shigeoka
//********************************************************************************
void	OisEna( void )
{
	// Servo ON
	SrvCon( X_DIR, ON ) ;
	SrvCon( Y_DIR, ON ) ;

	GyrCon( ON ) ;
}

//********************************************************************************
// Function Name 	: OisEnaLin
// Retun Value		: NON
// Argment Value	: Command Parameter
// Explanation		: OIS Enable Control Function for Line adjustment
// History			: First edition 						2013.09.05 Y.Shigeoka
//********************************************************************************
void	OisEnaLin( void )
{
	// Servo ON
	SrvCon( X_DIR, ON ) ;
	SrvCon( Y_DIR, ON ) ;

	GyrCon( SPC ) ;
}



//********************************************************************************
// Function Name 	: TimPro
// Retun Value		: NON
// Argment Value	: NON
// Explanation		: Timer Interrupt Process Function
// History			: First edition 						2013.01.15 Y.Shigeoka
//********************************************************************************
void	TimPro( void )
{
}



//********************************************************************************
// Function Name 	: S2cPro
// Retun Value		: NON
// Argment Value	: NON
// Explanation		: S2 Command Function
// History			: First edition 						2013.01.15 Y.Shigeoka
//********************************************************************************
void	S2cPro( unsigned char uc_mode )
{
	if( uc_mode == 1 )
	{
#ifdef H1COEF_CHANGER
		SetH1cMod( S2MODE ) ;							/* cancel Lvl change */
#endif
		// HPF��Through Setting
		RegWriteA( WG_SHTON, 0x11 ) ;							// 0x0107
		RamWrite32A( gxh1c, DIFIL_S2 );							// 0x1012
		RamWrite32A( gyh1c, DIFIL_S2 );							// 0x1112
	}
	else
	{
		RamWrite32A( gxh1c, UlH1Coefval );							// 0x1012
		RamWrite32A( gyh1c, UlH1Coefval );							// 0x1112
		// HPF��Through Setting
		RegWriteA( WG_SHTON, 0x00 ) ;							// 0x0107

#ifdef H1COEF_CHANGER
		SetH1cMod( UcH1LvlMod ) ;							/* Re-setting */
#endif
	}
	
}


//********************************************************************************
// Function Name 	: GenMes
// Retun Value		: A/D Convert Result
// Argment Value	: Measure Filter Input Signal Ram Address
// Explanation		: General Measure Function
// History			: First edition 						2013.01.10 Y.Shigeoka
//********************************************************************************
short	GenMes( unsigned short	UsRamAdd, unsigned char	UcMesMod )
{
	short	SsMesRlt ;

	RegWriteA( WC_MES1ADD0, (unsigned char)UsRamAdd ) ;							// 0x0194
	RegWriteA( WC_MES1ADD1, (unsigned char)(( UsRamAdd >> 8 ) & 0x0001 ) ) ;	// 0x0195
	RamWrite32A( MSABS1AV, 0x00000000 ) ;				// 0x1041	Clear
	
	if( !UcMesMod ) {
		RegWriteA( WC_MESLOOP1, 0x04 ) ;				// 0x0193
		RegWriteA( WC_MESLOOP0, 0x00 ) ;				// 0x0192	1024 Times Measure
		RamWrite32A( msmean	, 0x3A7FFFF7 );				// 0x1230	1/CmMesLoop[15:0]
	} else {
		RegWriteA( WC_MESLOOP1, 0x00 ) ;				// 0x0193
		RegWriteA( WC_MESLOOP0, 0x01 ) ;				// 0x0192	1 Times Measure
		RamWrite32A( msmean	, 0x3F800000 );				// 0x1230	1/CmMesLoop[15:0]
	}

	RegWriteA( WC_MESABS, 0x00 ) ;						// 0x0198	none ABS
	BsyWit( WC_MESMODE, 0x01 ) ;						// 0x0190	normal Measure

	RamAccFixMod( ON ) ;							// Fix mode
	
	RamReadA( MSABS1AV, ( unsigned short * )&SsMesRlt ) ;	// 0x1041

	RamAccFixMod( OFF ) ;							// Float mode
	
	return( SsMesRlt ) ;
}


//********************************************************************************
// Function Name 	: SetSinWavePara
// Retun Value		: NON
// Argment Value	: NON
// Explanation		: Sine wave Test Function
// History			: First edition 						2013.01.15 Y.Shigeoka
//********************************************************************************
	/********* Parameter Setting *********/
	/* Servo Sampling Clock		=	23.4375kHz						*/
	/* Freq						=	CmSinFreq*Fs/65536/16			*/
	/* 05 00 XX MM 				XX:Freq MM:Sin or Circle */
const unsigned short	CucFreqVal[ 17 ]	= {
		0xFFFF,				//  0:  Stop
		0x002C,				//  1: 0.983477Hz
		0x0059,				//  2: 1.989305Hz
		0x0086,				//  3: 2.995133Hz	
		0x00B2,				//  4: 3.97861Hz
		0x00DF,				//  5: 4.984438Hz
		0x010C,				//  6: 5.990267Hz
		0x0139,				//  7: 6.996095Hz
		0x0165,				//  8: 7.979572Hz
		0x0192,				//  9: 8.9854Hz
		0x01BF,				//  A: 9.991229Hz
		0x01EC,				//  B: 10.99706Hz
		0x0218,				//  C: 11.98053Hz
		0x0245,				//  D: 12.98636Hz
		0x0272,				//  E: 13.99219Hz
		0x029F,				//  F: 14.99802Hz
		0x02CB				// 10: 15.9815Hz
	} ;
	
#define		USE_SINLPF			/* if sin or circle movement is used LPF , this define has to enable */
	
/* �U����sxsin(0x10D5),sysin(0x11D5)�Œ��� */
void	SetSinWavePara( unsigned char UcTableVal ,  unsigned char UcMethodVal )
{
	unsigned short	UsFreqDat ;
	unsigned char	UcEqSwX , UcEqSwY ;

	
	if(UcTableVal > 0x10 )
		UcTableVal = 0x10 ;			/* Limit */
	UsFreqDat = CucFreqVal[ UcTableVal ] ;	
	
	if( UcMethodVal == SINEWAVE) {
		RegWriteA( WC_SINPHSX, 0x00 ) ;					/* 0x0183	*/
		RegWriteA( WC_SINPHSY, 0x00 ) ;					/* 0x0184	*/
	}else if( UcMethodVal == CIRCWAVE ){
		RegWriteA( WC_SINPHSX,	0x00 ) ;				/* 0x0183	*/
		RegWriteA( WC_SINPHSY,	0x20 ) ;				/* 0x0184	*/
	}else{
		RegWriteA( WC_SINPHSX, 0x00 ) ;					/* 0x0183	*/
		RegWriteA( WC_SINPHSY, 0x00 ) ;					/* 0x0184	*/
	}

#ifdef	USE_SINLPF
	if(( UcMethodVal == CIRCWAVE ) || ( UcMethodVal == SINEWAVE )) {
		MesFil( NOISE ) ;			/* LPF */
	}
#endif

	if( UsFreqDat == 0xFFFF )			/* Sine�g���~ */
	{

		RegReadA( WH_EQSWX, &UcEqSwX ) ;				/* 0x0170	*/
		RegReadA( WH_EQSWY, &UcEqSwY ) ;				/* 0x0171	*/
		UcEqSwX &= ~EQSINSW ;
		UcEqSwY &= ~EQSINSW ;
		RegWriteA( WH_EQSWX, UcEqSwX ) ;				/* 0x0170	*/
		RegWriteA( WH_EQSWY, UcEqSwY ) ;				/* 0x0171	*/
		
#ifdef	USE_SINLPF
		if(( UcMethodVal == CIRCWAVE ) || ( UcMethodVal == SINEWAVE ) || ( UcMethodVal == XACTTEST ) || ( UcMethodVal == YACTTEST )) {
			RegWriteA( WC_DPON,     0x00 ) ;			/* 0x0105	Data pass off */
			RegWriteA( WC_DPO1ADD0, 0x00 ) ;			/* 0x01B8	output initial */
			RegWriteA( WC_DPO1ADD1, 0x00 ) ;			/* 0x01B9	output initial */
			RegWriteA( WC_DPO2ADD0, 0x00 ) ;			/* 0x01BA	output initial */
			RegWriteA( WC_DPO2ADD1, 0x00 ) ;			/* 0x01BB	output initial */
			RegWriteA( WC_DPI1ADD0, 0x00 ) ;			/* 0x01B0	input initial */
			RegWriteA( WC_DPI1ADD1, 0x00 ) ;			/* 0x01B1	input initial */
			RegWriteA( WC_DPI2ADD0, 0x00 ) ;			/* 0x01B2	input initial */
			RegWriteA( WC_DPI2ADD1, 0x00 ) ;			/* 0x01B3	input initial */
			
			/* Ram Access */
			RamAccFixMod( ON ) ;							// Fix mode
			
			RamWriteA( SXOFFZ1, UsCntXof ) ;			/* 0x1461	set optical value */
			RamWriteA( SYOFFZ1, UsCntYof ) ;			/* 0x14E1	set optical value */
			
			/* Ram Access */
			RamAccFixMod( OFF ) ;							// Float mode
	
			RegWriteA( WC_MES1ADD0,  0x00 ) ;			/* 0x0194	*/
			RegWriteA( WC_MES1ADD1,  0x00 ) ;			/* 0x0195	*/
			RegWriteA( WC_MES2ADD0,  0x00 ) ;			/* 0x0196	*/
			RegWriteA( WC_MES2ADD1,  0x00 ) ;			/* 0x0197	*/
			
		}
#endif
		RegWriteA( WC_SINON,     0x00 ) ;			/* 0x0180	Sine wave  */
		
	}
	else
	{
		
		RegReadA( WH_EQSWX, &UcEqSwX ) ;				/* 0x0170	*/
		RegReadA( WH_EQSWY, &UcEqSwY ) ;				/* 0x0171	*/
		
		if(( UcMethodVal == CIRCWAVE ) || ( UcMethodVal == SINEWAVE )) {
#ifdef	USE_SINLPF
			RegWriteA( WC_DPI1ADD0,  ( unsigned char )MES1BZ2 ) ;						/* 0x01B0	input Meas-Fil */
			RegWriteA( WC_DPI1ADD1,  ( unsigned char )(( MES1BZ2 >> 8 ) & 0x0001 ) ) ;	/* 0x01B1	input Meas-Fil */
			RegWriteA( WC_DPI2ADD0,  ( unsigned char )MES2BZ2 ) ;						/* 0x01B2	input Meas-Fil */
			RegWriteA( WC_DPI2ADD1,  ( unsigned char )(( MES2BZ2 >> 8 ) & 0x0001 ) ) ;	/* 0x01B3	input Meas-Fil */
			RegWriteA( WC_DPO1ADD0, ( unsigned char )SXOFFZ1 ) ;						/* 0x01B8	output SXOFFZ1 */
			RegWriteA( WC_DPO1ADD1, ( unsigned char )(( SXOFFZ1 >> 8 ) & 0x0001 ) ) ;	/* 0x01B9	output SXOFFZ1 */
			RegWriteA( WC_DPO2ADD0, ( unsigned char )SYOFFZ1 ) ;						/* 0x01BA	output SYOFFZ1 */
			RegWriteA( WC_DPO2ADD1, ( unsigned char )(( SYOFFZ1 >> 8 ) & 0x0001 ) ) ;	/* 0x01BA	output SYOFFZ1 */
			
			RegWriteA( WC_MES1ADD0,  ( unsigned char )SINXZ ) ;							/* 0x0194	*/
			RegWriteA( WC_MES1ADD1,  ( unsigned char )(( SINXZ >> 8 ) & 0x0001 ) ) ;	/* 0x0195	*/
			RegWriteA( WC_MES2ADD0,  ( unsigned char )SINYZ ) ;							/* 0x0196	*/
			RegWriteA( WC_MES2ADD1,  ( unsigned char )(( SINYZ >> 8 ) & 0x0001 ) ) ;	/* 0x0197	*/
			
			RegWriteA( WC_DPON,     0x03 ) ;			/* 0x0105	Data pass[1:0] on */
			
			UcEqSwX &= ~EQSINSW ;
			UcEqSwY &= ~EQSINSW ;
#else
			UcEqSwX |= 0x08 ;
			UcEqSwY |= 0x08 ;
#endif
		} else if(( UcMethodVal == XACTTEST ) || ( UcMethodVal == YACTTEST )) {
			RegWriteA( WC_DPI2ADD0,  ( unsigned char )MES2BZ2 ) ;						/* 0x01B2	input Meas-Fil */
			RegWriteA( WC_DPI2ADD1,  ( unsigned char )(( MES2BZ2 >> 8 ) & 0x0001 ) ) ;	/* 0x01B3	input Meas-Fil */
			if( UcMethodVal == XACTTEST ){
				RegWriteA( WC_DPO2ADD0, ( unsigned char )SXOFFZ1 ) ;						/* 0x01BA	output SXOFFZ1 */
				RegWriteA( WC_DPO2ADD1, ( unsigned char )(( SXOFFZ1 >> 8 ) & 0x0001 ) ) ;	/* 0x01BB	output SXOFFZ1 */
				RegWriteA( WC_MES2ADD0,  ( unsigned char )SINXZ ) ;							/* 0x0196	*/
				RegWriteA( WC_MES2ADD1,  ( unsigned char )(( SINXZ >> 8 ) & 0x0001 ) ) ;	/* 0x0197	*/
			} else {
				RegWriteA( WC_DPO2ADD0, ( unsigned char )SYOFFZ1 ) ;						/* 0x01BA	output SYOFFZ1 */
				RegWriteA( WC_DPO2ADD1, ( unsigned char )(( SYOFFZ1 >> 8 ) & 0x0001 ) ) ;	/* 0x01BB	output SYOFFZ1 */
				RegWriteA( WC_MES2ADD0,  ( unsigned char )SINYZ ) ;							/* 0x0196	*/
				RegWriteA( WC_MES2ADD1,  ( unsigned char )(( SINYZ >> 8 ) & 0x0001 ) ) ;	/* 0x0197	*/
			}
			
			RegWriteA( WC_DPON,     0x02 ) ;			/* 0x0105	Data pass[1] on */
			
			UcEqSwX &= ~EQSINSW ;
			UcEqSwY &= ~EQSINSW ;

		}else{
			if( UcMethodVal == XHALWAVE ){
		    	UcEqSwX = 0x22 ;				/* SW[5] */
//		    	UcEqSwY = 0x03 ;
			}else{
//				UcEqSwX = 0x03 ;
				UcEqSwY = 0x22 ;				/* SW[5] */
			}
		}
		
		RegWriteA( WC_SINFRQ0,	(unsigned char)UsFreqDat ) ;				// 0x0181		Freq L
		RegWriteA( WC_SINFRQ1,	(unsigned char)(UsFreqDat >> 8) ) ;			// 0x0182		Freq H
		RegWriteA( WC_MESSINMODE,     0x00 ) ;			/* 0x0191	Sine 0 cross  */

		RegWriteA( WH_EQSWX, UcEqSwX ) ;				/* 0x0170	*/
		RegWriteA( WH_EQSWY, UcEqSwY ) ;				/* 0x0171	*/

		RegWriteA( WC_SINON,     0x01 ) ;			/* 0x0180	Sine wave  */
		
	}
	
	
}




#ifdef STANDBY_MODE
//********************************************************************************
// Function Name 	: SetStandby
// Retun Value		: NON
// Argment Value	: 0:Standby ON 1:Standby OFF 2:Standby2 ON 3:Standby2 OFF 
//					: 4:Standby3 ON 5:Standby3 OFF
// Explanation		: Set Standby
// History			: First edition 						2013.01.15 Y.Shigeoka
//********************************************************************************
void	SetStandby( unsigned char UcContMode )
{
	unsigned char	UcStbb0 , UcClkon ;
	
	switch(UcContMode)
	{
	case STB1_ON:

		RegWriteA( DRVFCAF	, 0x00 );				// 0x0081	Drv.MODEAF=0,Drv.ENAAF=0,MODE-0
		RegWriteA( STBB0 	, 0x00 );		// 0x0250 	[ STBAFDRV | STBOISDRV | STBOPAAF | STBOPAY ][ STBOPAX | STBDACI | STBDACV | STBADC ]
		RegWriteA( STBB1 	, 0x00 );		// 0x0264 	[ - | - | - | - ][ - | STBAFOP1 | - | STBAFDAC ]
		RegWriteA( PWMA 	, 0x00 );		// 0x0010		PWM Standby
		RegWriteA( PWMAAF 	, 0x00 );		// 0x0090		AF PWM Standby
		RegWriteA( CVA,  0x00 ) ;			/* 0x0020	LINEAR PWM mode standby	*/
		DrvSw( OFF ) ;						/* Driver OFF */
		AfDrvSw( OFF ) ;					/* AF Driver OFF */
#ifdef	MONITOR_OFF
#else
		RegWriteA( PWMMONA, 0x00 ) ;		// 0x0030	Monitor Standby
#endif
//		RegWriteA( DACMONFC, 0x01 ) ;		// 0x0032	DAC Monitor Standby
		SelectGySleep( ON ) ;				/* Gyro Sleep */
		break ;
	case STB1_OFF:
		SelectGySleep( OFF ) ;				/* Gyro Wake Up */
//		RegWriteA( DACMONFC, 0x81 ) ;		// 0x0032	DAC Monitor Active
		RegWriteA( PWMMONA, 0x80 ) ;		/* 0x0030	Monitor Active	*/
		DrvSw( ON ) ;						/* Driver Mode setting */
		AfDrvSw( ON ) ;						/* AF Driver Mode setting */
		RegWriteA( CVA		, 0xC0 );		// 0x0020	Linear PWM mode enable
		RegWriteA( PWMAAF 	, 0x00 );		// 0x0090		AF PWM Standby
		RegWriteA( PWMA		, 0xC0 );		// 0x0010	PWM enable
		RegWriteA( STBB1	, 0x05 ) ;		// 0x0264	[ - | - | - | - ][ - | STBAFOP1 | - | STBAFDAC ]
		RegWriteA( STBB0	, 0xDF );		// 0x0250 	[ STBAFDRV | STBOISDRV | STBOPAAF | STBOPAY ][ STBOPAX | STBDACI | STBDACV | STBADC ]
		break ;
	case STB2_ON:
		RegWriteA( DRVFCAF	, 0x00 );				// 0x0081	Drv.MODEAF=0,Drv.ENAAF=0,MODE-0
		RegWriteA( STBB0 	, 0x00 );		// 0x0250 	[ STBAFDRV | STBOISDRV | STBOPAAF | STBOPAY ][ STBOPAX | STBDACI | STBDACV | STBADC ]
		RegWriteA( STBB1 	, 0x00 );		// 0x0264 	[ - | - | - | - ][ - | STBAFOP1 | - | STBAFDAC ]
		RegWriteA( PWMA 	, 0x00 );		// 0x0010		PWM Standby
		RegWriteA( PWMAAF 	, 0x00 );		// 0x0090		AF PWM Standby
		RegWriteA( CVA,  0x00 ) ;			/* 0x0020	LINEAR PWM mode standby	*/
		DrvSw( OFF ) ;						/* Drvier Block Ena=0 */
		AfDrvSw( OFF ) ;					/* AF Drvier Block Ena=0 */
#ifdef	MONITOR_OFF
#else
		RegWriteA( PWMMONA, 0x00 ) ;		// 0x0030	Monitor Standby
#endif
//		RegWriteA( DACMONFC, 0x01 ) ;		// 0x0032	DAC Monitor Standby
		SelectGySleep( ON ) ;				/* Gyro Sleep */
		RegWriteA( CLKON, 0x00 ) ;			/* 0x020B	Servo & PWM Clock OFF + D-Gyro I/F OFF	*/
		break ;
	case STB2_OFF:
		RegWriteA( CLKON,	0x1F ) ;		// 0x020B	[ - | - | CmOpafClkOn | CmAfpwmClkOn | CMGifClkOn  | CmScmClkOn  | CmSrvClkOn  | CmPwmClkOn  ]
		SelectGySleep( OFF ) ;				/* Gyro Wake Up */
//		RegWriteA( DACMONFC, 0x81 ) ;		// 0x0032	DAC Monitor Active
		RegWriteA( PWMMONA, 0x80 ) ;		/* 0x0030	Monitor Active	*/
		DrvSw( ON ) ;						/* Driver Mode setting */
		AfDrvSw( ON ) ;						/* AF Driver Mode setting */
		RegWriteA( CVA, 	0xC0 );			// 0x0020	Linear PWM mode enable
		RegWriteA( PWMAAF 	, 0x00 );		// 0x0090		AF PWM Standby
		RegWriteA( PWMA	, 	0xC0 );			// 0x0010	PWM enable
		RegWriteA( STBB1	, 0x05 ) ;		// 0x0264	[ - | - | - | - ][ - | STBAFOP1 | - | STBAFDAC ]
		RegWriteA( STBB0	, 0xDF );		// 0x0250 	[ STBAFDRV | STBOISDRV | STBOPAAF | STBOPAY ][ STBOPAX | STBDACI | STBDACV | STBADC ]
		break ;
	case STB3_ON:
		RegWriteA( DRVFCAF	, 0x00 );				// 0x0081	Drv.MODEAF=0,Drv.ENAAF=0,MODE-0
		RegWriteA( STBB0 	, 0x00 );		// 0x0250 	[ STBAFDRV | STBOISDRV | STBOPAAF | STBOPAY ][ STBOPAX | STBDACI | STBDACV | STBADC ]
		RegWriteA( STBB1 	, 0x00 );		// 0x0264 	[ - | - | - | - ][ - | STBAFOP1 | - | STBAFDAC ]
		RegWriteA( PWMA 	, 0x00 );			// 0x0010		PWM Standby
		RegWriteA( PWMAAF 	, 0x00 );		// 0x0090		AF PWM Standby
		RegWriteA( CVA,  0x00 ) ;			/* 0x0020	LINEAR PWM mode standby	*/
		DrvSw( OFF ) ;						/* Drvier Block Ena=0 */
		AfDrvSw( OFF ) ;					/* AF Drvier Block Ena=0 */
#ifdef	MONITOR_OFF
#else
		RegWriteA( PWMMONA, 0x00 ) ;		// 0x0030	Monitor Standby
#endif
//		RegWriteA( DACMONFC, 0x01 ) ;		// 0x0032	DAC Monitor Standby
		SelectGySleep( ON ) ;				/* Gyro Sleep */
		RegWriteA( CLKON, 0x00 ) ;			/* 0x020B	Servo & PWM Clock OFF + D-Gyro I/F OFF	*/
		RegWriteA( I2CSEL, 0x01 ) ;			/* 0x0248	I2C Noise Cancel circuit OFF	*/
		RegWriteA( OSCSTOP, 0x02 ) ;		// 0x0256	Source Clock Input OFF
		break ;
	case STB3_OFF:
		RegWriteA( OSCSTOP, 0x00 ) ;		// 0x0256	Source Clock Input ON
		RegWriteA( I2CSEL, 0x00 ) ;			/* 0x0248	I2C Noise Cancel circuit ON	*/
		RegWriteA( CLKON,	0x1F ) ;		// 0x020B	[ - | - | - | - | CMGifClkOn  | CmScmClkOn  | CmSrvClkOn  | CmPwmClkOn  ]
		SelectGySleep( OFF ) ;				/* Gyro Wake Up */
//		RegWriteA( DACMONFC, 0x81 ) ;		// 0x0032	DAC Monitor Active
		RegWriteA( PWMMONA, 0x80 ) ;		/* 0x0030	Monitor Active	*/
		DrvSw( ON ) ;						/* Driver Mode setting */
		AfDrvSw( ON ) ;						/* AF Driver Mode setting */
		RegWriteA( CVA, 	0xC0 );			// 0x0020	Linear PWM mode enable
		RegWriteA( PWMAAF,	0x00 );			// 0x0090		AF PWM Standby
		RegWriteA( PWMA	, 	0xC0 );			// 0x0010	PWM enable
		RegWriteA( STBB1	, 0x05 ) ;		// 0x0264	[ - | - | - | - ][ - | STBAFOP1 | - | STBAFDAC ]
		RegWriteA( STBB0	, 0xDF );		// 0x0250 	[ STBAFDRV | STBOISDRV | STBOPAAF | STBOPAY ][ STBOPAX | STBDACI | STBDACV | STBADC ]
		break ;
		
	case STB4_ON:
		RegWriteA( DRVFCAF	, 0x00 );				// 0x0081	Drv.MODEAF=0,Drv.ENAAF=0,MODE-0
		RegWriteA( STBB0 	, 0x00 );		// 0x0250 	[ STBAFDRV | STBOISDRV | STBOPAAF | STBOPAY ][ STBOPAX | STBDACI | STBDACV | STBADC ]
		RegWriteA( STBB1 	, 0x00 );		// 0x0264 	[ - | - | - | - ][ - | STBAFOP1 | - | STBAFDAC ]
		RegWriteA( PWMA 	, 0x00 );		// 0x0010		PWM Standby
		RegWriteA( PWMAAF 	, 0x00 );		// 0x0090		AF PWM Standby
		RegWriteA( CVA,  	0x00 ) ;		/* 0x0020	LINEAR PWM mode standby	*/
		DrvSw( OFF ) ;						/* Drvier Block Ena=0 */
		AfDrvSw( OFF ) ;					/* AF Drvier Block Ena=0 */
#ifdef	MONITOR_OFF
#else
		RegWriteA( PWMMONA, 0x00 ) ;		// 0x0030	Monitor Standby
#endif
//		RegWriteA( DACMONFC, 0x01 ) ;		// 0x0032	DAC Monitor Standby
		GyOutSignalCont( ) ;				/* Gyro Continuos mode */
		RegWriteA( CLKON, 0x04 ) ;			/* 0x020B	Servo & PWM Clock OFF + D-Gyro I/F ON	*/
		break ;
	case STB4_OFF:
		RegWriteA( CLKON,	0x1F ) ;		// 0x020B	[ - | - | - | - | CMGifClkOn  | CmScmClkOn  | CmSrvClkOn  | CmPwmClkOn  ]
		SelectGySleep( OFF ) ;				/* Gyro OIS mode */
//		RegWriteA( DACMONFC, 0x81 ) ;		// 0x0032	DAC Monitor Active
		RegWriteA( PWMMONA, 0x80 ) ;		/* 0x0030	Monitor Active	*/
		DrvSw( ON ) ;						/* Driver Mode setting */
		AfDrvSw( ON ) ;						/* AF Driver Mode setting */
		RegWriteA( CVA, 	0xC0 );			// 0x0020	Linear PWM mode enable
		RegWriteA( PWMAAF, 	0x00 );			// 0x0090		AF PWM Standby
		RegWriteA( PWMA	, 	0xC0 );			// 0x0010	PWM enable
		RegWriteA( STBB1	, 0x05 ) ;		// 0x0264	[ - | - | - | - ][ - | STBAFOP1 | - | STBAFDAC ]
		RegWriteA( STBB0	, 0xDF );		// 0x0250 	[ STBAFDRV | STBOISDRV | STBOPAAF | STBOPAY ][ STBOPAX | STBDACI | STBDACV | STBADC ]
		break ;
		
		/************** special mode ************/
	case STB2_OISON:
		RegReadA( STBB0 	, &UcStbb0 );		// 0x0250 	[ STBAFDRV | STBOISDRV | STBOPAAF | STBOPAY ][ STBOPAX | STBDACI | STBDACV | STBADC ]
		UcStbb0 &= 0x80 ;
		RegWriteA( STBB0 	, UcStbb0 );		// 0x0250 	[ STBAFDRV | STBOISDRV | STBOPAAF | STBOPAY ][ STBOPAX | STBDACI | STBDACV | STBADC ]
		RegWriteA( PWMA 	, 0x00 );		// 0x0010		PWM Standby
		RegWriteA( CVA,  0x00 ) ;			/* 0x0020	LINEAR PWM mode standby	*/
		DrvSw( OFF ) ;						/* Drvier Block Ena=0 */
#ifdef	MONITOR_OFF
#else
		RegWriteA( PWMMONA, 0x00 ) ;		// 0x0030	Monitor Standby
#endif
//		RegWriteA( DACMONFC, 0x01 ) ;		// 0x0032	DAC Monitor Standby
		SelectGySleep( ON ) ;				/* Gyro Sleep */
		RegReadA( CLKON, &UcClkon ) ;		/* 0x020B	PWM Clock OFF + D-Gyro I/F OFF	SRVCLK can't OFF */
		UcClkon &= 0x1A ;
		RegWriteA( CLKON, UcClkon ) ;		/* 0x020B	PWM Clock OFF + D-Gyro I/F OFF	SRVCLK can't OFF */
		break ;
	case STB2_OISOFF:
		RegReadA( CLKON, &UcClkon ) ;		/* 0x020B	PWM Clock OFF + D-Gyro I/F ON  */
		UcClkon |= 0x05 ;
		RegWriteA( CLKON,	UcClkon ) ;		// 0x020B	[ - | - | CmOpafClkOn | CmAfpwmClkOn | CMGifClkOn  | CmScmClkOn  | CmSrvClkOn  | CmPwmClkOn  ]
		SelectGySleep( OFF ) ;				/* Gyro Wake Up */
//		RegWriteA( DACMONFC, 0x81 ) ;		// 0x0032	DAC Monitor Active
		RegWriteA( PWMMONA, 0x80 ) ;		/* 0x0030	Monitor Active	*/
		DrvSw( ON ) ;						/* Driver Mode setting */
		RegWriteA( CVA, 	0xC0 );			// 0x0020	Linear PWM mode enable
		RegWriteA( PWMA	, 	0xC0 );			// 0x0010	PWM enable
		RegReadA( STBB0	, &UcStbb0 );		// 0x0250 	[ STBAFDRV | STBOISDRV | STBOPAAF | STBOPAY ][ STBOPAX | STBDACI | STBDACV | STBADC ]
		UcStbb0 |= 0x5F ;
		RegWriteA( STBB0	, UcStbb0 );	// 0x0250 	[ STBAFDRV | STBOISDRV | STBOPAAF | STBOPAY ][ STBOPAX | STBDACI | STBDACV | STBADC ]
		break ;
		
	case STB2_AFON:
		RegWriteA( DRVFCAF	, 0x00 );				// 0x0081	Drv.MODEAF=0,Drv.ENAAF=0,MODE-0
		RegReadA( STBB0 	, &UcStbb0 );		// 0x0250 	[ STBAFDRV | STBOISDRV | STBOPAAF | STBOPAY ][ STBOPAX | STBDACI | STBDACV | STBADC ]
		UcStbb0 &= 0x7F ;
		RegWriteA( STBB0 	, UcStbb0 );		// 0x0250 	[ STBAFDRV | STBOISDRV | STBOPAAF | STBOPAY ][ STBOPAX | STBDACI | STBDACV | STBADC ]
		RegWriteA( STBB1 	, 0x00 );		// 0x0264 	[ - | - | - | - ][ - | STBAFOP1 | - | STBAFDAC ]
		RegWriteA( PWMAAF 	, 0x00 );		// 0x0090		AF PWM Standby
		AfDrvSw( OFF ) ;					/* AF Drvier Block Ena=0 */
#ifdef	MONITOR_OFF
#else
		RegWriteA( PWMMONA, 0x00 ) ;		// 0x0030	Monitor Standby
#endif
		RegReadA( CLKON, &UcClkon ) ;		/* 0x020B	OPAF Clock OFF + AFPWM OFF	SRVCLK can't OFF	*/
		UcClkon &= 0x07 ;
		RegWriteA( CLKON, UcClkon ) ;		/* 0x020B	OPAF Clock OFF + AFPWM OFF	SRVCLK can't OFF	*/
		break ;
	case STB2_AFOFF:
		RegReadA( CLKON, &UcClkon ) ;		/* 0x020B	OPAF Clock ON + AFPWM ON  */
		UcClkon |= 0x18 ;
		RegWriteA( CLKON,	UcClkon ) ;		// 0x020B	[ - | - | CmOpafClkOn | CmAfpwmClkOn | CMGifClkOn  | CmScmClkOn  | CmSrvClkOn  | CmPwmClkOn  ]
		AfDrvSw( ON ) ;						/* AF Driver Mode setting */
		RegWriteA( PWMAAF 	, 0x00 );		// 0x0090		AF PWM Standby
		RegWriteA( STBB1	, 0x05 ) ;		// 0x0264	[ - | - | - | - ][ - | STBAFOP1 | - | STBAFDAC ]
		RegReadA( STBB0	, &UcStbb0 );		// 0x0250 	[ STBAFDRV | STBOISDRV | STBOPAAF | STBOPAY ][ STBOPAX | STBDACI | STBDACV | STBADC ]
		UcStbb0 |= 0x80 ;
		RegWriteA( STBB0	, UcStbb0 );	// 0x0250 	[ STBAFDRV | STBOISDRV | STBOPAAF | STBOPAY ][ STBOPAX | STBDACI | STBDACV | STBADC ]
		break ;
		/************** special mode ************/
	}
}
#endif

//********************************************************************************
// Function Name 	: SetZsp
// Retun Value		: NON
// Argment Value	: Command Parameter
// Explanation		: Set Zoom Step parameter Function
// History			: First edition 						2013.01.15 Y.Shigeoka
//********************************************************************************
void	SetZsp( unsigned char	UcZoomStepDat )
{
	unsigned long	UlGyrZmx, UlGyrZmy, UlGyrZrx, UlGyrZry ;

	
	/* Zoom Step */
	if(UcZoomStepDat > (ZOOMTBL - 1))
		UcZoomStepDat = (ZOOMTBL -1) ;										/* �����ZOOMTBL-1�ɐݒ肷�� */

	if( UcZoomStepDat == 0 )				/* initial setting	*/
	{
		UlGyrZmx	= ClGyxZom[ 0 ] ;		// Same Wide Coefficient
		UlGyrZmy	= ClGyyZom[ 0 ] ;		// Same Wide Coefficient
		/* Initial Rate value = 1 */
	}
	else
	{
		UlGyrZmx	= ClGyxZom[ UcZoomStepDat ] ;
		UlGyrZmy	= ClGyyZom[ UcZoomStepDat ] ;
		
		
	}
	
	// Zoom Value Setting
	RamWrite32A( gxlens, UlGyrZmx ) ;		/* 0x1022 */
	RamWrite32A( gylens, UlGyrZmy ) ;		/* 0x1122 */

	RamRead32A( gxlens, &UlGyrZrx ) ;		/* 0x1022 */
	RamRead32A( gylens, &UlGyrZry ) ;		/* 0x1122 */

	// Zoom Value Setting Error Check
	if( UlGyrZmx != UlGyrZrx ) {
		RamWrite32A( gxlens, UlGyrZmx ) ;		/* 0x1022 */
	}

	if( UlGyrZmy != UlGyrZry ) {
		RamWrite32A( gylens, UlGyrZmy ) ;		/* 0x1122 */
	}

}

//********************************************************************************
// Function Name 	: StbOnn
// Retun Value		: NON
// Argment Value	: NON
// Explanation		: Stabilizer For Servo On Function
// History			: First edition 						2013.01.09 Y.Shigeoka
//********************************************************************************
 
void StbOnn( void )
{
	unsigned char	UcRegValx,UcRegValy;					// Registor value 
	unsigned char	UcRegIni ;
	unsigned char	UcRegIniCnt = 0;
	
	RegReadA( WH_EQSWX , &UcRegValx ) ;			// 0x0170
	RegReadA( WH_EQSWY , &UcRegValy ) ;			// 0x0171
	
	if( (( UcRegValx & 0x01 ) != 0x01 ) && (( UcRegValy & 0x01 ) != 0x01 ))
	{
		
		RegWriteA( WH_SMTSRVON,	0x01 ) ;				// 0x017C		Smooth Servo ON
		
		SrvCon( X_DIR, ON ) ;
		SrvCon( Y_DIR, ON ) ;
		
		UcRegIni = 0x11;
		while( (UcRegIni & 0x77) != 0x66 )
		{
			RegReadA( RH_SMTSRVSTT,	&UcRegIni ) ;		// 0x01F8		Smooth Servo phase read
			
			if( CmdRdChk() !=0 )	break;				// Dead Lock check (responce check)
			if((UcRegIni & 0x77 ) == 0 )	UcRegIniCnt++ ;
			if( UcRegIniCnt > 10 ){
				break ;			// Status Error
			}
			
		}
		RegWriteA( WH_SMTSRVON,	0x00 ) ;				// 0x017C		Smooth Servo OFF
		
	}
	else
	{
		SrvCon( X_DIR, ON ) ;
		SrvCon( Y_DIR, ON ) ;
	}
}

//********************************************************************************
// Function Name 	: StbOnnN
// Retun Value		: NON
// Argment Value	: NON
// Explanation		: Stabilizer For Servo On Function
// History			: First edition 						2013.10.09 Y.Shigeoka
//********************************************************************************
 
void StbOnnN( unsigned char UcStbY , unsigned char UcStbX )
{
	unsigned char	UcRegIni ;
	unsigned char	UcSttMsk = 0 ;
	unsigned char	UcRegIniCnt = 0;
	
	
	RegWriteA( WH_SMTSRVON,	0x01 ) ;				// 0x017C		Smooth Servo ON
	if( UcStbX == ON )	UcSttMsk |= 0x07 ;
	if( UcStbY == ON )	UcSttMsk |= 0x70 ;
	
	SrvCon( X_DIR, UcStbX ) ;
	SrvCon( Y_DIR, UcStbY ) ;
	
	UcRegIni = 0x11;
	while( (UcRegIni & UcSttMsk) != ( 0x66 & UcSttMsk ) )
	{
		RegReadA( RH_SMTSRVSTT,	&UcRegIni ) ;		// 0x01F8		Smooth Servo phase read
		
		if( CmdRdChk() !=0 )	break;				// Dead Lock check (responce check)
		if((UcRegIni & 0x77 ) == 0 )	UcRegIniCnt++ ;
		if( UcRegIniCnt > 10 ){
			break ;			// Status Error
		}
		
	}
	RegWriteA( WH_SMTSRVON,	0x00 ) ;				// 0x017C		Smooth Servo OFF
		
}

//********************************************************************************
// Function Name 	: OptCen
// Retun Value		: NON
// Argment Value	: UcOptMode 0:Set 1:Save&Set
//					: UsOptXval Xaxis offset
//					: UsOptYval Yaxis offset
// Explanation		: Send Optical Center
// History			: First edition 						2013.01.15 Y.Shigeoka
//********************************************************************************
void	OptCen( unsigned char UcOptmode , unsigned short UsOptXval , unsigned short UsOptYval )
{
	RamAccFixMod( ON ) ;							// Fix mode
	
	switch ( UcOptmode ) {
		case VAL_SET :
			RamWriteA( SXOFFZ1   , UsOptXval ) ;		/* 0x1461	Check Hall X optical center */
			RamWriteA( SYOFFZ1   , UsOptYval ) ;		/* 0x14E1	Check Hall Y optical center */
			break ;
		case VAL_FIX :
			UsCntXof = UsOptXval ;
			UsCntYof = UsOptYval ;
			RamWriteA( SXOFFZ1   , UsCntXof ) ;		/* 0x1461	Check Hall X optical center */
			RamWriteA( SYOFFZ1   , UsCntYof ) ;		/* 0x14E1	Check Hall Y optical center */

			break ;
		case VAL_SPC :
			RamReadA( SXOFFZ1   , &UsOptXval ) ;		/* 0x1461	Check Hall X optical center */
			RamReadA( SYOFFZ1   , &UsOptYval ) ;		/* 0x14E1	Check Hall Y optical center */
			UsCntXof = UsOptXval ;
			UsCntYof = UsOptYval ;


			break ;
	}

	RamAccFixMod( OFF ) ;							// Float mode
	
}






//********************************************************************************
// Function Name 	: GyrGan
// Retun Value		: NON
// Argment Value	: UcGygmode 0:Set 1:Save&Set
//					: UlGygXval Xaxis Gain
//					: UlGygYval Yaxis Gain
// Explanation		: Send Gyro Gain
// History			: First edition 						2011.02.09 Y.Shigeoka
//********************************************************************************
void	GyrGan( unsigned char UcGygmode , unsigned long UlGygXval , unsigned long UlGygYval )
{
	switch ( UcGygmode ) {
		case VAL_SET :
			RamWrite32A( gxzoom, UlGygXval ) ;		// 0x1020
			RamWrite32A( gyzoom, UlGygYval ) ;		// 0x1120
			break ;
		case VAL_FIX :
			RamWrite32A( gxzoom, UlGygXval ) ;		// 0x1020
			RamWrite32A( gyzoom, UlGygYval ) ;		// 0x1120

			break ;
		case VAL_SPC :
			RamRead32A( gxzoom, &UlGygXval ) ;		// 0x1020
			RamRead32A( gyzoom, &UlGygYval ) ;		// 0x1120
		
			break ;
	}

}

//********************************************************************************
// Function Name 	: SetPanTiltMode
// Retun Value		: NON
// Argment Value	: NON
// Explanation		: Pan-Tilt Enable/Disable
// History			: First edition 						2013.01.09 Y.Shigeoka
//********************************************************************************
void	SetPanTiltMode( unsigned char UcPnTmod )
{
	switch ( UcPnTmod ) {
		case OFF :
			RegWriteA( WG_PANON, 0x00 ) ;			// 0x0109	X,Y Pan/Tilt Function OFF
			break ;
		case ON :
			RegWriteA( WG_PANON, 0x01 ) ;			// 0x0109	X,Y Pan/Tilt Function ON
//			RegWriteA( WG_PANON, 0x10 ) ;			// 0x0109	X,Y New Pan/Tilt Function ON
			break ;
	}

}


#ifdef GAIN_CONT
//********************************************************************************
// Function Name 	: TriSts
// Retun Value		: Tripod Status
//					: bit0( 1:Y Tripod ON / 0:OFF)
//					: bit4( 1:X Tripod ON / 0:OFF)
//					: bit7( 1:Tripod ENABLE  / 0:DISABLE)
// Argment Value	: NON
// Explanation		: Read Status of Tripod mode Function
// History			: First edition 						2013.02.18 Y.Shigeoka
//********************************************************************************
unsigned char	TriSts( void )
{
	unsigned char UcRsltSts = 0;
	unsigned char UcVal ;

	RegReadA( WG_ADJGANGXATO, &UcVal ) ;	// 0x0129
	if( UcVal & 0x03 ){						// Gain control enable?
		RegReadA( RG_LEVJUGE, &UcVal ) ;	// 0x01F4
		UcRsltSts = UcVal & 0x11 ;		// bit0, bit4 set
		UcRsltSts |= 0x80 ;				// bit7 ON
	}
	return( UcRsltSts ) ;
}
#endif

//********************************************************************************
// Function Name 	: DrvPwmSw
// Retun Value		: Mode Status
//					: bit4( 1:PWM / 0:LinearPwm)
// Argment Value	: NON
// Explanation		: Select Driver mode Function
// History			: First edition 						2013.02.18 Y.Shigeoka
//********************************************************************************
unsigned char	DrvPwmSw( unsigned char UcSelPwmMod )
{

	switch ( UcSelPwmMod ) {
		case Mlnp :
			RegWriteA( DRVFC	, 0xF0 );			// 0x0001	Drv.MODE=1,Drv.BLK=1,MODE2,LCEN
			UcPwmMod = PWMMOD_CVL ;
			break ;
		
		case Mpwm :
			RegWriteA( DRVFC	, 0x00 );			// 0x0001	Drv.MODE=0,Drv.BLK=0,MODE0B
			UcPwmMod = PWMMOD_PWM ;
 			break ;
	}
	
	return( UcSelPwmMod << 4 ) ;
}

//********************************************************************************
// Function Name 	: SetGcf
// Retun Value		: NON
// Argment Value	: Command Parameter
// Explanation		: Set DI filter coefficient Function
// History			: First edition 						2013.03.22 Y.Shigeoka
//********************************************************************************
void	SetGcf( unsigned char	UcSetNum )
{
	
	/* Zoom Step */
	if(UcSetNum > (COEFTBL - 1))
		UcSetNum = (COEFTBL -1) ;			/* �����COEFTBL-1�ɐݒ肷�� */

	UlH1Coefval	= ClDiCof[ UcSetNum ] ;
		
	// Zoom Value Setting
	RamWrite32A( gxh1c, UlH1Coefval ) ;		/* 0x1012 */
	RamWrite32A( gyh1c, UlH1Coefval ) ;		/* 0x1112 */

#ifdef H1COEF_CHANGER
		SetH1cMod( UcSetNum ) ;							/* Re-setting */
#endif

}

#ifdef H1COEF_CHANGER
//********************************************************************************
// Function Name 	: SetH1cMod
// Retun Value		: NON
// Argment Value	: Command Parameter
// Explanation		: Set H1C coefficient Level chang Function
// History			: First edition 						2013.04.18 Y.Shigeoka
//********************************************************************************
void	SetH1cMod( unsigned char	UcSetNum )
{
	
	switch( UcSetNum ){
	case ( ACTMODE ):				// initial 
		IniPtMovMod( OFF ) ;							// Pan/Tilt setting (Still)
		
		/* enable setting */
		/* Zoom Step */
		UlH1Coefval	= ClDiCof[ 0 ] ;
			
		UcH1LvlMod = 0 ;
		
		// Limit value Value Setting
		RamWrite32A( gxlmt6L, MINLMT ) ;		/* 0x102D L-Limit */
		RamWrite32A( gxlmt6H, MAXLMT ) ;		/* 0x102E H-Limit */

		RamWrite32A( gylmt6L, MINLMT ) ;		/* 0x112D L-Limit */
		RamWrite32A( gylmt6H, MAXLMT ) ;		/* 0x112E H-Limit */

		RamWrite32A( gxhc_tmp, 	UlH1Coefval ) ;	/* 0x100E Base Coef */
		RamWrite32A( gxmg, 		CHGCOEF ) ;		/* 0x10AA Change coefficient gain */

		RamWrite32A( gyhc_tmp, 	UlH1Coefval ) ;	/* 0x110E Base Coef */
		RamWrite32A( gymg, 		CHGCOEF ) ;		/* 0x11AA Change coefficient gain */
		
		RegWriteA( WG_HCHR, 0x12 ) ;			// 0x011B	GmHChrOn[1]=1 Sw ON
		break ;
		
	case( S2MODE ):				// cancel lvl change mode 
		RegWriteA( WG_HCHR, 0x10 ) ;			// 0x011B	GmHChrOn[1]=0 Sw OFF
		break ;
		
	case( MOVMODE ):			// Movie mode 
		IniPtMovMod( ON ) ;							// Pan/Tilt setting (Movie)
		
		RamWrite32A( gxlmt6L, MINLMT_MOV ) ;	/* 0x102D L-Limit */
		RamWrite32A( gylmt6L, MINLMT_MOV ) ;	/* 0x112D L-Limit */

		RamWrite32A( gxmg, CHGCOEF_MOV ) ;		/* 0x10AA Change coefficient gain */
		RamWrite32A( gymg, CHGCOEF_MOV ) ;		/* 0x11AA Change coefficient gain */
			
		RamWrite32A( gxhc_tmp, UlH1Coefval ) ;		/* 0x100E Base Coef */
		RamWrite32A( gyhc_tmp, UlH1Coefval ) ;		/* 0x110E Base Coef */
		
		RegWriteA( WG_HCHR, 0x12 ) ;			// 0x011B	GmHChrOn[1]=1 Sw ON
		break ;
		
	default :
		IniPtMovMod( OFF ) ;							// Pan/Tilt setting (Still)
		
		UcH1LvlMod = UcSetNum ;
			
		RamWrite32A( gxlmt6L, MINLMT ) ;		/* 0x102D L-Limit */
		RamWrite32A( gylmt6L, MINLMT ) ;		/* 0x112D L-Limit */
		
		RamWrite32A( gxmg, 	CHGCOEF ) ;			/* 0x10AA Change coefficient gain */
		RamWrite32A( gymg, 	CHGCOEF ) ;			/* 0x11AA Change coefficient gain */
			
		RamWrite32A( gxhc_tmp, UlH1Coefval ) ;		/* 0x100E Base Coef */
		RamWrite32A( gyhc_tmp, UlH1Coefval ) ;		/* 0x110E Base Coef */
		
		RegWriteA( WG_HCHR, 0x12 ) ;			// 0x011B	GmHChrOn[1]=1 Sw ON
		break ;
	}
}
#endif

//********************************************************************************
// Function Name 	: RdFwVr
// Retun Value		: Firmware version
// Argment Value	: NON
// Explanation		: Read Fw Version Function
// History			: First edition 						2013.05.07 Y.Shigeoka
//********************************************************************************
unsigned short	RdFwVr( void )
{
	unsigned short	UsVerVal ;
	
	UsVerVal = (unsigned short)((MDL_VER << 8) | FW_VER ) ;
	return( UsVerVal ) ;
}

#ifdef	ACCEPTANCE
//********************************************************************************
// Function Name 	: RunHea
// Retun Value		: Result
// Argment Value	: NON
// Explanation		: Hall Examination of Acceptance
// History			: First edition 						2014.02.26 Y.Shigeoka
//********************************************************************************
unsigned char	RunHea( void )
{
	unsigned char 	UcRst ;
	
	UcRst = EXE_END ;
	UcRst |= TstActMov( X_DIR) ;
	UcRst |= TstActMov( Y_DIR) ;
	
	return( UcRst ) ;
}
unsigned char	TstActMov( unsigned char UcDirSel )
{
	unsigned char UcRsltSts;
	unsigned short	UsMsppVal ;

	MesFil( NOISE ) ;					// ����p�t�B���^�[��ݒ肷��B

	if ( !UcDirSel ) {
		RamWrite32A( sxsin , ACT_CHK_LVL );												// 0x10D5
		RamWrite32A( sysin , 0x000000 );												// 0x11D5
		SetSinWavePara( 0x05 , XACTTEST ); 
	}else{
		RamWrite32A( sxsin , 0x000000 );												// 0x10D5
		RamWrite32A( sysin , ACT_CHK_LVL );												// 0x11D5
		SetSinWavePara( 0x05 , YACTTEST ); 
	}

	if ( !UcDirSel ) {					// AXIS X
		RegWriteA( WC_MES1ADD0,  ( unsigned char )SXINZ1 ) ;							/* 0x0194	*/
		RegWriteA( WC_MES1ADD1,  ( unsigned char )(( SXINZ1 >> 8 ) & 0x0001 ) ) ;		/* 0x0195	*/
	} else {							// AXIS Y
		RegWriteA( WC_MES1ADD0,  ( unsigned char )SYINZ1 ) ;							/* 0x0194	*/
		RegWriteA( WC_MES1ADD1,  ( unsigned char )(( SYINZ1 >> 8 ) & 0x0001 ) ) ;		/* 0x0195	*/
	}

	RegWriteA( WC_MESSINMODE, 0x00 ) ;			// 0x0191	0 cross
	RegWriteA( WC_MESLOOP1, 0x00 ) ;				// 0x0193
	RegWriteA( WC_MESLOOP0, 0x02 ) ;				// 0x0192	2 Times Measure
	RamWrite32A( msmean	, 0x3F000000 );				// 0x10AE	1/CmMesLoop[15:0]/2
	RegWriteA( WC_MESABS, 0x00 ) ;				// 0x0198	none ABS
	BsyWit( WC_MESMODE, 0x02 ) ;				// 0x0190		Sine wave Measure

	RamAccFixMod( ON ) ;							// Fix mode
	RamReadA( MSPP1AV, &UsMsppVal ) ;
	RamAccFixMod( OFF ) ;							// Float mode

	if ( !UcDirSel ) {					// AXIS X
		SetSinWavePara( 0x00 , XACTTEST ); 	/* STOP */
	}else{
		SetSinWavePara( 0x00 , YACTTEST ); 	/* STOP */
	}

	
	UcRsltSts = EXE_END ;
	if( UsMsppVal > ACT_THR ){
		if ( !UcDirSel ) {					// AXIS X
			UcRsltSts = EXE_HXMVER ;
		}else{								// AXIS Y
			UcRsltSts = EXE_HYMVER ;
		}
	}
	
	return( UcRsltSts ) ;
}


//********************************************************************************
// Function Name 	: RunGea
// Retun Value		: Result
// Argment Value	: NON
// Explanation		: Gyro Examination of Acceptance
// History			: First edition 						2014.02.13 T.Tokoro
//********************************************************************************
unsigned char	RunGea( void )
{
	unsigned char 	UcRst, UcCnt, UcXLowCnt, UcYLowCnt, UcXHigCnt, UcYHigCnt ;
	unsigned short	UsGxoVal[10], UsGyoVal[10], UsDif;
	
	UcRst = EXE_END ;
	UcXLowCnt = UcYLowCnt = UcXHigCnt = UcYHigCnt = 0 ;
	
	MesFil( THROUGH ) ;				// ����p�t�B���^�[��ݒ肷��B
	
	for( UcCnt = 0 ; UcCnt < 10 ; UcCnt++ )
	{
		// X
		RegWriteA( WC_MES1ADD0, 0x00 ) ;		// 0x0194
		RegWriteA( WC_MES1ADD1, 0x00 ) ;		// 0x0195
		ClrGyr( 0x1000 , CLR_FRAM1 );							// Measure Filter RAM Clear
		UsGxoVal[UcCnt] = (unsigned short)GenMes( AD2Z, 0 );	// 64��̕��ϒl����	GYRMON1(0x1110) <- GXADZ(0x144A)
		
		// Y
		RegWriteA( WC_MES1ADD0, 0x00 ) ;		// 0x0194
		RegWriteA( WC_MES1ADD1, 0x00 ) ;		// 0x0195
		ClrGyr( 0x1000 , CLR_FRAM1 );							// Measure Filter RAM Clear
		UsGyoVal[UcCnt] = (unsigned short)GenMes( AD3Z, 0 );	// 64��̕��ϒl����	GYRMON2(0x1111) <- GYADZ(0x14CA)
		
		
		if( UcCnt > 0 )
		{
			if ( (short)UsGxoVal[0] > (short)UsGxoVal[UcCnt] ) {
				UsDif = (unsigned short)((short)UsGxoVal[0] - (short)UsGxoVal[UcCnt]) ;
			} else {
				UsDif = (unsigned short)((short)UsGxoVal[UcCnt] - (short)UsGxoVal[0]) ;
			}
			
			if( UsDif > GEA_DIF_HIG ) {
				//UcRst = UcRst | EXE_GXABOVE ;
				UcXHigCnt ++ ;
			}
			if( UsDif < GEA_DIF_LOW ) {
				//UcRst = UcRst | EXE_GXBELOW ;
				UcXLowCnt ++ ;
			}
			
			if ( (short)UsGyoVal[0] > (short)UsGyoVal[UcCnt] ) {
				UsDif = (unsigned short)((short)UsGyoVal[0] - (short)UsGyoVal[UcCnt]) ;
			} else {
				UsDif = (unsigned short)((short)UsGyoVal[UcCnt] - (short)UsGyoVal[0]) ;
			}
			
			if( UsDif > GEA_DIF_HIG ) {
				//UcRst = UcRst | EXE_GYABOVE ;
				UcYHigCnt ++ ;
			}
			if( UsDif < GEA_DIF_LOW ) {
				//UcRst = UcRst | EXE_GYBELOW ;
				UcYLowCnt ++ ;
			}
		}
	}
	
	if( UcXHigCnt >= 1 ) {
		UcRst = UcRst | EXE_GXABOVE ;
	}
	if( UcXLowCnt > 8 ) {
		UcRst = UcRst | EXE_GXBELOW ;
	}
	
	if( UcYHigCnt >= 1 ) {
		UcRst = UcRst | EXE_GYABOVE ;
	}
	if( UcYLowCnt > 8 ) {
		UcRst = UcRst | EXE_GYBELOW ;
	}
	
	return( UcRst ) ;
}
#endif	//ACCEPTANCE


//********************************************************************************
// Function Name 	: CmdRdChk
// Retun Value		: 1 : ERROR
// Argment Value	: NON
// Explanation		: Check Cver function
// History			: First edition 						2014.02.27 K.abe
//********************************************************************************

unsigned char CmdRdChk( void )
{
	unsigned char UcTestRD;
	unsigned char UcCount;
	
	for(UcCount=0; UcCount < READ_COUNT_NUM; UcCount++){
		RegReadA( TESTRD ,	&UcTestRD );					// 0x027F
		if( UcTestRD == 0xAC){
			return(0);
		}
	}
	return(1);
}


//********************************************************************************
// Function Name 	: MeasGyroAmp
// Retun Value		: NON
// Argment Value	: NON
// Explanation		: Measuring Gyro Amp
// History			: First edition 						
//********************************************************************************
 #define		FIXCOEFF		412L	// 32.8*2*PI*2
void	MeasGyroAmp ( unsigned char	UcFreq , unsigned char	UcMeasMode )
{

	MesFil( NOISE ) ;					// ����p�t�B���^�[��ݒ肷��B


	RamWrite32A( sxsin , 0x00000000 );		// 0x10D5
    RamWrite32A( sysin , 0x00000000 ); 		// 0x11D5
	SetSinWavePara( 0x04 , XHALWAVE ); 

	if( UcMeasMode == 0x00 ){
		RegWriteA( WC_MES1ADD0,  ( unsigned char )GXIZ ) ;							/* 0x0194	*/
		RegWriteA( WC_MES1ADD1,  ( unsigned char )(( GXIZ >> 8 ) & 0x0001 ) ) ;		/* 0x0195	*/
		RegWriteA( WC_MES2ADD0,  ( unsigned char )GYIZ ) ;							/* 0x0196	*/
		RegWriteA( WC_MES2ADD1,  ( unsigned char )(( GYIZ >> 8 ) & 0x0001 ) ) ;		/* 0x0197	*/
	} else {
		RegWriteA( WC_MES1ADD0,  ( unsigned char )GX2SXZ ) ;							/* 0x0194	*/
		RegWriteA( WC_MES1ADD1,  ( unsigned char )(( GX2SXZ >> 8 ) & 0x0001 ) ) ;		/* 0x0195	*/
		RegWriteA( WC_MES2ADD0,  ( unsigned char )GY2SYZ ) ;							/* 0x0196	*/
		RegWriteA( WC_MES2ADD1,  ( unsigned char )(( GY2SYZ >> 8 ) & 0x0001 ) ) ;		/* 0x0197	*/
	}

	RegWriteA( WC_MESLOOP1	, 0x00 );			// 0x0193	CmMesLoop[15:8]
//	RegWriteA( WC_MESLOOP0	, 0x10);			// 0x0192	CmMesLoop[7:0]
//	RamWrite32A( msmean	, 0x3D800000 );			// 0x1230	1/CmMesLoop[15:0]
	RegWriteA( WC_MESLOOP0	, 0x08);			// 0x0192	CmMesLoop[7:0]
	RamWrite32A( msmean	, 0x3E000000 );			// 0x1230	1/CmMesLoop[15:0]
	
	RamWrite32A( MSPP1AV, 	0x00000000 ) ;		// 0x1042
	RamWrite32A( MSPP2AV, 	0x00000000 ) ;		// 0x1142
	
	RegWriteA( WC_MESABS, 0x00 ) ;				// 0x0198	none ABS
	BsyWit( WC_MESMODE, 0x02 ) ;				// 0x0190		Sine wave Measure

	RamAccFixMod( ON ) ;							// Fix mode
	RamReadA( MSPP1AV, &StMeasGInfo.StGyrAmp.UsGyrXamp ) ;		// 0x1042
	RamReadA( MSPP2AV, &StMeasGInfo.StGyrAmp.UsGyrYamp ) ;		// 0x1142
	RamAccFixMod( OFF ) ;							// Float mode

	SetSinWavePara( 0x00 , XHALWAVE ); 	/* STOP */
	
	
	if( UcMeasMode == 0x00 ){
		StMeasGInfo.StGyrAgl.UlGyrXagl = (unsigned long)StMeasGInfo.StGyrAmp.UsGyrXamp * 1000L / ( (unsigned long)UcFreq * FIXCOEFF );
		StMeasGInfo.StGyrAgl.UlGyrYagl = (unsigned long)StMeasGInfo.StGyrAmp.UsGyrYamp * 1000L / ( (unsigned long)UcFreq * FIXCOEFF );

	} else {
		StMeasGInfo.StGyrAgl.UlGyrXagl = 0x00000000 ;
		StMeasGInfo.StGyrAgl.UlGyrYagl = 0x00000000 ;
	}
}