uint16_t LSM9DS0_begin( LSM9DS0_t* lsm_t, LMS9DS0_Init_t* init_t )
{
#if (DEBUG0==1)
uint8_t test[8];
#endif
uint8_t gTest, xmTest;
if(init_t==NULL)
init_t = &LMS_Device_Defaults;
// Store the given scales in class variables. These scale variables
// are used throughout to calculate the actual g's, DPS,and Gs's.
lsm_t->gScale = init_t->gScl;
lsm_t->aScale = init_t->aScl;
lsm_t->mScale = init_t->mScl;
// Once we have the scale values, we can calculate the resolution
// of each sensor. That's what these functions are for. One for each sensor
calcgRes(lsm_t); // Calculate DPS / ADC tick, stored in gRes variable
calcmRes(lsm_t); // Calculate Gs / ADC tick, stored in mRes variable
calcaRes(lsm_t); // Calculate g / ADC tick, stored in aRes variable
// Now, initialize our hardware interface.
#if(LSM_I2C_SUPPORT==1)
if (lsm_t->interfaceMode == MODE_I2C) // If we're using I2C
LSM_initI2C(); // Initialize I2C
else if (lsm_t->interfaceMode == MODE_SPI) // else, if we're using SPI
#endif
//LSM_initSPI(); // Initialize SPI
// To verify communication, we can read from the WHO_AM_I register of
// each device. Store those in a variable so we can return them.
gTest = gReadByte(lsm_t,WHO_AM_I_G); // Read the gyro WHO_AM_I
xmTest = xmReadByte(lsm_t,WHO_AM_I_XM); // Read the accel/mag WHO_AM_I
// Gyro initialization stuff:
initGyro(lsm_t); // This will "turn on" the gyro. Setting up interrupts, etc.
setGyroODR(lsm_t,init_t->gODR); // Set the gyro output data rate and bandwidth.
setGyroScale(lsm_t,lsm_t->gScale); // Set the gyro range
// Accelerometer initialization stuff:
initAccel(lsm_t); // "Turn on" all axes of the accel. Set up interrupts, etc.
setAccelODR(lsm_t,init_t->aODR); // Set the accel data rate.
setAccelScale(lsm_t,lsm_t->aScale); // Set the accel range.
// Magnetometer initialization stuff:
initMag(lsm_t); // "Turn on" all axes of the mag. Set up interrupts, etc.
setMagODR(lsm_t,init_t->mODR); // Set the magnetometer output data rate.
setMagScale(lsm_t,lsm_t->mScale); // Set the magnetometer's range.
LSM9DS0_Update_Ctrl(lsm_t);
lsm_t->update=UPDATE_ON_SET;
#if (DEBUG0==1)
I2CreadBytes(lsm_t->gAddress,CTRL_REG1_G,test,5);
I2CreadBytes(lsm_t->xmAddress,CTRL_REG0_XM,test,7);
#endif
// Once everything is initialized, return the WHO_AM_I registers we read:
return (xmTest << 8) | gTest;
}
void readAccelToSensor(accel *pomiar)
{
uint8_t i = 0; //licznik dla czytania
uint8_t temp[6]; // We'll read six bytes from the gyro into temp
uint8_t subAddr = OUT_X_L_XL;
while(i < 6)
{
subAddr = OUT_X_L_XL;
subAddr = subAddr + i;
temp[i] = I2CreadBytes(_xgAddress, subAddr, NULL, 0);
i++;
}
ax = (temp[1] << 8) | temp[0]; // Store x-axis values into ax
ay = (temp[3] << 8) | temp[2]; // Store y-axis values into ay
az = (temp[5] << 8) | temp[4]; // Store z-axis values into az
if (_autoCalc) //kalibracja
{
ax -= aBiasRaw[X_AXIS];
ay -= aBiasRaw[Y_AXIS];
az -= aBiasRaw[Z_AXIS];
}
ax = calcAccel(ax);
ay = calcAccel(ay);
az = calcAccel(az);
pomiar->ax = ax;
pomiar->ay = ay;
pomiar->az = az;
}
void readGyro1(void)
{
uint8_t i = 0; //licznik dla czytania
uint8_t temp[6]; // We'll read six bytes from the gyro into temp
uint8_t subAddr = OUT_X_L_G;
while(i < 6)
{
subAddr = OUT_X_L_G;
subAddr = subAddr + i;
temp[i] = I2CreadBytes(_xgAddress, subAddr, NULL, 0);
delay(DELAY);
i++;
}
gx = ((int8_t)temp[1] << 8) | (int8_t)temp[0]; // Store x-axis values into gx
gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy
gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz
if (_autoCalc) //kalibracja
{
gx -= gBiasRaw[X_AXIS];
gy -= gBiasRaw[Y_AXIS];
gz -= gBiasRaw[Z_AXIS];
}
/*gx = calcGyro(gx);
gy = calcGyro(gy);
gz = calcGyro(gz);*/
}
void ITG3701::updateGyro(){
uint8_t rawData[6]; // x/y/z gyro register data stored here
I2CreadBytes(ITG3701_ADDRESS, ITG3701_GYRO_XOUT_H, &rawData[0], 6); // Read the six raw data registers sequentially into data array
x = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value
y = ((int16_t)rawData[2] << 8) | rawData[3] ;
z = ((int16_t)rawData[4] << 8) | rawData[5] ;
}
void LSM330D::xmReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
{
// Whether we're using I2C or SPI, read multiple bytes using the
// accelerometer-specific I2C address or SPI CS pin.
if (interfaceMode == MODE_I2C)
I2CreadBytes(xmAddress, subAddress, dest, count);
}
void LSM9DS0::gReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
{
// Whether we're using I2C or SPI, read multiple bytes using the
// gyro-specific I2C address or SPI CS pin.
if (interfaceMode == MODE_I2C)
I2CreadBytes(gAddress, subAddress, dest, count);
else if (interfaceMode == MODE_SPI)
SPIreadBytes(gAddress, subAddress, dest, count);
}
/* ************************************************************************** */
static void xmReadBytes(stLSM9DS0_t * stThis, uint8_t subAddress, uint8_t * dest, uint8_t count)
{
// Whether we're using I2C or SPI, read multiple bytes using the
// accelerometer-specific I2C address or SPI CS pin.
if (stThis->interfaceMode == MODE_I2C)
I2CreadBytes(stThis, stThis->xmAddress, subAddress, dest, count);
else if (stThis->interfaceMode == MODE_SPI)
SPIreadBytes(stThis, stThis->xmAddress, subAddress, dest, count);
}
void xgReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
{
// Whether we're using I2C or SPI, read multiple bytes using the
// gyro-specific I2C address or SPI CS pin.
if (settings.device.commInterface == IMU_MODE_I2C)
I2CreadBytes(_xgAddress, subAddress, dest, count);
//I2CreadBytes1(_xgAddress, subAddress, dest, count);
else if (settings.device.commInterface == IMU_MODE_SPI)
SPIreadBytes(_xgAddress, subAddress, dest, count);
}
void xmReadBytes(LSM9DS0_t* lsm_t, uint8_t subAddress, uint8_t * dest, uint8_t count)
{
// Whether we're using I2C or SPI, read multiple bytes using the
// accelerometer-specific I2C address or SPI CS pin.
if (lsm_t->interfaceMode == MODE_I2C)
I2CreadBytes(lsm_t->xmAddress, subAddress, dest, count);
//else if (lsm_t->interfaceMode == MODE_SPI)
//SPIreadBytes(lsm_t->xmAddress, subAddress, dest, count, 'a');
}
void gReadBytes(LSM9DS0_t* lsm_t, uint8_t subAddress, uint8_t * dest, uint8_t count)
{
// Whether we're using I2C or SPI, read multiple bytes using the
// gyro-specific I2C address or SPI CS pin.
#if(LSM_I2C_SUPPORT==1)
if (lsm_t->interfaceMode == MODE_I2C)
I2CreadBytes(lsm_t->gAddress, subAddress, dest, count);
else if (lsm_t->interfaceMode == MODE_SPI)
#endif
SPIreadBytes(lsm_t->gAddress, subAddress, dest, count,'g');
}
int16_t readMag(lsm9ds1_axis axis)
{
uint8_t temp[2];
int i = 0;
uint8_t subAddress = OUT_X_L_M + (2 * axis);
while(i < 2)
{
subAddress = subAddress + i;
temp[i] = I2CreadBytes(_mAddress, subAddress, NULL, 0);
i++;
}
int16_t value = (temp[1] << 8) | temp[0];
return value;
}
int16_t readTemp(void)
{
uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp
int i = 0;
uint8_t subAddress = OUT_TEMP_L;
while(i < 2)
{
subAddress = subAddress + i;
temp[i] = I2CreadBytes(_xgAddress, subAddress, NULL, 0);
i++;
}
//xgReadBytes(OUT_TEMP_L, temp, 2); // Read 2 bytes, beginning at OUT_TEMP_L
temperature = (temp[1] << 8) | temp[0];
return temperature;
}
int16_t readGyro(lsm9ds1_axis axis)
{
uint8_t temp[2];
int16_t value;
int i = 0;
uint8_t subAddress = OUT_X_L_G + (2 * axis);
while(i < 2)
{
subAddress = subAddress + i;
temp[i] = I2CreadBytes(_xgAddress, subAddress, NULL, 0);
i++;
}
value = (temp[1] << 8) | temp[0];
if (_autoCalc)
value -= gBiasRaw[axis];
return value;
}
void readMag1(void)
{
//for(int kl = 0; kl < 10; kl++){
uint8_t temp[6]; // We'll read six bytes from the mag into temp
uint8_t subAddress = OUT_X_L_M;
uint8_t i = 0;
while(i < 6)
{
temp[i] = I2CreadBytes(_mAddress, subAddress, NULL, 0);
i++;
}
mx = (temp[1] << 8) | temp[0]; // Store x-axis values into mx
my = (temp[3] << 8) | temp[2]; // Store y-axis values into my
mz = (temp[5] << 8) | temp[4]; // Store z-axis values into mz
/*mx = calcMag(mx);
my = calcMag(my);
mz = calcMag(mz);*?
/*}*/
}
