uint8_t LSM9DS1::readTemp()
{
uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp
uint8_t status = xgReadBytes(OUT_TEMP_L, temp, 2); // Read 2 bytes, beginning at OUT_TEMP_L
if (status)
temperature = ((int16_t)temp[1] << 8) | temp[0];
return status;
}
void readAccel1v1(accel *a)
{
uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp
int index = 0;
xgReadBytes(OUT_X_L_XL, temp, 6); // Read 6 bytes, beginning at OUT_X_L_XL
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)
{
ax -= aBiasRaw[X_AXIS];
ay -= aBiasRaw[Y_AXIS];
az -= aBiasRaw[Z_AXIS];
}
ax = calcAccel(ax);
ay = calcAccel(ay);
az = calcAccel(az);
if((accelMeasurementsNum > 99) && (measurementsLSMRead == 0))
{
measurementsLSMRead = 1;
/*a[accelMeasurementsNum].ax = ax;
a[accelMeasurementsNum].ay = ay;
a[accelMeasurementsNum].az = az;*/
return;
}
a[accelMeasurementsNum].ax = ax;
a[accelMeasurementsNum].ay = ay;
a[accelMeasurementsNum].az = az;
accelMeasurementsNum++;
/*a[accelMeasurementsNum].ax = ax;
a[accelMeasurementsNum].ay = ay;
a[accelMeasurementsNum].az = az;
accelMeasurementsNum++;*/
toAscii(ax, &index);
toAscii(ay, &index);
toAscii(az, &index);
/*lk[0] = '0' + ax;
lk[1] = '0' + ay;
lk[2] = '0' + az;*/
for(int k = 0; k < 6; k++)
{
accelerationXYZ[k] = 0;
}
}
int16_t readAccel(lsm9ds1_axis axis)
{
uint8_t temp[2];
int16_t value;
xgReadBytes(OUT_X_L_XL + (2 * axis), temp, 2);
value = (temp[1] << 8) | temp[0];
if (_autoCalc)
value -= aBiasRaw[axis];
return value;
}
int16_t LSM9DS1::readGyro(lsm9ds1_axis axis)
{
uint8_t temp[2];
int16_t value;
xgReadBytes(OUT_X_L_G + (2 * axis), temp, 2);
value = (temp[1] << 8) | temp[0];
if (_autoCalc)
value -= gBiasRaw[axis];
return value;
}
void LSM9DS1::readGyro()
{
uint8_t temp[6]; // We'll read six bytes from the gyro into temp
xgReadBytes(OUT_X_L_G, temp, 6); // Read 6 bytes, beginning at OUT_X_L_G
gx = (temp[1] << 8) | 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)
{
gx -= gBiasRaw[X_AXIS];
gy -= gBiasRaw[Y_AXIS];
gz -= gBiasRaw[Z_AXIS];
}
}
void LSM9DS1::readAccel()
{
uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp
xgReadBytes(OUT_X_L_XL, temp, 6); // Read 6 bytes, beginning at OUT_X_L_XL
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)
{
ax -= aBiasRaw[X_AXIS];
ay -= aBiasRaw[Y_AXIS];
az -= aBiasRaw[Z_AXIS];
}
}
int16_t LSM9DS1::readAccel(lsm9ds1_axis axis)
{
uint8_t temp[2];
int16_t value;
uint8_t status = xgReadBytes(OUT_X_L_XL + (2 * axis), temp, 2);
if (status)
{
value = (temp[1] << 8) | temp[0];
if (_autoCalc)
value -= aBiasRaw[axis];
return value;
}
else
return 0;
}
void readGyro1(void)
{
/*for(int g = 0; g < 3; g++){*/
uint8_t temp[6]; // We'll read six bytes from the gyro into temp
xgReadBytes(OUT_X_L_G, temp, 6); // Read 6 bytes, beginning at OUT_X_L_G
gx = ((int8_t)temp[1] << 8) | (int8_t)temp[0]; // Store x-axis values into gx
//gx = ((int8_t)temp[0] << 8) | (int8_t)temp[1]; // Store x-axis values into gx
gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy
//gy = (temp[2] << 8) | temp[3];
gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz
//gz = (temp[4] << 8) | temp[5];
/*if(g == 2)
{
int flu = 0;
}*/
if (_autoCalc)
{
gx -= gBiasRaw[X_AXIS];
gy -= gBiasRaw[Y_AXIS];
gz -= gBiasRaw[Z_AXIS];
}
gx = calcGyro(gx);
gy = calcGyro(gy);
gz = calcGyro(gz);
/*poms.gxs[g] = gx;
poms.gys[g] = gy;
poms.gzs[g] = gz;*/
for(int k = 0; k < 6; k++)
{
accelerationXYZ[k] = 0;
}
//}
}
void readTemp(void)
{
uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp
xgReadBytes(OUT_TEMP_L, temp, 2); // Read 2 bytes, beginning at OUT_TEMP_L
temperature = ((int16_t)temp[1] << 8) | temp[0];
}
