void readADXL345(int i2c)
{
char buf[8];
static int reported = 0;
selectDevice(i2c, ADXL345_I2C_ADDR, "ADXL345");
writeToDevice(i2c, "\x32", 1);
if (read(i2c, buf, 6) != 6)
{
if (!reported)
{
fprintf(stderr, "Unable to read from ADXL345\n");
reported = 1;
}
}
else
{
reported = 0;
ACC_ORIENTATION (
((buf[1]<<8) | buf[0]),
((buf[3]<<8) | buf[2]),
((buf[5]<<8) | buf[4]) );
}
}
/* Sensors Read */
void SensorReadACC()
{
#if STACK_ACC
int16_t rawACC[3];
#ifdef MPU6050
MPU6050_getAcceleration(&rawACC[0],&rawACC[1], &rawACC[2]);
#endif
ACC_ORIENTATION(rawACC[0],rawACC[1],rawACC[2]);
#endif
}
void acc_query(void) {
uint8_t buf[6];
twi_start(ACC_ADDRESS<<1);
twi_write(0x32);
twi_start(ACC_ADDRESS<<1 | 1);
for (uint8_t i=0; i<sizeof(buf); i++) {
buf[i] = twi_read(i<sizeof(buf)-1);
}
twi_stop();
ACC_ORIENTATION(buf[1]<<8 | buf[0],
buf[3]<<8 | buf[2],
buf[5]<<8 | buf[4]);
}
void readacc(void)
{
unsigned char data[6];
lib_i2c_readdata( MC3210_ADDRESS, 0x0D, (unsigned char *)&data, 6);
// convert readings to fixedpointnum (in g's)
// Sensor output is 14 bit signed, sign extended to 16 bit, full scale +/- 8g
// So we have 13 bit fractional part, need to shift that to FIXEDPOINTSHIFT and
// take 8g into account (further shift by 3 bits).
// This only works if FIXEDPOINTSHIFT >= 10
ACC_ORIENTATION(global.acc_g_vector,
((fixedpointnum)((int16_t)((data[1] << 8) | data[0]))) << (FIXEDPOINTSHIFT - 13 + 3),
((fixedpointnum)((int16_t)((data[3] << 8) | data[2]))) << (FIXEDPOINTSHIFT - 13 + 3),
((fixedpointnum)((int16_t)((data[5] << 8) | data[4]))) << (FIXEDPOINTSHIFT - 13 + 3));
}
/* Sensors Read */
void SensorReadACC()
{
#if STACK_ACC
int16_t rawACC[3];//,rawGYRO[3];
#if defined(MPU6050) || defined(MPU6500)
MPU6050_getAcceleration(&rawACC[0],&rawACC[1], &rawACC[2]);
//MPU6050_getMotion6(&rawACC[0],&rawACC[1], &rawACC[2],&rawGYRO[0],&rawGYRO[1], &rawGYRO[2]);
#else
rawACC[0] = readRawAccelX();
rawACC[1] = readRawAccelY();
rawACC[2] = readRawAccelZ();
#endif
ACC_ORIENTATION(rawACC[0],rawACC[1],rawACC[2]);
//GYRO_ORIENTATION(rawGYRO[0],rawGYRO[1],rawGYRO[2]);
#endif
}
