void imuConfig(void) {
i2cOpen(); // TODO: catch error
// configure accelerometer
i2cSetAddress(ACCEL_I2C_ADDR);
i2cWriteSingle(CTRL_REG1_A, 0b01100111); // 200Hz update, XYZ enabled
i2cWriteSingle(CTRL_REG2_A, 0x00);
i2cWriteSingle(CTRL_REG3_A, 0x00);
i2cWriteSingle(CTRL_REG4_A, 0b00111000); // block update, little-endian, FS = 2G, high-res mode
i2cWriteSingle(CTRL_REG5_A, 0x00);
i2cWriteSingle(CTRL_REG6_A, 0x00);
// configure magnetometer
i2cSetAddress(MAG_I2C_ADDR);
i2cWriteSingle(CRA_REG_M, 0b00011100); // temp sensor off, 220Hz update rate
i2cWriteSingle(CRB_REG_M, 0b00100000); // set full-scale range, 1.3 gauss
i2cWriteSingle(MR_REG_M, 0b00000000); // continuous conversion mode
// configure gyroscope
i2cSetAddress(GYRO_I2C_ADDR);
i2cWriteSingle(CTRL_REG1_G, 0b10001111); // 380Hz update, 100Hz LPF cutoff, XYZ enabled
//i2cWriteSingle(CTRL_REG2_G, 0b00001000); // normal mode, 0.045 Hz HPF cutoff
i2cWriteSingle(CTRL_REG4_G, 0b00000000); // continuous update, little-endian, 250dps
i2cWriteSingle(CTRL_REG5_G, 0b00000010); // high-pass filter disabled, output LPFed
sem_init(&sem_imu_trigger, 0, 0);
pthread_create(&_thread_imu, NULL, imuUpdate, NULL);
}
// Reads x, y and z gyroscope registers
void Read_Gyro()
{
unsigned char buff[6];
unsigned short raw[3];
unsigned char reg = GYRO_DATAREG;
int i = 0;
// Select device
i2cSetAddress(GYRO_ADDRESS);
// Read 6 bytes
while(i<6&®<GYRO_DATAREG+6){
buff[i] = i2cReadByte(reg);
//printf("gyro[%x]:%x ",reg,buff[i]);
//usleep(5000);
reg++;
i++;
}
if (i == 6) // All bytes received?
{
//ints are stored as 2-byte 2s-complements on an arduino so casting to int is sufficent to
//retrieve 16bit 2s-complement values from sensor registers on an arduino.
//gyro1[0] = -1 * ((((int) buff[2]) << 8) | buff[3]); // X axis (internal sensor -y axis)
//gyro1[1] = -1 * ((((int) buff[0]) << 8) | buff[1]); // Y axis (internal sensor -x axis)
//gyro1[2] = -1 * ((((int) buff[4]) << 8) | buff[5]); // Z axis (internal sensor -z axis)
// MSB first, X Y reversed
raw[0] = ((buff[2]) << 8) | buff[3]; // X axis (internal sensor -y axis)
raw[1] = ((buff[0]) << 8) | buff[1]; // Y axis (internal sensor -x axis)
raw[2] = ((buff[4]) << 8) | buff[5]; // Z axis (internal sensor -z axis)
//Manual 2s-complement conversion on ARM v8
//nasty bit of code can be optimized.
//TODO: make it branch-less
int k;
for(k=0;k<3;k++){
//Convert from twos complement
if((raw[k] >> 15) == 1){
raw[k] = ~raw[k] + 1;
gyro[k] = raw[k];
gyro[k] *= -1;
//printf("negative");
}else{
gyro[k] = raw[k];
}
//XXXX is the maximum value of an X-bit signed register
//TODO: Scale based on Max value being read.
//gyro2[k] = (float)16 * (gyro2[k]/(0x1FF));
//printf("\n%f\n",gyro[k]);
}
int i2cReadByte(int i2cFD, unsigned char add)
{
int byte;
i2cSetAddress(i2cFD, add);
if ((byte = i2c_smbus_read_byte(i2cFD)) < 0)
{
return -1;
}
return byte;
}
void sensorRead(imu_data *pimu_data) {
union {
char as_char[6];
int16_t as_int16[3];
} raw;
// read and scale accelerometer values
i2cSetAddress(ACCEL_I2C_ADDR);
i2cReadBlock(ACCEL_DATA_BASE_ADD, raw.as_char, 6);
pimu_data->accel[0] = raw.as_int16[0] * ACCEL_RES;
pimu_data->accel[1] = raw.as_int16[1] * ACCEL_RES;
pimu_data->accel[2] = raw.as_int16[2] * ACCEL_RES;
// read and scale magnetometer values - returned as big-endien
i2cSetAddress(MAG_I2C_ADDR);
i2cReadBlock(MAG_DATA_BASE_ADD, raw.as_char, 6);
pimu_data->mag[0] = (int16_t) (raw.as_char[0] << 8 | raw.as_char[1]) / MAG_SEN;
pimu_data->mag[1] = (int16_t) (raw.as_char[4] << 8 | raw.as_char[5]) / MAG_SEN;
pimu_data->mag[2] = (int16_t) (raw.as_char[2] << 8 | raw.as_char[3]) / MAG_SENZ;
// read and scale gyroscope values
i2cSetAddress(GYRO_I2C_ADDR);
i2cReadBlock(GYRO_DATA_BASE_ADD, raw.as_char, 6);
pimu_data->gyro[0] = raw.as_int16[0] * GYRO_RES;
pimu_data->gyro[1] = raw.as_int16[1] * GYRO_RES;
pimu_data->gyro[2] = raw.as_int16[2] * GYRO_RES;
// printf("IMU data: \t%.3f\t%.3f\t%.3f", pimu_data->accel[0], pimu_data->accel[1], pimu_data->accel[2]);
// printf("\t\t%.3f\t%.3f\t%.3f", pimu_data->mag[0], pimu_data->mag[1], pimu_data->mag[2]);
// printf("\t\t%.3f\t%.3f\t%.3f\n", pimu_data->gyro[0], pimu_data->gyro[1], pimu_data->gyro[2]);
}
int main(int argc, char** argv)
{
int count;
uint8_t red = 0;
uint8_t green = 0;
uint8_t blue = 0;
if (argc > 1)
{
for (count = 1; count < argc; count++)
{
printf("argv[%d] = %s\n", count, argv[count]);
if (count == 1) red = atoi(argv[1]);
if (count == 2) green = atoi(argv[2]);
if (count == 3) blue = atoi(argv[3]);
}
}
else
{
printf("please set colour eg 255 0 15\n");
return 0;
}
// open Linux I2C device
i2cOpen();
// set address of the controller
i2cSetAddress(0x53);
writeValue(red++, green++, blue++);
// close Linux I2C device
i2cClose();
return 0;
}
