void acquire_accel_values()
{
uint8_t data[8];
read_accel_data(0x00, 8, data);
raw_accel_values.x = ((uint16_t)data[3] << 8) + (data[2] & ~1);
raw_accel_values.y = ((uint16_t)data[5] << 8) + (data[4] & ~1);
raw_accel_values.z = ((uint16_t)data[7] << 8) + (data[6] & ~1);
accel_fir.sum.x -= accel_fir.data[accel_fir.pos].x;
accel_fir.sum.y -= accel_fir.data[accel_fir.pos].y;
accel_fir.sum.z -= accel_fir.data[accel_fir.pos].z;
accel_fir.sum.x += raw_accel_values.x;
accel_fir.sum.y += raw_accel_values.y;
accel_fir.sum.z += raw_accel_values.z;
accel_fir.data[accel_fir.pos].x = raw_accel_values.x;
accel_fir.data[accel_fir.pos].y = raw_accel_values.y;
accel_fir.data[accel_fir.pos].z = raw_accel_values.z;
accel_values.x = accel_fir.sum.x / ACCEL_FILTER_LENGTH;
accel_values.y = accel_fir.sum.y / ACCEL_FILTER_LENGTH;
accel_values.z = accel_fir.sum.z / ACCEL_FILTER_LENGTH;
accel_fir.pos = (accel_fir.pos + 1) % ACCEL_FILTER_LENGTH;
}
int main(){
imu_data_t data; //struct to hold new data
if(initialize_cape()){
printf("ERROR: failed to initialize_cape\n");
return -1;
}
// use defaults for now, except also enable magnetometer.
imu_config_t conf = get_default_imu_config();
conf.enable_magnetometer=1;
if(initialize_imu(&data, conf)){
printf("initialize_imu_failed\n");
return -1;
}
// print a header
printf("\n");
printf(" Accel XYZ(m/s^2) |");
printf(" Gyro XYZ (deg/s) |");
printf(" Mag Field XYZ(uT) |");
printf(" Temp (C)");
printf("\n");
//now just wait, print_data will run
while (get_state() != EXITING) {
printf("\r");
if(read_accel_data(&data)<0)
printf("read accel data failed\n");
else printf("%6.2f %6.2f %6.2f |", data.accel[0],\
data.accel[1],\
data.accel[2]);
if(read_gyro_data(&data)<0)
printf("read gyro data failed\n");
else printf("%6.1f %6.1f %6.1f |", data.gyro[0],\
data.gyro[1],\
data.gyro[2]);
if(read_mag_data(&data)<0){
printf("read mag data failed\n");
}
else printf("%6.1f %6.1f %6.1f |", data.mag[0],\
data.mag[1],\
data.mag[2]);
if(read_imu_temp(&data)<0){
printf("read temp data failed\n");
}
else printf(" %4.1f ", data.temp);
fflush(stdout);
usleep(100000);
}
power_off_imu();
cleanup_cape();
return 0;
}
