//********************************************************************************
// Function Name : SelectGySleep
// Retun Value : NON
// Argment Value : mode
// Explanation : Select Gyro mode Function
// History : First edition 2010.12.27 Y.Shigeoka
//********************************************************************************
void SelectGySleep( unsigned char UcSelMode )
{
unsigned char UcRamIni ;
unsigned char UcGrini ;
if(UcSelMode == ON)
{
RegWriteA( GEQON, 0x00 ) ; // 0x0100 GYRO Equalizer OFF
RegWriteA( GRSEL, 0x01 ) ; /* 0x0380 Set Command Mode */
RegReadA( GRINI , &UcGrini ); // 0x0381 [ PARA_REG | AXIS7EN | AXIS4EN | - ][ LSBF | SLOWMODE | I2CMODE | - ]
RegWriteA( GRINI, ( UcGrini | SLOWMODE) ); // 0x0381 [ PARA_REG | AXIS7EN | AXIS4EN | - ][ LSBF | SLOWMODE | I2CMODE | - ]
RegWriteA( GRADR0, 0x6B ) ; /* 0x0383 Set Write Command */
RegWriteA( GRACC, 0x02 ) ; /* 0x0382 Set Read Trigger ON */
AccWit( 0x10 ) ; /* Digital Gyro busy wait */
RegReadA( GRADT0H, &UcRamIni ) ; /* 0x0390 */
UcRamIni |= 0x40 ; /* Set Sleep bit */
RegWriteA( GRADR0, 0x6B ) ; /* 0x0383 Set Write Command */
RegWriteA( GSETDT, UcRamIni ) ; /* 0x038A Set Write Data(Sleep ON) */
RegWriteA( GRACC, 0x10 ) ; /* 0x0382 Set Trigger ON */
AccWit( 0x10 ) ; /* Digital Gyro busy wait */
}
else
{
RegWriteA( GRADR0, 0x6B ) ; /* 0x0383 Set Write Command */
RegWriteA( GRACC, 0x02 ) ; /* 0x0382 Set Read Trigger ON */
AccWit( 0x10 ) ; /* Digital Gyro busy wait */
RegReadA( GRADT0H, &UcRamIni ) ; /* 0x0390 */
UcRamIni &= ~0x40 ; /* Clear Sleep bit */
RegWriteA( GSETDT, UcRamIni ) ; // 0x038A Set Write Data(Sleep OFF)
RegWriteA( GRACC, 0x10 ) ; /* 0x0382 Set Trigger ON */
AccWit( 0x10 ) ; /* Digital Gyro busy wait */
RegReadA( GRINI , &UcGrini ); // 0x0381 [ PARA_REG | AXIS7EN | AXIS4EN | - ][ LSBF | SLOWMODE | I2CMODE | - ]
RegWriteA( GRINI, ( UcGrini & ~SLOWMODE) ); // 0x0381 [ PARA_REG | AXIS7EN | AXIS4EN | - ][ LSBF | SLOWMODE | I2CMODE | - ]
GyOutSignal( ) ; /* Select Gyro output signal */
WitTim( 50 ) ; // 50ms wait
RegWriteA( GEQON, 0x01 ) ; // 0x0100 GYRO Equalizer ON
ClrGyr( 0x06 , CLR_GYR_DLY_RAM );
}
}
static int fuji_ois_poll_ready(int limit)
{
uint8_t ois_status;
int read_byte = 0;
/* polling status ready */
RegReadA(OIS_READ_STATUS_ADDR, &ois_status);
read_byte++;
while ((ois_status != 0x01) && (read_byte < limit)) {
usleep(1000); /* wait 1ms */
RegReadA(OIS_READ_STATUS_ADDR, &ois_status); /* polling status ready */
read_byte++;
}
return ois_status;
}
static int lgit2_ois_poll_ready(int limit)
{
uint8_t ois_status;
int read_byte = 0;
RegReadA(OIS_READ_STATUS_ADDR, &ois_status); //polling status ready
read_byte++;
while((ois_status != 0x01) && (read_byte < limit)) {
RegReadA(OIS_READ_STATUS_ADDR, &ois_status); //polling status ready
usleep(1000); //wait 1ms
read_byte++;
}
return ois_status;
}
//********************************************************************************
// Function Name : IniDgy
// Retun Value : NON
// Argment Value : NON
// Explanation : Digital Gyro Initial Setting
// History : First edition 2009.11.10 Y.Hayashi
// LC898111 changes 2011.04.08 d.yamagata
//********************************************************************************
void IniDgy( void )
{
unsigned char UcGrini ;
/*************/
/*For ST gyro*/
/*************/
/*Set SPI Type*/
RegWriteA( SPIM , 0x01 ); // 0x038F [ - | - | - | - ][ - | - | - | DGSPI4 ]
// DGSPI4 0: 3-wire SPI, 1: 4-wire SPI
/*Set to Command Mode*/
RegWriteA( GRSEL , 0x01 ); // 0x0380 [ - | - | - | - ][ - | SRDMOE | OISMODE | COMMODE ]
/*Digital Gyro Read settings*/
RegWriteA( GRINI , 0x80 ); // 0x0381 [ PARA_REG | AXIS7EN | AXIS4EN | - ][ LSBF | SLOWMODE | I2CMODE | - ]
RegWriteA( GRINT , 0x00 ); // 0x03B0 [ - | - | - | - ][ - | - | INTB | INTEN ]
RegReadA( GRINI , &UcGrini ); // 0x0381 [ PARA_REG | AXIS7EN | AXIS4EN | - ][ LSBF | SLOWMODE | I2CMODE | - ]
RegWriteA( GRINI , ( UcGrini | SLOWMODE) ); // 0x0381 [ PARA_REG | AXIS7EN | AXIS4EN | - ][ LSBF | SLOWMODE | I2CMODE | - ]
RegWriteA( GRADR0 , 0x6A ) ; // 0x0383 Set USER CONTROL
RegWriteA( GSETDT , 0x10 ) ; // 0x038A Set Write Data
RegWriteA( GRACC , 0x10 ); // 0x0382 [ ADRPLUS(1:0) | - | WR1B ][ - | RD4B | RD2B | RD1B ]
AccWit( 0x10 ) ; /* Digital Gyro busy wait */
RegWriteA( GRADR0, 0x1B ) ; // 0x0383 Set GYRO_CONFIG
RegWriteA( GSETDT, ( FS_SEL << 3) ) ; // 0x038A Set Write Data
RegWriteA( GRACC, 0x10 ) ; /* 0x0382 Set Trigger ON */
AccWit( 0x10 ) ; /* Digital Gyro busy wait */
RegReadA( GRINI , &UcGrini ); // 0x0381 [ PARA_REG | AXIS7EN | AXIS4EN | - ][ LSBF | SLOWMODE | I2CMODE | - ]
RegWriteA( GRINI, ( UcGrini & ~SLOWMODE) ); // 0x0381 [ PARA_REG | AXIS7EN | AXIS4EN | - ][ LSBF | SLOWMODE | I2CMODE | - ]
UcStbySt = STBYST_OFF ; /* TEST */
GyOutSignal() ;
}
//********************************************************************************
// Function Name : AccWit
// Retun Value : NON
// Argment Value : Trigger Register Data
// Explanation : Acc Wait Function
// History : First edition 2010.12.27 Y.Shigeoka
//********************************************************************************
void AccWit( unsigned char UcTrgDat )
{
unsigned char UcFlgVal ;
UcFlgVal = 1 ;
while( UcFlgVal ) {
RegReadA( GRACC, &UcFlgVal ) ;
UcFlgVal &= UcTrgDat ;
} ;
}
//********************************************************************************
// Function Name : Bsy2Wit
// Retun Value : NON
// Argment Value : void
// Explanation : Busy2 Wait Function
// History : First edition 2011.05.17 Y.Shigeoka
//********************************************************************************
void Bsy2Wit( void )
{
unsigned char UcFlgVal ;
UcFlgVal = 1 ;
while( UcFlgVal ) {
RegReadA( BSYSEL, &UcFlgVal ) ;
UcFlgVal &= 0x80 ;
} ;
}
int32_t lgit2_ois_move_lens(int16_t target_x, int16_t target_y)
{
int8_t hallx = target_x / HALL_SCALE_FACTOR;
int8_t hally = target_y / HALL_SCALE_FACTOR;
uint8_t result = 0;
/* check ois mode & change to suitable mode */
RegReadA(0x6020, &result);
if (result != 0x01) {
RegWriteA(0x6020, 0x01);
if (!lgit2_ois_poll_ready(LIMIT_STATUS_POLLING))
return OIS_INIT_TIMEOUT;
}
printk("%s target : %d(0x%x), %d(0x%x)\n", __func__,
hallx, hallx, hally, hally);
/* hallx range -> D2 to 2E (-46, 46) */
RegWriteA(0x6099, 0xFF & hallx); /* target x position input */
RegWriteA(0x609A, 0xFF & hally); /* target y position input */
/* wait 100ms */
usleep(100000);
RegWriteA(0x6098, 0x01); /* order to move. */
if (!lgit2_ois_poll_ready(LIMIT_STATUS_POLLING * 12))
return OIS_INIT_TIMEOUT;
RegReadA(0x609B, &result);
RegWriteA(0x6023, 0x00);//Gyro On
RegWriteA(0x6021, 0x12);//LGIT STILL & PAN ON MODE
RegWriteA(0x6020, 0x02);//OIS ON
if (result == 0x03)
return OIS_SUCCESS;
printk("%s move fail : 0x%x \n", __func__, result);
return OIS_FAIL;
}
//********************************************************************************
// Function Name : BsyWit
// Retun Value : NON
// Argment Value : Trigger Register Address, Trigger Register Data
// Explanation : Busy Wait Function
// History : First edition 2009.07.30 Y.Tashita
//********************************************************************************
void BsyWit( unsigned short UsTrgAdr, unsigned char UcTrgDat )
{
unsigned char UcFlgVal ;
RegWriteA( UsTrgAdr, UcTrgDat ) ; // Trigger Register Setting
UcFlgVal = 1 ;
while( UcFlgVal ) {
RegReadA( FLGM, &UcFlgVal ) ; // 0x00F8
UcFlgVal &= 0x40 ;
} ;
}
//********************************************************************************
// Function Name : ClrGyr
// Retun Value : NON
// Argment Value : UcClrFil - Select filter to clear. If 0x00, clears entire filter
// UcClrMod - 0x01: Parameter RAM Clear, 0x02: Delay RAM Clear, 0x03: All RAM Clear
// Explanation : Gyro RAM clear function
// History : First edition 2011.04.08 d.yamagata
//********************************************************************************
void ClrGyr( unsigned char UcClrFil , unsigned char UcClrMod )
{
unsigned char UcRamClr;
unsigned char UcClrBit;
while(1){
if( UcClrMod == CLR_GYR_DLY_RAM )
{
if( UcClrFil & 0x10 ){
UcClrBit = 0x10 ;
}else if( UcClrFil & 0x08 ){
UcClrBit = 0x08 ;
}else if( UcClrFil & 0x04 ){
UcClrBit = 0x04 ;
}else if( UcClrFil & 0x02 ){
UcClrBit = 0x02 ;
}else if( UcClrFil & 0x01 ){
UcClrBit = 0x01 ;
}else{
UcClrBit = 0x00 ;
}
UcClrFil &= ~UcClrBit ;
}else{
UcClrBit = 0x00 ;
}
/*Select Filter to clear*/
RegWriteA( GRAMDLYMOD , UcClrBit ) ; // 0x011B [ - | - | - | P ][ T | L | H | I ]
// 指定したフィルターをクリアするが、
// 0x00の場合はフィルター全体をクリアする
/*Enable Clear*/
RegWriteA( GRAMINITON , UcClrMod ) ; // 0x0103 [ - | - | - | - ][ - | - | 遅延Clr | 係数Clr ]
/*Check RAM Clear complete*/
do{
RegReadA( GRAMINITON, &UcRamClr );
UcRamClr &= 0x03;
}while( UcRamClr != 0x00 );
if(( UcClrMod != CLR_GYR_DLY_RAM ) || ( UcClrFil == 0x00 )){
break ;
}
}
}
