//------------------------------------------------------------------------------
//! HWStatus_t MEMS_BMC050_SetAccRange(u8_t val)
//------------------------------------------------------------------------------
//! @brief set range of acceleration measurement
//------------------------------------------------------------------------------
HWStatus_t MEMS_BMC050_SetAccRange(u8_t val) {
if(val > 4)
return HW_ERR_ILLPARAMVAL;
switch(val){
default:
case 1:
AccSensitivity = 1;
break;
case 2:
AccSensitivity = 2;
break;
case 3:
AccSensitivity = 4;
break;
case 4:
AccSensitivity = 8;
break;
}
if(ssp_init) // only try to write value if spi is already running
setAccRange();
return HW_ERR_OK;
}
//------------------------------------------------------------------------------
//! HWStatus_t MEMS_BMC050_init_Accelerometer(void)
//------------------------------------------------------------------------------
//! @brief initializes accelerometer part of BMC50/150 sensor with default
//! settings. If not done yet SSP is also initialized
//------------------------------------------------------------------------------
//! @return one of the default HW_ERR codes
//------------------------------------------------------------------------------
HWStatus_t MEMS_BMC050_init_Accelerometer(void){
u8_t wBuff[bufsize];
u8_t rBuff[bufsize];
if(!ssp_init){
if(initSSP() != HW_ERR_OK)
return HW_ERR_RESOURCE;
}
if(!data_init)
initData();
// this initializes the accelerometer
// write register 34h: spi 4-wire
wAccReg(0x34, 0x00);
// write register 0Fh: G-range
setAccRange();
AccBWPwrConfig.cmd=0x10;
AccBWPwrConfig.Reg_0x10.bandwidth = 0x4;
AccBWPwrConfig.Reg_0x11.lowpower_en=0;
AccBWPwrConfig.Reg_0x11.sleep_dur=0;
AccBWPwrConfig.Reg_0x11.suspend=0;
rwACC((u8_t*)&AccBWPwrConfig, rBuff, 3);
// write register 13h: Filtered Data & Shadow on
wAccReg(0x13, 0x00);
readAccIRQConfig();
// disable desired interrupt, change parameters, wait for about 1[ms],
// enable the desired interrupt
AccIRQConfig.Reg_0x2C.orient_blocking = 3;
AccIRQConfig.Reg_0x2C.orient_hyst = 3;
wAccReg(0x2C, *((u8_t*)&AccIRQConfig.Reg_0x2C));
AccIRQConfig.Reg_0x2F.flat_hold_time = 0;
wAccReg(0x2F, *((u8_t*)&AccIRQConfig.Reg_0x2F));
AccIRQConfig.int_map_01 = 0xC0;
wAccReg(0x19, AccIRQConfig.int_map_01);
AccIRQConfig.int_map_02 = 1;
wAccReg(0x1A, AccIRQConfig.int_map_02);
if (cfg_data.Acc.flags & ACC_USE_EEPROM_FILT_COMPENSATION_VALUES){
BMC050_AccCompensate.AccelerationCompFilt_X = cfg_data.Acc.cmp_filt_x;
BMC050_AccCompensate.AccelerationCompFilt_Y = cfg_data.Acc.cmp_filt_y;
BMC050_AccCompensate.AccelerationCompFilt_Z = cfg_data.Acc.cmp_filt_z;
wAccReg(0x38, BMC050_AccCompensate.AccelerationCompFilt_X);
wAccReg(0x39, BMC050_AccCompensate.AccelerationCompFilt_Y);
wAccReg(0x3A, BMC050_AccCompensate.AccelerationCompFilt_Z);
}
if (cfg_data.Acc.flags & ACC_USE_EEPROM_RAW_COMPENSATION_VALUES){
BMC050_AccCompensate.AccelerationComp_X = cfg_data.Acc.cmp_raw_x;
BMC050_AccCompensate.AccelerationComp_Y = cfg_data.Acc.cmp_raw_y;
BMC050_AccCompensate.AccelerationComp_Z = cfg_data.Acc.cmp_raw_z;
wAccReg(0x3B, BMC050_AccCompensate.AccelerationComp_X);
wAccReg(0x3C, BMC050_AccCompensate.AccelerationComp_Y);
wAccReg(0x3D, BMC050_AccCompensate.AccelerationComp_Z);
}
AccIRQConfig.Reg_0x20.int1_lvl = 1;
AccIRQConfig.Reg_0x20.int2_lvl = 1;
AccIRQConfig.Reg_0x21.latch_int = 0xC;
AccIRQConfig.Reg_0x21.reset_int = 1;
wBuff[0] = 0x20; // command: write register 20h
wBuff[1] = *((u8_t*)&AccIRQConfig.Reg_0x20);
wBuff[2] = *((u8_t*)&AccIRQConfig.Reg_0x21);
rwACC(wBuff, rBuff, 3);
/* @ ToDo check if necessary */
Wait_Usec(2000);
AccIRQConfig.Reg_0x16.orient_en = 1;
AccIRQConfig.Reg_0x16.flat_en = 1;
wAccReg(0x16, *((u8_t*)&AccIRQConfig.Reg_0x16));
return HW_ERR_OK;
}
/**
*@brief 初期化
*/
void LSM303DLHC::reset(void) {
//_i2c->address(_Accaddr);
writeByte(_Accaddr,CTRL_REG1_A,0x27);
struct timespec ts;
ts.tv_sec = 0;
ts.tv_nsec = 200000000;
nanosleep(&ts, NULL);
setAccRange(_Ascale);
writeByte(_Magnaddr,CRA_REG_M,0x14);
nanosleep(&ts, NULL);
//writeByte(_Magnaddr,CRB_REG_M,0x20);
writeByte(_Magnaddr,MR_REG_M,0x00);
nanosleep(&ts, NULL);
setMagnRange(_Mscale);
nanosleep(&ts, NULL);
const double count = 10;
double ax,ay,az;
lastAX = 0;
lastAY = 0;
lastAZ = 0;
for(int i=0;i < count;i++)
{
getAccData(ax,ay,az);
lastAX += ax/count;
lastAY += ay/count;
lastAZ += az/count;
usleep(10000);
}
double mx,my,mz;
lastMX = 0;
lastMY = 0;
lastMZ = 0;
for(int i=0;i < count;i++)
{
getMagnData(mx,my,mz);
lastMX += mx/count;
lastMY += my/count;
lastMZ += mz/count;
usleep(10000);
}
}
