void Orientation::calculate_new_gravity(float acceleration[3])
{
double gravity_[3];
// Check if the acceleration vector is the gravity
detectGravity(gravity_, acceleration);
// If it is, we update out gravity vector and calcule the new angle offset due to gravity. Indeed, the gravity measure give us an error on our orientation if not taken into account.
if(!isEqual(gravity, gravity_))
{
/*****We save the new gravity******/
gravity[0] = gravity_[0]/GRAVITY;
gravity[1] = gravity_[1]/GRAVITY;
gravity[2] = gravity_[2]/GRAVITY;
/****** This will give us the pitch and the yaw relative to the User axis and not the Device axis at t0
Calculations are made using the formulation of the rotation matrix*******/
pitchOffset = gravity[1];
if(pitchOffset>1.0f)
pitchOffset = 1.0f;
if(pitchOffset<-1.0f)
pitchOffset = -1.0f;
pitchOffset = asin(-pitchOffset);
rollOffset = gravity[2]/cos(pitchOffset);
if(rollOffset>1.0f)
rollOffset = 1.0f;
if(rollOffset<-1.0f)
rollOffset = -1.0f;
//We substract the actual pitch value from the algorithm
pitchOffset -= (pitch);
//We substract the actual roll value from the algorithm
rollOffset = acos(rollOffset) - (roll);
}
}
int SpatialDataHandler(CPhidgetSpatialHandle spatial, void *userptr, CPhidgetSpatial_SpatialEventDataHandle *data, int count)
{
int i;
//printf("Number of Data Packets in this event: %d\n", count);
/*for(i = 0; i < count; i++)
{
printf("=== Data Set: %d ===\n", i);
printf("Acceleration> x: %6f y: %6f x: %6f\n", data[i]->acceleration[0], data[i]->acceleration[1], data[i]->acceleration[2]);
printf("Angular Rate> x: %6f y: %6f x: %6f\n", data[i]->angularRate[0], data[i]->angularRate[1], data[i]->angularRate[2]);
printf("Magnetic Field> x: %6f y: %6f x: %6f\n", data[i]->magneticField[0], data[i]->magneticField[1], data[i]->magneticField[2]);
printf("Timestamp> seconds: %d -- microseconds: %d\n", data[i]->timestamp.seconds, data[i]->timestamp.microseconds);
}
printf("---------------------------------------------\n");*/
for(i = 0; i< count; i++)
{
acc[0] = data[i]->acceleration[0];
acc[1] = data[i]->acceleration[1];
acc[2] = data[i]->acceleration[2];
ang[0] = data[i]->angularRate[0];
ang[1] = data[i]->angularRate[1];
ang[2] = data[i]->angularRate[2];
mag[0] = data[i]->magneticField[0];
mag[1] = data[i]->magneticField[1];
mag[2] = data[i]->magneticField[2];
if (ang[0] != 0 || ang[1] != 0 || ang[2] != 0)
AnglesMatrixUpdate(ang, data[i]->timestamp.seconds + ((float)data[i]->timestamp.microseconds)/1000000);
AnglesNormalize();
AnglesDriftCorrection(acc);
double gravity_[3];
detectGravity(gravity_, acc);
if(!isEqual(gravity, gravity_))
{
/*****We save the new gravity******/
gravity[0] = gravity_[0]/GRAVITY;
gravity[1] = gravity_[1]/GRAVITY;
gravity[2] = gravity_[2]/GRAVITY;
/****** This will give us the pitch and the yaw relative to the User axis and not the Device axis at t0
Calculations are made using the formulation of the rotation matrix*******/
pitchOffset = gravity[1];
if(pitchOffset>1.0f)
pitchOffset = 1.0f;
if(pitchOffset<-1.0f)
pitchOffset = -1.0f;
pitchOffset = asin(-pitchOffset);
rollOffset = gravity[2]/cos(pitchOffset);
if(rollOffset>1.0f)
rollOffset = 1.0f;
if(rollOffset<-1.0f)
rollOffset = -1.0f;
pitchOffset -= (-asin(dCMMatrix[2][0]));//We substract the actual pitch value from the algorithm
rollOffset = acos(rollOffset) - (atan2(dCMMatrix[2][1],dCMMatrix[2][2]));//We substract the actual roll value from the algorithm
}
}
ROS_INFO("publishing IMU data in ROS!");
spatial_good = true;
return 0;
}
