bool NiAnimation::calculateRotation(xn::UserGenerator &user_generator, XnSkeletonJoint joint, double &x, double &y, double &z, double m) { XnSkeletonJointOrientation o; MT_Quaternion q; user_generator.GetSkeletonCap().GetSkeletonJointOrientation(DEFAULT_USER, joint, o); if (o.fConfidence == 0) return false; jointOrientationToQuaternion(o, q); quaternionToEuler(q, x, y, z); x *= m; y *= m; z *= m; return true; }
int ms_update() { static unsigned int dt = 0; float x,y,z; if (!dmpReady) { printf("Error: DMP not ready!!\n"); return -1; } fifoCount = -1; err=dmp_read_fifo(g,a,_q,&sensors,&fifoCount); //gyro and accel can be null because of being disabled in the efeatures ms.count = fifoCount; if (fifoCount>1) { //printf("MPU fifo queue: %i!!!\n",fifoCount); do { fifoCount = -1; err=dmp_read_fifo(g,a,_q,&sensors,&fifoCount); //gyro and accel can be null because of being disabled in the efeatures } while (fifoCount>1); } if (err!=0 && fifoCount!=0) return -1; if (err && fifoCount==0) return 0; //no new data for(int i = 0; i < 4; ++i ) q[i] = (float)_q[i] / QUAT_SENS; quaternionToEuler( q, &x, &y, &z ); //euler and gyro must be in sync, i.e. MPU pitches left, euler decreases and gyro shows minus, etc //0 = yaw //1 = pitch //2 = roll ms.ypr[0] = rad2deg(z); ms.ypr[1] = rad2deg(y); ms.ypr[2] = -rad2deg(x); ms.gyro[0] = -(float)g[2] / GYRO_SENS; ms.gyro[1] = (float)g[1] / GYRO_SENS; ms.gyro[2] = (float)g[0] / GYRO_SENS; return 1; }
int mympu_update() { do { ret = dmp_read_fifo(gyro,NULL,q._l,NULL,&sensors,&fifoCount); /* will return: 0 - if ok 1 - no packet available 2 - if BIT_FIFO_OVERFLOWN is set 3 - if frame corrupted <0 - if error */ if (ret!=0) return ret; } while (fifoCount>1); q._f.w = (float)q._l[0] / (float)QUAT_SENS; q._f.x = (float)q._l[1] / (float)QUAT_SENS; q._f.y = (float)q._l[2] / (float)QUAT_SENS; q._f.z = (float)q._l[3] / (float)QUAT_SENS; mympu.quat.w = q._f.w; mympu.quat.x = q._f.x; mympu.quat.y = q._f.y; mympu.quat.z = q._f.z; quaternionToEuler( &q._f, &mympu.ypr[2], &mympu.ypr[1], &mympu.ypr[0] ); /* need to adjust signs and do the wraps depending on the MPU mount orientation */ /* if axis is no centered around 0 but around i.e 90 degree due to mount orientation */ /* then do: mympu.ypr[x] = wrap_180(90.f+rad2deg(mympu.ypr[x])); */ mympu.ypr[0] = rad2deg(mympu.ypr[0]); mympu.ypr[1] = rad2deg(mympu.ypr[1]); mympu.ypr[2] = rad2deg(mympu.ypr[2]); /* need to adjust signs depending on the MPU mount orientation */ mympu.gyro[0] = -(float)gyro[2] / GYRO_SENS; mympu.gyro[1] = (float)gyro[1] / GYRO_SENS; mympu.gyro[2] = (float)gyro[0] / GYRO_SENS; return 0; }
int data_fusion(mpudata_t *mpu) { quaternion_t dmpQuat; vector3d_t dmpEuler; quaternion_t unfusedQuat; dmpQuat[QUAT_W] = (float)mpu->rawQuat[QUAT_W]; dmpQuat[QUAT_X] = (float)mpu->rawQuat[QUAT_X]; dmpQuat[QUAT_Y] = (float)mpu->rawQuat[QUAT_Y]; dmpQuat[QUAT_Z] = (float)mpu->rawQuat[QUAT_Z]; quaternionNormalize(dmpQuat); quaternionToEuler(dmpQuat, dmpEuler); mpu->fusedEuler[VEC3_X] = dmpEuler[VEC3_X]; mpu->fusedEuler[VEC3_Y] = -dmpEuler[VEC3_Y]; mpu->fusedEuler[VEC3_Z] = -dmpEuler[VEC3_Z]; //0; return 0; }
void estimate_pos_mahony(double time){ quaternion_t quaternion; vector3d_t euler; int i; double timedelay = (time-timeOld); unsigned char data_read[6]; //read accel i2c_read(0x69, 0x3B, 6, data_read); rawAccel[VEC3_X] = ((short)data_read[0] << 8) | data_read[1]; rawAccel[VEC3_Y] = ((short)data_read[2] << 8) | data_read[3]; rawAccel[VEC3_Z] = ((short)data_read[4] << 8) | data_read[5]; //read gyro i2c_read(0x69, 0x43, 6, data_read); rawGyro[VEC3_X] = ((short)data_read[0] << 8) | data_read[1]; rawGyro[VEC3_Y] = ((short)data_read[2] << 8) | data_read[3]; rawGyro[VEC3_Z] = ((short)data_read[4] << 8) | data_read[5]; for(i=VEC3_X;i<(VEC3_Z+1);i++){ accel[i] = (float)rawAccel[i] / (ACCSENS); gyro[i] = (float)rawGyro[i] * DEGREE_TO_RAD / (GYROSENS); } MahonyAHRSupdate(gyro[VEC3_X],gyro[VEC3_Y],gyro[VEC3_Z],accel[VEC3_X],accel[VEC3_Y],accel[VEC3_Z],0,0,0); quaternion[QUAT_W] = q0; quaternion[QUAT_X] = q1; quaternion[QUAT_Y] = q2; quaternion[QUAT_Z] = q3; quaternionToEuler(quaternion,euler); // printf("%f %f %f | %f\n",euler[VEC3_X]*RAD_TO_DEGREE,euler[VEC3_Y]*RAD_TO_DEGREE,euler[VEC3_Z]*RAD_TO_DEGREE, timedelay); printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n", gyro[VEC3_X], gyro[VEC3_Y], gyro[VEC3_Z], accel[VEC3_X], accel[VEC3_Y], accel[VEC3_Z], euler[VEC3_X]*RAD_TO_DEGREE,euler[VEC3_Y]*RAD_TO_DEGREE,euler[VEC3_Z]*RAD_TO_DEGREE, q0,q1,q2,q3, timedelay); timeOld = time; }
int data_fusion(mpudata_t *mpu) { quaternion_t dmpQuat; vector3d_t dmpEuler; quaternion_t magQuat; quaternion_t unfusedQuat; float deltaDMPYaw; float deltaMagYaw; float newMagYaw; float newYaw; dmpQuat[QUAT_W] = (float)mpu->rawQuat[QUAT_W]; dmpQuat[QUAT_X] = (float)mpu->rawQuat[QUAT_X]; dmpQuat[QUAT_Y] = (float)mpu->rawQuat[QUAT_Y]; dmpQuat[QUAT_Z] = (float)mpu->rawQuat[QUAT_Z]; quaternionNormalize(dmpQuat); quaternionToEuler(dmpQuat, dmpEuler); mpu->fusedEuler[VEC3_X] = dmpEuler[VEC3_X]; mpu->fusedEuler[VEC3_Y] = -dmpEuler[VEC3_Y]; mpu->fusedEuler[VEC3_Z] = 0; eulerToQuaternion(mpu->fusedEuler, unfusedQuat); deltaDMPYaw = -dmpEuler[VEC3_Z] + mpu->lastDMPYaw; mpu->lastDMPYaw = dmpEuler[VEC3_Z]; magQuat[QUAT_W] = 0; magQuat[QUAT_X] = mpu->calibratedMag[VEC3_X]; magQuat[QUAT_Y] = mpu->calibratedMag[VEC3_Y]; magQuat[QUAT_Z] = mpu->calibratedMag[VEC3_Z]; tilt_compensate(magQuat, unfusedQuat); newMagYaw = -atan2f(magQuat[QUAT_Y], magQuat[QUAT_X]); if (newMagYaw != newMagYaw) { printf("newMagYaw NAN\n"); return -1; } if (newMagYaw < 0.0f) newMagYaw = TWO_PI + newMagYaw; newYaw = mpu->lastYaw + deltaDMPYaw; if (newYaw > TWO_PI) newYaw -= TWO_PI; else if (newYaw < 0.0f) newYaw += TWO_PI; deltaMagYaw = newMagYaw - newYaw; if (deltaMagYaw >= (float)M_PI) deltaMagYaw -= TWO_PI; else if (deltaMagYaw < -(float)M_PI) deltaMagYaw += TWO_PI; if (yaw_mixing_factor > 0) newYaw += deltaMagYaw / yaw_mixing_factor; if (newYaw > TWO_PI) newYaw -= TWO_PI; else if (newYaw < 0.0f) newYaw += TWO_PI; mpu->lastYaw = newYaw; if (newYaw > (float)M_PI) newYaw -= TWO_PI; mpu->fusedEuler[VEC3_Z] = newYaw; eulerToQuaternion(mpu->fusedEuler, mpu->fusedQuat); return 0; }
vector3df toIrrlicht(btQuaternion quat) { btVector3 euler; quaternionToEuler(quat, euler); return toIrrlicht(euler); }
int main(int argc, char **argv) { // Initialize ROS ros::init(argc, argv, "am_mpu9150_node"); ros::NodeHandle n; ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>("imu", 1); ros::Publisher imu_euler_pub = n.advertise<geometry_msgs::Vector3Stamped>("imu_euler", 1); ros::Rate loop_rate(50); sensor_msgs::Imu imu; imu.header.frame_id = "imu"; geometry_msgs::Vector3Stamped euler; euler.header.frame_id = "imu_euler"; // Init the sensor the values are hardcoded at the local_defaults.h file int opt, len; int i2c_bus = DEFAULT_I2C_BUS; int sample_rate = DEFAULT_SAMPLE_RATE_HZ; int yaw_mix_factor = DEFAULT_YAW_MIX_FACTOR; int verbose = 0; std::string accelFile; std::string magFile; // Parameters from ROS ros::NodeHandle n_private("~"); int def_i2c_bus = 2; n_private.param("i2cbus", i2c_bus, def_i2c_bus); if (i2c_bus < MIN_I2C_BUS || i2c_bus > MAX_I2C_BUS) { ROS_ERROR("am_mpu9150: Imu Bus problem"); return -1; } int def_sample_rate = 50; n_private.param("samplerate", sample_rate, def_sample_rate); if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE) { ROS_ERROR("am_mpu9150: Sample rate problem"); return -1; } loop_rate = sample_rate; int def_yaw_mix_factor = 0; n_private.param("yawmixfactor", yaw_mix_factor, def_yaw_mix_factor); if (yaw_mix_factor < 0 || yaw_mix_factor > 100) { ROS_ERROR("am_mpu9150: yawmixfactor problem"); return -1; } std::string defAccelFile = "./accelcal.txt"; n_private.param("accelfile", accelFile, defAccelFile); std::string defMagFile = "./magcal.txt"; n_private.param("magfile", magFile, defMagFile); unsigned long loop_delay; mpudata_t mpu; // Initialize the MPU-9150 mpu9150_set_debug(verbose); if (mpu9150_init(i2c_bus, sample_rate, yaw_mix_factor)) { ROS_ERROR("am_mpu9150::Failed init!"); exit(1); } //load_calibration(0, accelFile); //load_calibration(1, magFile); memset(&mpu, 0, sizeof(mpudata_t)); vector3d_t e; quaternion_t q; while (ros::ok()) { std::stringstream ss; if (mpu9150_read(&mpu) == 0) { // ROTATE The angles correctly... quaternionToEuler(mpu.fusedQuat, e); e[VEC3_Z] = -e[VEC3_Z]; eulerToQuaternion(e, q); // FUSED imu.header.stamp = ros::Time::now(); imu.orientation.x = q[QUAT_X]; imu.orientation.y = q[QUAT_Y]; imu.orientation.z = q[QUAT_Z]; imu.orientation.w = q[QUAT_W]; // GYRO double gx = mpu.rawGyro[VEC3_X]; double gy = mpu.rawGyro[VEC3_Y]; double gz = mpu.rawGyro[VEC3_Z]; gx = gx * gyroScaleX; gy = gy * gyroScaleY; gz = gz * gyroScaleZ; imu.angular_velocity.x = gx; imu.angular_velocity.y = gy; imu.angular_velocity.z = gz; // ACCEL imu.linear_acceleration.x = mpu.calibratedAccel[VEC3_X]; imu.linear_acceleration.y = mpu.calibratedAccel[VEC3_Y]; imu.linear_acceleration.z = mpu.calibratedAccel[VEC3_Z]; // EULER euler.header.stamp = imu.header.stamp; euler.vector.x = mpu.fusedEuler[VEC3_Y]; euler.vector.y = mpu.fusedEuler[VEC3_X]; euler.vector.z = -mpu.fusedEuler[VEC3_Z]; // Publish the messages imu_pub.publish(imu); imu_euler_pub.publish(euler); //ROS_INFO("y=%f, p=%f, r=%f", euler.vector.z, euler.vector.y, euler.vector.x); } ros::spinOnce(); loop_rate.sleep(); } return 0; }
int main(void) { FILE * fp; char * line = NULL; size_t len = 0; ssize_t read; int line_counter=0; unsigned long qw,qx,qy,qz; quaternion_t q_t; vector3d_t v_t; register_sig_handler(); while(!done) { fp = fopen("/dev/imu", "r"); if (fp == NULL) exit(-1); line_counter=0; while ((read = getline(&line, &len, fp)) != -1) { switch (line_counter++) { case 1: sscanf(line,"%*[^0123456789]%lu",&qw); //printf("Q_W: %lu\n",qw); break; case 2: sscanf(line,"%*[^0123456789]%lu",&qx); //printf("Q_X: %lu\n",qx); ; break; case 3: sscanf(line,"%*[^0123456789]%lu",&qy); //printf("Q_Y: %lu\n",qy); break; case 4: sscanf(line,"%*[^0123456789]%lu",&qz); //printf("Q_Z: %lu\n",qz); break; case 6: //printf("Time\n"); break; case 9: //printf("Mag_X\n"); break; case 10: //printf("Mag_Y\n"); break; case 11: //printf("Mag_Z\n"); break; case 12: //printf("Mag Time\n"); break; default: //printf("%d ist irgendwas\n",line_counter); break; } } if (line) free(line); fclose(fp); //printf("\rW: %lu X: %lu Y: %lu Z: %lu ",qw,qx,qy,qz); //fflush(stdout); q_t[QUAT_W] = qw; q_t[QUAT_X] = qx; q_t[QUAT_Y] = qy; q_t[QUAT_Z] = qz; quaternionToEuler(q_t, v_t); printf("\rX: %f Y: %f Z: %f",v_t[VEC3_X] ,v_t[VEC3_Y] ,v_t[VEC3_Z]); fflush(stdout); usleep(100000); } exit(1); }