static void
check_quaternion(generator_t &gen, distribution_t &dist)
{
// check identity
{
quat q;
glm::quat g;
quatIdentity(&q);
assert(q == g);
}
for(size_t x = 0; x < 10000; ++x)
{
// check negation
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
assert(quatNegate(q) == -g);
}
// check addition
{
quat q1 = randomQuat(gen, dist);
quat q2 = randomQuat(gen, dist);
glm::quat g1 = loadQuat(q1);
glm::quat g2 = loadQuat(q2);
assert(quatAdd(q1, q2) == g1+g2);
}
// check subtraction
{
quat q1 = randomQuat(gen, dist);
quat q2 = randomQuat(gen, dist);
glm::quat g1 = loadQuat(q1);
glm::quat g2 = loadQuat(q2);
assert(quatSubtract(q1, q2) == g1 + (-g2));
}
// check scale
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
float f = dist(gen);
assert(quatScale(q, f) == g*f);
}
// check normalize
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
assert(quatNormalize(q) == glm::normalize(g));
}
// check dot
{
quat q1 = randomQuat(gen, dist);
quat q2 = randomQuat(gen, dist);
glm::quat g1 = loadQuat(q1);
glm::quat g2 = loadQuat(q2);
assert(std::abs(quatDot(q1, q2) - glm::dot(g1, g2)) < 0.0001f);
}
// check conjugate
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
assert(quatConjugate(q) == glm::conjugate(g));
}
// check inverse
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
assert(quatInverse(q) == glm::inverse(g));
}
// check quaternion multiplication
{
quat q1 = randomQuat(gen, dist);
quat q2 = randomQuat(gen, dist);
glm::quat g1 = loadQuat(q1);
glm::quat g2 = loadQuat(q2);
assert(quatMultiply(q1, q2) == g1*g2);
}
// check vector multiplication
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
glm::vec3 v = randomVector(gen, dist);
assert(quatMultiplyVec3f(q, (vec3f){ v.x, v.y, v.z }) == g*v);
}
// check rotation
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
glm::vec3 v = randomVector(gen, dist);
float r = randomAngle(gen, dist);
assert(quatRotate(q, (vec3f){ v.x, v.y, v.z }, r) == glm::rotate(g, r, v));
}
// check rotate X
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
float r = randomAngle(gen, dist);
assert(quatRotateX(q, r) == glm::rotate(g, r, x_axis));
}
// check rotate Y
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
float r = randomAngle(gen, dist);
assert(quatRotateY(q, r) == glm::rotate(g, r, y_axis));
}
// check rotate Z
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
float r = randomAngle(gen, dist);
assert(quatRotateZ(q, r) == glm::rotate(g, r, z_axis));
}
// check conversion from matrix
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
mtx44 m;
quatToMtx44(&m, q);
assert(m == glm::mat4_cast(g));
}
}
}
myQuat_t getPosition(mpu9250_t dev)
{
imuData_t imuData;
if (getIMUData(dev, &imuData))
{
if (! validIMUData(imuData))
{
puts("invalid IMU data");
if (failureCount++ > 20)
{
puts("Error in IMU, restarting!");
identifyYourself("IMU failure");
if (! initialiseIMU(&dev))
{
puts("Could not initialise IMU! dying...");
exit(1);
}
}
return currentQ;
}
failureCount = 0;
/************************************************************************
* Get Gyro data that records current angular velocity and create an
* estimated quaternion representation of orientation using this
* velocity, the time interval between the last calculation and the
* previous orientation:
* change in position = velocity * time,
* new position = old position + change in position)
************************************************************************/
// get orientation as deduced by adding gyro measured rotational
// velocity (times time interval) to previous orientation.
double gx1 = (double)(imuData.gyro.x_axis);
double gy1 = (double)(imuData.gyro.y_axis);
double gz1 = (double)(imuData.gyro.z_axis);
double omega[3] = { gx1, gy1, gz1 }; // this will be in degrees/sec
//double omegaMagnitude = fabs(vecLength(omega));
uint32_t dt = imuData.ts - lastIMUData.ts; // dt in microseconds
lastIMUData = imuData;
myQuat_t gRot = makeQuatFromAngularVelocityTime(omega, dt/1000000.0);
myQuat_t gyroDeducedQ = quatMultiply(currentQ, gRot);
quatNormalise(&gyroDeducedQ);
if (! useGyroscopes)
{
makeIdentityQuat(&gyroDeducedQ);
}
if (! (useAccelerometers | useMagnetometers))
{
// return now with just the gyro data
currentQ = gyroDeducedQ;
lastIMUData = imuData;
return currentQ;
}
// get orientation from the gyro as Roll Pitch Yaw (ie Euler angles)
double gyroDeducedYPR[3];
quatToEuler(gyroDeducedQ, gyroDeducedYPR);
double currentYPR[3];
quatToEuler(currentQ, currentYPR);
// Calculate a roll/pich/yaw from the accelerometers and magnetometers.
/************************************************************************
* Get Accelerometer data that records gravity direction and create a
* 'measured' Quaternion representation of orientation (will only be
* pitch and roll, not yaw)
************************************************************************/
// get gravity direction from accelerometer
double ax1 = imuData.accel.x_axis / 1024.0;
double ay1 = imuData.accel.y_axis / 1024.0;
double az1 = imuData.accel.z_axis / 1024.0;
double downSensor[3] = { ax1, ay1, az1 };
if (fabs(vecLength(downSensor) - 0.98) > 0.5)
{
// excessive accelerometer reading, bail out
//printf("too much accel: %f ", fabs(vecLength(downSensor)));
//dumpVec(downSensor);
currentQ = gyroDeducedQ;
lastIMUData = imuData;
return currentQ;
}
vecNormalise(downSensor);
// The accelerometers can only measure pitch and roll (in the world reference
// frame), calculate the pitch and roll values to account for
// accelerometer readings, make yaw = 0.
double ypr[3];
double downX = downSensor[X_AXIS];
double downY = downSensor[Y_AXIS];
double downZ = downSensor[Z_AXIS];
// add a little X into Z when calculating roll to compensate for
// situations when pitching near vertical as Y, Z will be near 0 and
// the results will be unstable
double fiddledDownZ = sqrt(downZ*downZ + 0.001*downX*downX);
if (downZ <= 0) // compensate for loss of sign when squaring Z
fiddledDownZ *= -1.0;
ypr[ROLL] = atan2(downY, fiddledDownZ);
ypr[PITCH] = -atan2(downX, sqrt(downY*downY + downZ*downZ));
ypr[YAW] = 0; // yaw == 0, accelerometer cannot measure yaw
if (! useAccelerometers)
{
// if no accelerometers, use roll and pitch values from gyroscope
// derived orientation
ypr[PITCH] = gyroDeducedYPR[PITCH];
ypr[ROLL] = gyroDeducedYPR[ROLL];
ypr[YAW] = gyroDeducedYPR[YAW];
}
else if (vecLength(downSensor) == 0)
{
puts("weightless?");
// something wrong! - weightless??
ypr[PITCH] = gyroDeducedYPR[PITCH];
ypr[ROLL] = gyroDeducedYPR[ROLL];
ypr[YAW] = gyroDeducedYPR[YAW];
}
if (! useMagnetometers)
{
ypr[YAW] = gyroDeducedYPR[YAW];
}
myQuat_t accelQ = eulerToQuat(ypr);
myQuat_t invAccelQ = quatConjugate(accelQ);
/************************************************************************
* Get Magnetometer data that records north direction and create a
* 'measured' Quaternion representation of orientation (will only be
* yaw)
************************************************************************/
// get magnetometer indication of North
double mx1 = (double)(imuData.mag.x_axis - magHardCorrection[X_AXIS]);
double my1 = (double)(imuData.mag.y_axis - magHardCorrection[Y_AXIS]);
double mz1 = (double)(imuData.mag.z_axis - magHardCorrection[Z_AXIS]);
mx1 *= magSoftCorrection[X_AXIS];
my1 *= magSoftCorrection[Y_AXIS];
mz1 *= magSoftCorrection[Z_AXIS];
// NB MPU9250 has different XYZ axis for magnetometers than for gyros
// and accelerometers so juggle x,y,z here...
double magV[3] = { my1, mx1, -mz1 };
vecNormalise(magV);
// make quaternion representing mag readings
myQuat_t magQ = quatFromValues(0, magV[X_AXIS], magV[Y_AXIS], magV[Z_AXIS]);
if (vecLength(magV) == 0)
{
// something wrong! - no magnetic field??
//puts("not on earth?");
magQ = quatFromValues(1, 0, 0, 0);
}
// the magnetometers can only measure yaw (in the world reference
// frame), adjust the yaw of the roll/pitch/yaw values calculated
// earlier to account for the magnetometer readings.
if (useMagnetometers)
{
// pitch and roll the mag direction using accelerometer info to
// create a quaternion that represents the IMU as if it was
// horizontal (so we can get pure yaw)
myQuat_t horizQ = quatMultiply(accelQ, magQ);
horizQ = quatMultiply(horizQ, invAccelQ);
// calculate pure yaw
ypr[YAW] = -atan2(horizQ.y, horizQ.x);
}
// measuredQ is the orientation as measured using the
// accelerometer/magnetometer readings
myQuat_t measuredQ = eulerToQuat(ypr);
// check measured and deduced orientations are not wildly different (eg
// due to a 360d alias flip) and adjust if problem...
measuredQ = adjustForCongruence(measuredQ, gyroDeducedQ);
/*************************************************************************
* The gyro estimated orientation will be smooth and precise over short
* time intervals but small errors will accumulate causing it to
* drift over time. The 'measured' orientation as obtained from the
* magnetometer and accelerometer will be jumpy but will always average
* around the correct orientation.
*
* Take the mag and accel 'measured' orientation and the gyro
* 'estimated' orientation and create a new 'current' orientation that
* is the estimated orientation corrected slightly towards the measured
* orientation. That way we get the smooth precision of the gyros but
* eliminate the accumulation of drift errors.
************************************************************************/
if (useGyroscopes)
{
// the new orientation is the gyro deduced position corrected
// towards the accel/mag measured position using interpolation
// between quaternions.
currentQ = slerp(gyroDeducedQ, measuredQ, 0.02);
}
else
{
currentQ = measuredQ;
}
if (! isQuatValid(currentQ))
{
puts("error in quaternion, reseting orientation...");
displayData(imuData);
dumpQuat(currentQ);
makeIdentityQuat(¤tQ);
}
}
return currentQ;
}
