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