int lgit_ois_calibration(int ver)
{
int16_t gyro_offset_value_x, gyro_offset_value_y = 0;
CDBG("%s: lgit_ois_calibration start \n", __func__);
//Gyro Zero Calibration Starts.
RegWriteA(0x6020,0x01);//update mode & servo on & ois off mode
if (!lgit2_ois_poll_ready(LIMIT_STATUS_POLLING)) {
CDBG("%s 0x6024 result fail 1 @01\n", __func__);
return OIS_INIT_TIMEOUT;
}
//Gyro On
RegWriteA(0x6023,0x00);
if (!lgit2_ois_poll_ready(LIMIT_STATUS_POLLING)) {
CDBG("%s 0x6024 result fail 3 @02\n", __func__);
return OIS_INIT_TIMEOUT;
}
//X
RegWriteA(0x6088,0);
//usleep(50000);
if (!lgit2_ois_poll_ready(LIMIT_STATUS_POLLING + 30)) {
CDBG("%s 0x6024 result fail 3 @03\n", __func__);
return OIS_INIT_TIMEOUT;
}
RegReadB(0x608A, &gyro_offset_value_x);
if (!lgit2_ois_poll_ready(LIMIT_STATUS_POLLING)) {
CDBG("%s 0x6024 result fail 3 @04\n", __func__);
return OIS_INIT_TIMEOUT;
}
//Y
RegWriteA(0x6088,1);
//usleep(50000);
if (!lgit2_ois_poll_ready(LIMIT_STATUS_POLLING + 30)) {
CDBG("%s 0x6024 result fail 3 @05\n", __func__);
return OIS_INIT_TIMEOUT;
}
RegReadB(0x608A, &gyro_offset_value_y);
if (!lgit2_ois_poll_ready(LIMIT_STATUS_POLLING)) {
CDBG("%s 0x6024 result fail 3 @06A\n", __func__);
return OIS_INIT_TIMEOUT;
}
//Cal. Dato to eeprom
ois_i2c_e2p_write(0x0908, (uint16_t)(0xFFFF & gyro_offset_value_x), 2);
ois_i2c_e2p_write(0x090A, (uint16_t)(0xFFFF & gyro_offset_value_y), 2); //gyro_offset_value_x -> gyro_offset_value_y로 수정함.(김형관)
//Cal. Data to OIS Driver
RegWriteA(0x609C, 0x00);
RamWriteA(0x609D, gyro_offset_value_x); //16
RegWriteA(0x609C, 0x01);
RamWriteA(0x609D, gyro_offset_value_y); //16
// Gyro Zero Calibration Ends.
CDBG("%s gyro_offset_value_x %d gyro_offset_value_y %d \n", __func__, gyro_offset_value_x, gyro_offset_value_y);
g_gyro_offset_value_x = gyro_offset_value_x;
g_gyro_offset_value_y = gyro_offset_value_y;
CDBG("%s: lgit_ois_calibration end\n", __func__);
return OIS_SUCCESS;
}
//********************************************************************************
// Function Name : IniHfl
// Retun Value : NON
// Argment Value : NON
// Explanation : Hall Filter Initial Parameter Setting
// History : First edition 2009.07.30 Y.Tashita
//********************************************************************************
void IniHfl( void )
{
unsigned short UsAryId ;
// Hall&Gyro Register Parameter Setting
UsAryId = 0 ;
while( CsHalReg[ UsAryId ].UsRegAdd != 0xFFFF )
{
RegWriteA( CsHalReg[ UsAryId ].UsRegAdd, CsHalReg[ UsAryId ].UcRegDat ) ;
UsAryId++ ;
}
// Hall Filter Parameter Setting
UsAryId = 0 ;
while( CsHalFil[ UsAryId ].UsRamAdd != 0xFFFF )
{
RamWriteA( CsHalFil[ UsAryId ].UsRamAdd, CsHalFil[ UsAryId ].UsRamDat ) ;
UsAryId++ ;
}
}
//********************************************************************************
// Function Name : IniAdj
// Retun Value : NON
// Argment Value : NON
// Explanation : Adjust Value Setting
// History : First edition 2009.07.30 Y.Tashita
//********************************************************************************
void IniAdj( void )
{
RegWriteA( CMSDAC, BAIS_CUR ) ; // 0x0261 Hall Dac電流
RegWriteA( OPGSEL, AMP_GAIN ) ; // 0x0262 Hall amp Gain
#ifdef USE_EXE2PROM
unsigned short UsAdjCompF ;
unsigned short UsAdjData ;
unsigned long UlAdjData ;
E2pRed( (unsigned short)ADJ_COMP_FLAG, 2, ( unsigned char * )&UsAdjCompF ) ; // Eeprom Read
/* Hall Xaxis Bias,Offset */
if( (UsAdjCompF == 0x0000 ) || (UsAdjCompF & ( EXE_HXADJ - EXE_END )) ){
RamWriteA( DAHLXO, DAHLXO_INI ) ; // 0x1114
RamWriteA( DAHLXB, DAHLXB_INI ) ; // 0x1115
RamWriteA( ADHXOFF, 0x0000 ) ; // 0x1102
}else{
E2pRed( (unsigned short)HALL_OFFSET_X, 2, ( unsigned char * )&UsAdjData ) ;
RamWriteA( DAHLXO, UsAdjData ) ; // 0x1114
E2pRed( (unsigned short)HALL_BIAS_X, 2, ( unsigned char * )&UsAdjData ) ;
RamWriteA( DAHLXB, UsAdjData ) ; // 0x1115
E2pRed( (unsigned short)HALL_AD_OFFSET_X, 2, ( unsigned char * )&UsAdjData ) ;
RamWriteA( ADHXOFF, UsAdjData ) ; // 0x1102
}
/* Hall Yaxis Bias,Offset */
if( (UsAdjCompF == 0x0000 ) || (UsAdjCompF & ( EXE_HYADJ - EXE_END )) ){
RamWriteA( DAHLYO, DAHLYO_INI ) ; // 0x1116
RamWriteA( DAHLYB, DAHLYB_INI ) ; // 0x1117
RamWriteA( ADHYOFF, 0x0000 ) ; // 0x1105
}else{
E2pRed( (unsigned short)HALL_OFFSET_Y, 2, ( unsigned char * )&UsAdjData ) ;
RamWriteA( DAHLYO, UsAdjData ) ; // 0x1116
E2pRed( (unsigned short)HALL_BIAS_Y, 2, ( unsigned char * )&UsAdjData ) ;
RamWriteA( DAHLYB, UsAdjData ) ; // 0x1117
E2pRed( (unsigned short)HALL_AD_OFFSET_Y, 2, ( unsigned char * )&UsAdjData ) ;
RamWriteA( ADHYOFF, UsAdjData ) ; // 0x1105
}
/* Hall Xaxis Loop Gain */
if( (UsAdjCompF == 0x0000 ) || (UsAdjCompF & ( EXE_LXADJ - EXE_END )) ){
RamWriteA( lxgain, LXGAIN_INI ) ; // 0x132A
}else{
E2pRed( (unsigned short)LOOP_GAIN_X, 2, ( unsigned char * )&UsAdjData ) ;
RamWriteA( lxgain, UsAdjData ) ; // 0x132A
}
/* Hall Yaxis Loop Gain */
if( (UsAdjCompF == 0x0000 ) || (UsAdjCompF & ( EXE_LYADJ - EXE_END )) ){
RamWriteA( lygain, LYGAIN_INI ) ; // 0x136A
}else{
E2pRed( (unsigned short)LOOP_GAIN_Y, 2, ( unsigned char * )&UsAdjData ) ;
RamWriteA( lygain, UsAdjData ) ; // 0x136A
}
// X axis Optical Center Offset Read & Setting
E2pRed( (unsigned short)OPT_CENTER_X, 2, ( unsigned char * )&UsAdjData ) ;
if( ( UsAdjData != 0x0000 ) && ( UsAdjData != 0xffff )){
UsCntXof = UsAdjData ; /* Set Optical center X value */
} else {
UsCntXof = OPTCEN_X ; /* Clear Optical center X value */
}
RamWriteA( HXINOD, UsCntXof ) ; // 0x1127
// Y axis Optical Center Offset Read & Setting
E2pRed( (unsigned short)OPT_CENTER_Y, 2, ( unsigned char * )&UsAdjData ) ;
if( ( UsAdjData != 0x0000 ) && ( UsAdjData != 0xffff )){
UsCntYof = UsAdjData ; /* Set Optical center Y value */
} else {
UsCntYof = OPTCEN_Y ; /* Clear Optical center Y value */
}
RamWriteA( HYINOD, UsCntYof ) ; // 0x1167
/* Gyro Xaxis Offset */
E2pRed( (unsigned short)GYRO_AD_OFFSET_X, 2, ( unsigned char * )&UsAdjData ) ;
if( ( UsAdjData == 0x0000 ) || ( UsAdjData == 0xffff )){
RamWriteA( ADGXOFF, 0x0000 ) ; // 0x1108
RegWriteA( IZAH, DGYRO_OFST_XH ) ; // 0x03A0 Set Offset High byte
RegWriteA( IZAL, DGYRO_OFST_XL ) ; // 0x03A1 Set Offset Low byte
}else{
RamWriteA( ADGXOFF, 0x0000 ) ; // 0x1108
RegWriteA( IZAH, (unsigned char)(UsAdjData >> 8) ) ; // 0x03A0 Set Offset High byte
RegWriteA( IZAL, (unsigned char)(UsAdjData) ) ; // 0x03A1 Set Offset Low byte
}
/* Gyro Yaxis Offset */
E2pRed( (unsigned short)GYRO_AD_OFFSET_Y, 2, ( unsigned char * )&UsAdjData ) ;
if( ( UsAdjData == 0x0000 ) || ( UsAdjData == 0xffff )){
RamWriteA( ADGYOFF, 0x0000 ) ; // 0x110B
RegWriteA( IZBH, DGYRO_OFST_YH ) ; // 0x03A2 Set Offset High byte
RegWriteA( IZBL, DGYRO_OFST_YL ) ; // 0x03A3 Set Offset Low byte
}else{
RamWriteA( ADGYOFF, 0x0000 ) ; // 0x110B
RegWriteA( IZBH, (unsigned char)(UsAdjData >> 8) ) ; // 0x03A2 Set Offset High byte
RegWriteA( IZBL, (unsigned char)(UsAdjData) ) ; // 0x03A3 Set Offset Low byte
}
/* Gyro Xaxis Gain */
E2pRed( (unsigned short)GYRO_GAIN_X, 4 , ( unsigned char * )&UlAdjData ) ;
if( ( UlAdjData != 0x00000000 ) && ( UlAdjData != 0xffffffff )){
RamWrite32A( gxzoom, UlAdjData ) ; // 0x1828 Gyro X axis Gain adjusted value
}else{
RamWrite32A( gxzoom, GXGAIN_INI ) ; // 0x1828 Gyro X axis Gain adjusted initial value
}
/* Gyro Yaxis Gain */
E2pRed( (unsigned short)GYRO_GAIN_Y, 4 , ( unsigned char * )&UlAdjData ) ;
if( ( UlAdjData != 0x00000000 ) && ( UlAdjData != 0xffffffff )){
RamWrite32A( gyzoom, UlAdjData ) ; // 0x1928 Gyro Y axis Gain adjusted value
}else{
RamWrite32A( gyzoom, GXGAIN_INI ) ; // 0x1928 Gyro Y axis Gain adjusted initial value
}
/* OSC Clock value */
E2pRed( (unsigned short)OSC_CLK_VAL, 2 , ( unsigned char * )&UsAdjData ) ;
if((unsigned char)UsAdjData != 0xff ){
UsCntYof = UsAdjData ; /* Optical center X value */
RegWriteA( OSCSET, (unsigned char)UsAdjData ) ; // 0x0264
}else{
RegWriteA( OSCSET, OSC_INI ) ; // 0x0264
}
#else
/* adjusted value */
RegWriteA( IZAH, DGYRO_OFST_XH ) ; // 0x03A0 Set Offset High byte
RegWriteA( IZAL, DGYRO_OFST_XL ) ; // 0x03A1 Set Offset Low byte
RegWriteA( IZBH, DGYRO_OFST_YH ) ; // 0x03A2 Set Offset High byte
RegWriteA( IZBL, DGYRO_OFST_YL ) ; // 0x03A3 Set Offset Low byte
RamWriteA( DAHLXO, DAHLXO_INI ) ; // 0x1114
RamWriteA( DAHLXB, DAHLXB_INI ) ; // 0x1115
RamWriteA( DAHLYO, DAHLYO_INI ) ; // 0x1116
RamWriteA( DAHLYB, DAHLYB_INI ) ; // 0x1117
RamWriteA( ADHXOFF, ADHXOFF_INI ) ; // 0x1102
RamWriteA( ADHYOFF, ADHYOFF_INI ) ; // 0x1105
RamWriteA( lxgain, LXGAIN_INI ) ; // 0x132A
RamWriteA( lygain, LYGAIN_INI ) ; // 0x136A
RamWriteA( ADGXOFF, 0x0000 ) ; // 0x1108
RamWriteA( ADGYOFF, 0x0000 ) ; // 0x110B
UsCntXof = OPTCEN_X ; /* Clear Optical center X value */
UsCntYof = OPTCEN_Y ; /* Clear Optical center Y value */
RamWriteA( HXINOD, UsCntXof ) ; // 0x1127
RamWriteA( HYINOD, UsCntYof ) ; // 0x1167
RamWrite32A( gxzoom, GXGAIN_INI ) ; // 0x1828 Gyro X axis Gain adjusted value
RamWrite32A( gyzoom, GYGAIN_INI ) ; // 0x1928 Gyro Y axis Gain adjusted value
RegWriteA( OSCSET, OSC_INI ) ; // 0x0264 OSC adj
#endif
RamWriteA( pzgxp, PZGXP_INI ) ; // 0x133C X axis output direction initial value
RamWriteA( pzgyp, PZGYP_INI ) ; // 0x137C Y axis output direction initial value
RamWriteA( hxinog, 0x7fff ) ; // 0x1128 back up initial value
RamWriteA( hyinog, 0x7fff ) ; // 0x1168 back up initial value
SetZsp(0) ; // Zoom coefficient Initial Setting
RegWriteA( PWMA , 0xC0 ); // 0x0074 PWM enable
RegWriteA( STBB , 0x0F ); // 0x0260 [ - | - | - | - ][ STBOPAY | STBOPAX | STBDAC | STBADC ]
RegWriteA( LXEQEN , 0x45 ); // 0x0084
RegWriteA( LYEQEN , 0x45 ); // 0x008E
SetPanTiltMode( OFF ) ; /* Pan/Tilt OFF */
#ifdef H1COEF_CHANGER
SetH1cMod( ACTMODE ) ; /* Lvl Change Active mode */
#endif
DrvSw( ON ) ; /* 0x0070 Driver Mode setting */
}
//********************************************************************************
// Function Name : IniSrv
// Retun Value : NON
// Argment Value : NON
// Explanation : Servo Initial Setting
// History : First edition 2009.07.30 Y.Tashita
//********************************************************************************
void IniSrv( void )
{
UcPwmMod = INIT_PWMMODE ; // Driver output mode
RegWriteA( VGA_SET, 0x30 ) ; // 0x0267 X,Y connect
RegWriteA( LSVFC1 , 0x00 ) ; // 0x0082
if( UcPwmMod == PWMMOD_CVL ) {
RegWriteA( LXEQFC2 , 0x01 ) ; // 0x0083 Linear補正OFF
RegWriteA( LYEQFC2 , 0x01 ) ; // 0x008D
}else{
RegWriteA( LXEQFC2 , 0x00 ) ; // 0x0083 Linear補正OFF
RegWriteA( LYEQFC2 , 0x00 ) ; // 0x008D
}
/* X axis */
RegWriteA( LXEQEN , 0x45 ); // 0x0084 LXSW OFF
RegWriteA( LXEQFC , 0x00 ); // 0x0085 LXDSWB/LXPSWB/LXISWB ON
RamWriteA( ADHXOFF, 0x0000 ) ; // 0x1102
RamWriteA( ADSAD4OFF, 0x0000 ) ; // 0x110E
RamWriteA( HXINOD, 0x0000 ) ; // 0x1127
RamWriteA( HXDCIN, 0x0000 ) ; // 0x1126
RamWriteA( HXSEPT1, 0x0000 ) ; // 0x1123
RamWriteA( HXSEPT2, 0x0000 ) ; // 0x1124
RamWriteA( HXSEPT3, 0x0000 ) ; // 0x1125
RamWriteA( LXDOBZ, 0x0000 ) ; // 0x114A
RamWriteA( LXFZF, 0x0000 ) ; // 0x114B
RamWriteA( LXFZB, 0x0000 ) ; // 0x1156
RamWriteA( LXDX, 0x0000 ) ; // 0x1148
RamWriteA( LXLMT, 0x7FFF ) ; // 0x1157
RamWriteA( LXLMT2, 0x7FFF ) ; // 0x1158
RamWriteA( LXLMTSD, 0x0000 ) ; // 0x1159
RamWriteA( PLXOFF, 0x0000 ) ; // 0x115B
RamWriteA( LXDODAT, 0x0000 ) ; // 0x115A
/* Y axis */
RegWriteA( LYEQEN , 0x45 ); // 0x008E LYSW OFF
RegWriteA( LYEQFC , 0x00 ); // 0x008F LYDSWB/LYPSWB/LYISWB ON
RamWriteA( ADHYOFF, 0x0000 ) ; // 0x1105
RamWriteA( HYINOD, 0x0000 ) ; // 0x1167
RamWriteA( HYDCIN, 0x0000 ) ; // 0x1166
RamWriteA( HYSEPT1, 0x0000 ) ; // 0x1163
RamWriteA( HYSEPT2, 0x0000 ) ; // 0x1164
RamWriteA( HYSEPT3, 0x0000 ) ; // 0x1165
RamWriteA( LYDOBZ, 0x0000 ) ; // 0x118A
RamWriteA( LYFZF, 0x0000 ) ; // 0x118B
RamWriteA( LYFZB, 0x0000 ) ; // 0x1196
RamWriteA( LYDX, 0x0000 ) ; // 0x1188
RamWriteA( LYLMT, 0x7FFF ) ; // 0x1197
RamWriteA( LYLMT2, 0x7FFF ) ; // 0x1198
RamWriteA( LYLMTSD, 0x0000 ) ; // 0x1199
RamWriteA( PLYOFF, 0x0000 ) ; // 0x119B
RamWriteA( LYDODAT, 0x0000 ) ; // 0x119A
/* General Equalizer */
RegWriteA( GNEQEN, 0x00 ) ; // 0x009E General Equalizer OFF
RegWriteA( GNEQFC, 0x00 ) ; // 0x009F General mode
RegWriteA( GNINADD, 0x0F ) ; // 0x00A2 Default Setting
RegWriteA( GNOUTADD, 0x00 ) ; // 0x00A3 General Equalizer Output Cut Off
RamWriteA( GNLMT, 0x7FFF ) ; // 0x11AC Limmiter
RamWriteA( GDX, 0x0000 ) ; // 0x11AF
RamWriteA( GDOFFSET, 0x0000 ) ; // 0x11B0
RamWriteA( GOFF, 0x0000 ) ; // 0x11A0
// General Data Pass
RegWriteA( GDPXFC, 0x00 ) ; // 0x00A4 General data pass
/* Calculation flow X : Y1->X1 Y : X2->Y2 */
RegWriteA( LCXFC, (unsigned char)0x00 ) ; // 0x0001 High-order function X function setting
RegWriteA( LCYFC, (unsigned char)0x00 ) ; // 0x0006 High-order function Y function setting
RegWriteA( LCY1INADD, (unsigned char)LXDOIN ) ; // 0x0007 High-order function Y1 input selection
RegWriteA( LCY1OUTADD, (unsigned char)DLY00 ) ; // 0x0008 High-order function Y1 output selection
RegWriteA( LCX1INADD, (unsigned char)DLY00 ) ; // 0x0002 High-order function X1 input selection
RegWriteA( LCX1OUTADD, (unsigned char)LXADOIN ) ; // 0x0003 High-order function X1 output selection
RegWriteA( LCX2INADD, (unsigned char)LYDOIN ) ; // 0x0004 High-order function X2 input selection
RegWriteA( LCX2OUTADD, (unsigned char)DLY01 ) ; // 0x0005 High-order function X2 output selection
RegWriteA( LCY2INADD, (unsigned char)DLY01 ) ; // 0x0009 High-order function Y2 input selection
RegWriteA( LCY2OUTADD, (unsigned char)LYADOIN ) ; // 0x000A High-order function Y2 output selection
/* (0.5X^3+0.4453261X)*(0.5X^3+0.4453261X) */
RamWriteA( LCY1A0, 0x0000 ) ; // 0x12F2 0
RamWriteA( LCY1A1, 0x3900 ) ; // 0x12F3 1
RamWriteA( LCY1A2, 0x0000 ) ; // 0x12F4 2
RamWriteA( LCY1A3, 0x4000 ) ; // 0x12F5 3
RamWriteA( LCY1A4, 0x0000 ) ; // 0x12F6 4
RamWriteA( LCY1A5, 0x0000 ) ; // 0x12F7 5
RamWriteA( LCY1A6, 0x0000 ) ; // 0x12F8 6
RamWriteA( LCX1A0, 0x0000 ) ; // 0x12D2 0
RamWriteA( LCX1A1, 0x3900 ) ; // 0x12D3 1
RamWriteA( LCX1A2, 0x0000 ) ; // 0x12D4 2
RamWriteA( LCX1A3, 0x4000 ) ; // 0x12D5 3
RamWriteA( LCX1A4, 0x0000 ) ; // 0x12D6 4
RamWriteA( LCX1A5, 0x0000 ) ; // 0x12D7 5
RamWriteA( LCX1A6, 0x0000 ) ; // 0x12D8 6
RamWriteA( LCX2A0, 0x0000 ) ; // 0x12D9 0
RamWriteA( LCX2A1, 0x3900 ) ; // 0x12DA 1
RamWriteA( LCX2A2, 0x0000 ) ; // 0x12DB 2
RamWriteA( LCX2A3, 0x4000 ) ; // 0x12DC 3
RamWriteA( LCX2A4, 0x0000 ) ; // 0x12DD 4
RamWriteA( LCX2A5, 0x0000 ) ; // 0x12DE 5
RamWriteA( LCX2A6, 0x0000 ) ; // 0x12DF 6
RamWriteA( LCY2A0, 0x0000 ) ; // 0x12F9 0
RamWriteA( LCY2A1, 0x3900 ) ; // 0x12FA 1
RamWriteA( LCY2A2, 0x0000 ) ; // 0x12FB 2
RamWriteA( LCY2A3, 0x4000 ) ; // 0x12FC 3
RamWriteA( LCY2A4, 0x0000 ) ; // 0x12FD 4
RamWriteA( LCY2A5, 0x0000 ) ; // 0x12FE 5
RamWriteA( LCY2A6, 0x0000 ) ; // 0x12FF 6
RegWriteA( GDPX1INADD, 0x00 ) ; // 0x00A5 Default Setting
RegWriteA( GDPX1OUTADD, 0x00 ) ; // 0x00A6 General Data Pass Output Cut Off
RegWriteA( GDPX2INADD, 0x00 ) ; // 0x00A7 Default Setting
RegWriteA( GDPX2OUTADD, 0x00 ) ; // 0x00A8 General Data Pass Output Cut Off
RegWriteA( GDPX3INADD, 0x00 ) ; // 0x00A9 Default Setting
RegWriteA( GDPX3OUTADD, 0x00 ) ; // 0x00AA General Data Pass Output Cut Off
RegWriteA( GDPYFC, 0x00 ) ; // 0x00AB General data pass
RegWriteA( GDPY1INADD, 0x00 ) ; // 0x00AC Default Setting
RegWriteA( GDPY1OUTADD, 0x00 ) ; // 0x00AD General Data Pass Output Cut Off
RegWriteA( GDPY2INADD, 0x00 ) ; // 0x00AE Default Setting
RegWriteA( GDPY2OUTADD, 0x00 ) ; // 0x00AF General Data Pass Output Cut Off
RegWriteA( GDPY3INADD, 0x00 ) ; // 0x00B0 Default Setting
RegWriteA( GDPY3OUTADD, 0x00 ) ; // 0x00B1 General Data Pass Output Cut Off
// Feed Forward X Filter
RegWriteA( FFXEN, 0x00 ) ; // 0x00B2 Equalizer OFF
RegWriteA( FFXFC, 0x00 ) ; // 0x00B3 45°Convert Circuit OFF
RegWriteA( FFXDS, 0x00 ) ; // 0x00B4 Down Sampling 1/1
RegWriteA( FXINADD, 0x2C ) ; // 0x00B7 LXGZF
RegWriteA( FXOUTADD, 0x49 ) ; // 0x00B8 LXGZB
// Feed Forward Y Filter
RegWriteA( FFYEN, 0x00 ) ; // 0x00B9 Equalizer OFF
RegWriteA( FFYFC, 0x00 ) ; // 0x00BA 45°Convert Circuit OFF
RegWriteA( FFYDS, 0x00 ) ; // 0x00BB Down Sampling 1/1
RegWriteA( FYINADD, 0x6C ) ; // 0x00BE LYGZF
RegWriteA( FYOUTADD, 0x89 ) ; // 0x00BF LYGZB
// Measure Filter
RegWriteA( MSF1EN, 0x00 ) ; // 0x00C0 Measure Equalizer1 OFF
RegWriteA( MSF2EN, 0x00 ) ; // 0x00C4 Measure Equalizer2 OFF
RegWriteA( MSFDS, 0x00 ) ; // 0x00C8 Down sampling 1/1
RegWriteA( MS1INADD, 0x46 ) ; // 0x00C2 LXC1
RegWriteA( MS1OUTADD, 0x00 ) ; // 0x00C3 Measure Filter1 Output Cut Off
RegWriteA( MS2INADD, 0x47 ) ; // 0x00C6 LXC2 Setting
RegWriteA( MS2OUTADD, 0x00 ) ; // 0x00C7 Measure Filter2 Output Cut Off
// Gyro Filter Interface
RegWriteA( GYINFC, 0x00 ) ; // 0x00DA LXGZB,LYGZB Input Cut Off, 0 Sampling Delay, Down Sampling 1/1
// Sin Wave Generater
RegWriteA( SWEN, 0x08 ) ; // 0x00DB Sin Wave Generate OFF, Sin Wave Setting
RegWriteA( SWFC2, 0x08 ) ; // 0x00DE SWC = 0
RegWriteA( SWSEL, 0x00 ) ; // 0x00E2 No Operation
RegWriteA( SINXADD, 0x00 ) ; // 0x00E3
RegWriteA( SINYADD, 0x00 ) ; // 0x00E4
// Delay RAM Monitor
RegWriteA( DAMONFC, 0x00 ) ; // 0x00F5 ExDAC OFF , Default Setting
RegWriteA( MDLY1ADD, 0x10 ) ; // 0x00E5 Delay Monitor1
RegWriteA( MDLY2ADD, 0x11 ) ; // 0x00E6 Delay Monitor2
// Delay RAM Clear
BsyWit( DLYCLR, 0xFF ) ; // 0x00EE Delay RAM All Clear
BsyWit( DLYCLR2, 0xEC ) ; // 0x00EF Delay RAM All Clear
RegWriteA( DLYCLR , 0x00 ); // 0x00EE CLR disable
// Hall Amp...
RegWriteA( RTXADD, 0x00 ) ; // 0x00CE Cal OFF
RegWriteA( RTYADD, 0x00 ) ; // 0x00E8 Cal OFF
// PWM Signal Generate
DrvSw( OFF ) ; /* 0x0070 Drvier Block Ena=0 */
RegWriteA( DRVFC2 , 0x40 ); // 0x0068 PriDriver:Slope, Driver:DeadTime 12.5ns
RegWriteA( DRVSELX , 0x00 ); // 0x0071 PWM X drv max current DRVSELX[7:0]
RegWriteA( DRVSELY , 0x00 ); // 0x0072 PWM Y drv max current DRVSELY[7:0]
#ifdef LOWCURRENT
RegWriteA( PWMFC, 0x4D ) ; // 0x0075 VREF, PWMCLK/64 MODEB, 12Bit Accuracy
#else
RegWriteA( PWMFC, 0x11 ) ; // 0x0075 VREF, PWMCLK/512, MODE1, 12Bit Accuracy
#endif
RegWriteA( PWMA, 0x00 ) ; // 0x0074 PWM X/Y standby
RegWriteA( PWMDLY1, 0x04 ) ; // 0x0076 X Phase Delay Setting
RegWriteA( PWMDLY2, 0x04 ) ; // 0x0077 Y Phase Delay Setting
RegWriteA( LNA , 0xC0 ); // 0x0078 Low Noise mode enable
RegWriteA( LNFC , 0x02 ); // 0x0079
RegWriteA( LNSMTHX , 0x80 ); // 0x007A
RegWriteA( LNSMTHY , 0x80 ); // 0x007B
RegWriteA( GEPWMFC, 0x01 ) ; // 0x007E General PWM Output Stanby, 12Bit Accuracy
RegWriteA( GEPWMDLY, 0x00 ) ; // 0x007F Default Setting
// Measure Circuit
RegWriteA( MSMA, 0x00 ) ; // 0x00C9 Measure mode OFF
// Flag Monitor
RegWriteA( FLGM, 0xCC ) ; // 0x00F8 BUSY2 Output ON
RegWriteA( FLGIST, 0xCC ) ; // 0x00F9 Interrupt Clear
RegWriteA( FLGIM2, 0xF8 ) ; // 0x00FA BUSY2 Output ON
RegWriteA( FLGIST2, 0xF8 ) ; // 0x00FB Interrupt Clear
// Function Setting
RegWriteA( FCSW, 0x00 ) ; // 0x00F6 2Axis Input, PWM Mode, X,Y Axis Reverse OFF
RegWriteA( FCSW2, 0x00 ) ; // 0x00F7 X,Y Axis Invert OFF, PWM Synchronous, A/D Over Sampling ON
/* Srv Smooth start */
RamWriteA( HXSMSTP , 0x0400 ) ; /* 0x1120 */
RamWriteA( HYSMSTP , 0x0400 ) ; /* 0x1160 */
RegWriteA( SSSFC1, 0x43 ) ; // 0x0098 0.68ms * 8times = 5.46ms
RegWriteA( SSSFC2, 0x03 ) ; // 0x0099 1.36ms * 3 = 4.08ms
RegWriteA( SSSFC3, 0x50 ) ; // 0x009A 1.36ms
/* Srv Emargency */
RegWriteA( SEOEN, 0x00 ) ; // 0x009B Emargency mode off
RegWriteA( SEOFC1, 0x01 ) ; // 0x009C Emargency para 1
RegWriteA( SEOFC2, 0x77 ) ; // 0x009D Emargency para 2 699ms
RamWriteA( LXSEOLMT , 0x7000 ) ; /* 0x12D0 */
RamWriteA( LYSEOLMT , 0x7000 ) ; /* 0x12D1 */
RegWriteA( STBB, 0x00 ) ; // 0x0260 All standby
}
