short MPU9250_DMP::getIntStatus(void)
{
short status;
if (mpu_get_int_status(&status) == INV_SUCCESS)
{
return status;
}
return 0;
}
int data_ready(void)
{
short status;
if (mpu_get_int_status(&status) < 0) {
printk(KERN_WARNING "mpu_get_int_status() failed\n");
return 0;
}
return (status & (MPU_INT_STATUS_DATA_READY | MPU_INT_STATUS_DMP | MPU_INT_STATUS_DMP_0));
}
int data_ready()
{
short status;
if (mpu_get_int_status(&status) < 0) {
printf("mpu_get_int_status() failed\n");
return 0;
}
return (status == (MPU_INT_STATUS_DATA_READY/* | MPU_INT_STATUS_DMP | MPU_INT_STATUS_DMP_0*/));
}
int INVMPU9150::dmpFifoDataReady() {
short status;
// Get interrupt status
if (mpu_get_int_status(&status) < 0) {
return 0;
}
return (status = 0x0103);
}
int data_ready() {
short status;
if (mpu_get_int_status(&status) < 0) {
printf("mpu_get_int_status() failed\n");
return 0;
}
// debug
//if (status != 0x0103)
// fprintf(stderr, "%04X\n", status);
return (status == (MPU_INT_STATUS_DATA_READY | MPU_INT_STATUS_DMP | MPU_INT_STATUS_DMP_0));
}
int data_ready()
{
short status;
if (mpu_get_int_status(&status) < 0) {
printf("mpu_get_int_status() failed\n");
return 0;
}
// debug
//if (status != 0x0103)
// fprintf(stderr, "%04X\n", status);
return (status == 0x0103);
}
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");
}
}
}
boolean MPU9150Lib::read()
{
short intStatus;
int result;
short sensors;
unsigned char more;
unsigned long timestamp;
mpu_get_int_status(&intStatus); // get the current MPU state
if ((intStatus & MPU_INT_STATUS_DMP_0) == 0) // return false if definitely not ready yet
return false;
// get the data from the fifo
if ((result = dmp_read_fifo(m_rawGyro, m_rawAccel, m_rawQuaternion, ×tamp, &sensors, &more)) != 0) {
return false;
}
// got the fifo data so now get the mag data
if ((result = mpu_get_compass_reg(m_rawMag, ×tamp)) != 0) {
#ifdef MPULIB_DEBUG
Serial.print("Failed to read compass: ");
Serial.println(result);
return false;
#endif
}
// got the raw data - now process
m_dmpQuaternion[QUAT_W] = (float)m_rawQuaternion[QUAT_W]; // get float version of quaternion
m_dmpQuaternion[QUAT_X] = (float)m_rawQuaternion[QUAT_X];
m_dmpQuaternion[QUAT_Y] = (float)m_rawQuaternion[QUAT_Y];
m_dmpQuaternion[QUAT_Z] = (float)m_rawQuaternion[QUAT_Z];
MPUQuaternionNormalize(m_dmpQuaternion); // and normalize
MPUQuaternionQuaternionToEuler(m_dmpQuaternion, m_dmpEulerPose);
// *** Note mag axes are changed here to align with gyros: Y = -X, X = Y
if (m_useMagCalibration) {
m_calMag[VEC3_Y] = -(short)(((long)(m_rawMag[VEC3_X] - m_magXOffset) * (long)SENSOR_RANGE) / (long)m_magXRange);
m_calMag[VEC3_X] = (short)(((long)(m_rawMag[VEC3_Y] - m_magYOffset) * (long)SENSOR_RANGE) / (long)m_magYRange);
m_calMag[VEC3_Z] = (short)(((long)(m_rawMag[VEC3_Z] - m_magZOffset) * (long)SENSOR_RANGE) / (long)m_magZRange);
}
else {
m_calMag[VEC3_Y] = -m_rawMag[VEC3_X];
m_calMag[VEC3_X] = m_rawMag[VEC3_Y];
m_calMag[VEC3_Z] = m_rawMag[VEC3_Z];
}
// Scale accel data
if (m_useAccelCalibration) {
m_calAccel[VEC3_X] = -(short)(((long)m_rawAccel[VEC3_X] * (long)SENSOR_RANGE) / (long)m_accelXRange);
m_calAccel[VEC3_Y] = (short)(((long)m_rawAccel[VEC3_Y] * (long)SENSOR_RANGE) / (long)m_accelYRange);
m_calAccel[VEC3_Z] = (short)(((long)m_rawAccel[VEC3_Z] * (long)SENSOR_RANGE) / (long)m_accelZRange);
}
else {
m_calAccel[VEC3_X] = -m_rawAccel[VEC3_X];
m_calAccel[VEC3_Y] = m_rawAccel[VEC3_Y];
m_calAccel[VEC3_Z] = m_rawAccel[VEC3_Z];
}
dataFusion();
return true;
}
