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"); } } }