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