int main(int argc, char** argv){
parse_args(argc, argv);
if(print_usage_flag) {
print_usage();
return 0;
}
init();
short accel[3], gyro[3], sensors[1];
long quat[4];
unsigned long timestamp;
unsigned char more[0];
struct pollfd fdset[1];
char buf[1];
// File descriptor for the GPIO interrupt pin
int gpio_fd = open(GPIO_INT_FILE, O_RDONLY | O_NONBLOCK);
// Create an event on the GPIO value file
memset((void*)fdset, 0, sizeof(fdset));
fdset[0].fd = gpio_fd;
fdset[0].events = POLLPRI;
while (1){
// Blocking poll to wait for an edge on the interrupt
if(!no_interrupt_flag) {
poll(fdset, 1, -1);
}
if (no_interrupt_flag || fdset[0].revents & POLLPRI) {
// Read the file to make it reset the interrupt
if(!no_interrupt_flag) {
read(fdset[0].fd, buf, 1);
}
int fifo_read = dmp_read_fifo(gyro, accel, quat, ×tamp, sensors, more);
if (fifo_read != 0) {
//printf("Error reading fifo.\n");
continue;
}
float angles[NOSENTVALS];
rescale_l(quat, angles+9, QUAT_SCALE, 4);
// rescale the gyro and accel values received from the IMU from longs that the
// it uses for efficiency to the floats that they actually are and store these values in the angles array
rescale_s(gyro, angles+3, GYRO_SCALE, 3);
rescale_s(accel, angles+6, ACCEL_SCALE, 3);
// turn the quaternation (that is already in angles) into euler angles and store it in the angles array
euler(angles+9, angles);
if(!silent_flag && verbose_flag) {
printf("Yaw: %+5.1f\tPitch: %+5.1f\tRoll: %+5.1f\n", angles[0]*180.0/PI, angles[1]*180.0/PI, angles[2]*180.0/PI);
}
// send the values in angles over UDP as a string (in udp.c/h)
if(!no_broadcast_flag) {
udp_send(angles, 13);
}
}
}
}
int main(int argc, char **argv){
init();
short accel[3], gyro[3], sensors[1];
long quat[4];
unsigned long timestamp;
unsigned char more[0];
time_t sec, current_time; // set to the time before calibration
int running = 0; // set to 1 once the calibration has been done
time(&sec);
printf("Read system time\n");
while (1){
short status;
mpu_get_int_status(&status);
if (! (status & MPU_INT_STATUS_DMP) ){
continue;
}
int fifo_read = dmp_read_fifo(gyro, accel, quat, ×tamp, sensors, more);
if (fifo_read != 0) {
printf("Error reading fifo.\n");
}
if (fifo_read == 0 && sensors[0] && running) {
float angles[NOSENTVALS];
float temp_quat[4];
rescale_l(quat, temp_quat, QUAT_SCALE, 4);
if (!quat_offset[0]) {
// check if the IMU has finished calibrating
if(abs(last_quat[1]-temp_quat[1]) < THRESHOLD) {
// the IMU has finished calibrating
int i;
quat_offset[0] = temp_quat[0]; // treat the w value separately as it does not need to be reversed
for(i=1;i<4;++i){
quat_offset[i] = -temp_quat[i];
}
}
else {
memcpy(last_quat, temp_quat, 4*sizeof(float));
}
}
else {
q_multiply(quat_offset, temp_quat, angles+9); // multiply the current quaternstion by the offset caputured above to re-zero the values
// rescale the gyro and accel values received from the IMU from longs that the
// it uses for efficiency to the floats that they actually are and store these values in the angles array
rescale_s(gyro, angles+3, GYRO_SCALE, 3);
rescale_s(accel, angles+6, ACCEL_SCALE, 3);
// turn the quaternation (that is already in angles) into euler angles and store it in the angles array
euler(angles+9, angles);
printf("Yaw: %+5.1f\tRoll: %+5.1f\tPitch: %+5.1f\n", angles[0]*180.0/PI, angles[1]*180.0/PI, angles[2]*180.0/PI);
// send the values in angles over UDP as a string (in udp.c/h)
udp_send(angles, 13);
}
}
else {
time(¤t_time);
// check if more than CALIBRATION_TIME seconds has passed since calibration started
if(abs(difftime(sec, current_time)) > CALIBRATION_TIME) {
printf("Calibration time up...\n");
// allow all of the calculating, broadcasting and offset code from above to run
running = 1;
}
else
printf("Calibrating...\n");
}
}
}
