static msg_t sensorsRead (void*arg)
{
float acc_x, acc_y, acc_z;
float gyro_x, gyro_y, gyro_z;
uint8_t temp;
/*sensors initializing*/
gyroInit (&I2CD2, IMU01A_GYRO);
accInit (&I2CD2, IMU01A_ACC);
chprintf ((BaseSequentialStream *)&SD2, "Configuration done\n\r");
while (true)
{
/*gyroscope and temperature reading*/
gyroRead (&I2CD2, IMU01A_GYRO, &gyro_x, &gyro_y, &gyro_z);
tempRead (&I2CD2, IMU01A_GYRO, &temp);
/*accelerometer reading*/
accRead (&I2CD2, IMU01A_ACC, &acc_x, &acc_y, &acc_z);
/*printing out magnetometer values*/
chprintf ((BaseSequentialStream *)&SD2, "%f gX %f gY %f gZ ", gyro_x, gyro_y, gyro_z);
/*printing out accelerometer and temperature values*/
chprintf ((BaseSequentialStream *)&SD2, "%f aX %f aY %f aZ ", acc_x, acc_y, acc_z);
chprintf ((BaseSequentialStream *)&SD2, "%d T\n\r", temp);
}
}
void printRaw(void) {
Vector3f v;
accRead(&v);
printf("Ax: %f Ay: %f Az: %f\n", v.x, v.y, v.z);
gyroRead(&v);
printf("Gx: %f Gy: %f Gz: %f\n", v.x, v.y, v.z);
magRead(&v);
printf("Mx: %f My: %f Mz: %f\n", v.x, v.y, v.z);
}
uint8_t compassRead(int16_t *data)
{ int ii;
int16_t buf[4]; uint8_t *tt=(uint8_t *) &buf[0];
// data are LSB first no need for byte swap
gyroRead(tt); for(ii=0;ii<3;ii++) data[ii]=buf[ii];
#ifdef LSM303D
accRead(tt); for(ii=0;ii<3;ii++) data[3+ii]=buf[ii];
magRead(tt); for(ii=0;ii<3;ii++) data[6+ii]=buf[ii];
tempRead(tt); data[9]=buf[0];
#else
accRead(tt); for(ii=0;ii<3;ii++) data[3+ii]=buf[ii]>>4;
magRead(tt); for(ii=0;ii<3;ii++) data[6+ii]=ByteSwap(buf[ii]);//(int16_t)(tt[jj++]<<8 | tt[jj++]);
tempRead(tt); data[9]=ByteSwap(buf[0])>>4;// ((int16_t)(tt[0]<<8 | tt[1]))>>4;
#endif
//
return 1;
}
// Returns the angular difference in the horizontal plane between
// a default vector an north in degrees.
float getHeading()
{
// Default vector of x axis
float from[3] = { 1, 0, 0 };
// Get latest compass readings for calculations
accRead(&acc);
magRead(&mag);
return calcHeading(from, &acc, &mag);
}
void getAccel(int16_t *data)
{ int ii;
int16_t buf[4]; uint8_t *tt=(uint8_t *) &buf[0];
accRead(tt); for(ii=0;ii<3;ii++) data[3+ii]=buf[ii]>>4;
}
void getAccel(uint16_t *data){ accRead((uint8_t *)data);}
