//******************************************************************************** // 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 ; } } }