void RefInterface::run()
{
while(!stopped)
{
//qDebug("Reference thread run");
mutex.lock();
range();
emit getIMUData();
if(!msg.isEmpty())
{
msg.clear();
}
switch(count)
{
case 1: solver->find_intersection_points_nlls3d(x0 ,y0, z0, r0, x1, y1, z1, r1, x2
, y2, z2, r2, 0.0, 0.0, 0.5, px, py, pz );
count = count + 1;
break;
case 2: solver->find_intersection_points_nlls3d(x1 ,y1, z1, r1, x2, y2, z2, r2, x3
, y3, z3, r3, 0.0, 0.0, 0.5, px, py, pz );
count = count + 1;
break;
case 3: solver->find_intersection_points_nlls3d(x2 ,y2, z2, r2, x3, y3, z3, r3, x0
, y0, z0, r0, 0.0, 0.0, 0.5, px, py, pz );
count = count + 1;
break;
case 4: solver->find_intersection_points_nlls3d(x3 ,y3, z3, r3, x3, y0, z0, r0, x1
, y1, z1, r1, 0.0, 0.0, 0.5, px, py, pz );
count = 1;
break;
default:
break;
}
//Here I set the buffer struct to the position returned from solver
oldx = px;
oldy = py;
oldz = pz;
buffer.x = px;
buffer.y = py;
buffer.z = pz;
//append the buffer to the queue
msg.append(buffer);
//unlock the mutex so the consumer can have access to buffer
mutex.unlock();
//set stopped to true, so consumer has a turn
stopped = false;
}
}
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;
}
