void INVMPU_ReadGyro(MPUSensorData* data)
{
unsigned long sensor_timestamp;
short gyro[3];
mpu_get_gyro_reg(gyro, &sensor_timestamp);
//芯片安装情况的不同影响这里赋值的顺序和正负，x轴为俯仰
data->gyro_y = gyro[1];
data->gyro_x = gyro[0];
data->gyro_z = gyro[2];
}
int MPU9250_DMP::updateGyro(void)
{
short data[3];
if (mpu_get_gyro_reg(data, &time))
{
return INV_ERROR;
}
gx = data[X_AXIS];
gy = data[Y_AXIS];
gz = data[Z_AXIS];
return INV_SUCCESS;
}
void updateGyroData()
{
mpu_get_gyro_reg (&(globalAttitudeSensor.gx), 0) ;
// 制造漂移，用于调试
// globalAttitudeSensor.gx -= 120 ;
// globalAttitudeSensor.gy -= 120 ;
// globalAttitudeSensor.gz -= 120 ;
// ITG3205_readXYZ ( &(globalAttitudeSensor.gx) ) ;
globalAttitudeSensor.gx -= globalAttitudeSensor.GyroZero[0] ;
globalAttitudeSensor.gy -= globalAttitudeSensor.GyroZero[1] ;
globalAttitudeSensor.gz -= globalAttitudeSensor.GyroZero[2] ;
}
void SerialInterface::readData(float deltaTime) {
#ifndef _WIN32
int initialSamples = totalSamples;
if (USING_INVENSENSE_MPU9150) {
// ask the invensense for raw gyro data
short accelData[3];
if (mpu_get_accel_reg(accelData, 0)) {
close(_serialDescriptor);
qDebug("Disconnected SerialUSB.\n");
_active = false;
return; // disconnected
}
const float LSB_TO_METERS_PER_SECOND2 = 1.f / 16384.f * GRAVITY_EARTH;
// From MPU-9150 register map, with setting on
// highest resolution = +/- 2G
_lastAcceleration = glm::vec3(-accelData[2], -accelData[1], -accelData[0]) * LSB_TO_METERS_PER_SECOND2;
short gyroData[3];
mpu_get_gyro_reg(gyroData, 0);
// Convert the integer rates to floats
const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f; // From MPU-9150 register map, 2000 deg/sec.
glm::vec3 rotationRates;
rotationRates[0] = ((float) -gyroData[2]) * LSB_TO_DEGREES_PER_SECOND;
rotationRates[1] = ((float) -gyroData[1]) * LSB_TO_DEGREES_PER_SECOND;
rotationRates[2] = ((float) -gyroData[0]) * LSB_TO_DEGREES_PER_SECOND;
short compassData[3];
mpu_get_compass_reg(compassData, 0);
// Convert integer values to floats, update extents
_lastCompass = glm::vec3(compassData[2], -compassData[0], -compassData[1]);
// update and subtract the long term average
_averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates +
1.f/(float)LONG_TERM_RATE_SAMPLES * rotationRates;
rotationRates -= _averageRotationRates;
// compute the angular acceleration
glm::vec3 angularAcceleration = (deltaTime < EPSILON) ? glm::vec3() : (rotationRates - _lastRotationRates) / deltaTime;
_lastRotationRates = rotationRates;
// Update raw rotation estimates
glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
glm::quat(glm::radians(deltaTime * _lastRotationRates));
// Update acceleration estimate: first, subtract gravity as rotated into current frame
_estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() :
_lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
// update and subtract the long term average
_averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration +
1.f/(float)LONG_TERM_RATE_SAMPLES * _estimatedAcceleration;
_estimatedAcceleration -= _averageAcceleration;
// Consider updating our angular velocity/acceleration to linear acceleration mapping
if (glm::length(_estimatedAcceleration) > EPSILON &&
(glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON)) {
// compute predicted linear acceleration, find error between actual and predicted
glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates +
_angularAccelToLinearAccel * angularAcceleration;
glm::vec3 error = _estimatedAcceleration - predictedAcceleration;
// the "error" is actually what we want: the linear acceleration minus rotational influences
_estimatedAcceleration = error;
// adjust according to error in each dimension, in proportion to input magnitudes
for (int i = 0; i < 3; i++) {
if (fabsf(error[i]) < EPSILON) {
continue;
}
const float LEARNING_RATE = 0.001f;
float rateSum = fabsf(_lastRotationRates.x) + fabsf(_lastRotationRates.y) + fabsf(_lastRotationRates.z);
if (rateSum > EPSILON) {
for (int j = 0; j < 3; j++) {
float proportion = LEARNING_RATE * fabsf(_lastRotationRates[j]) / rateSum;
if (proportion > EPSILON) {
_angularVelocityToLinearAccel[j][i] += error[i] * proportion / _lastRotationRates[j];
}
}
}
float accelSum = fabsf(angularAcceleration.x) + fabsf(angularAcceleration.y) + fabsf(angularAcceleration.z);
if (accelSum > EPSILON) {
for (int j = 0; j < 3; j++) {
float proportion = LEARNING_RATE * fabsf(angularAcceleration[j]) / accelSum;
if (proportion > EPSILON) {
_angularAccelToLinearAccel[j][i] += error[i] * proportion / angularAcceleration[j];
}
}
}
}
}
// rotate estimated acceleration into global rotation frame
_estimatedAcceleration = estimatedRotation * _estimatedAcceleration;
// Update estimated position and velocity
float const DECAY_VELOCITY = 0.975f;
float const DECAY_POSITION = 0.975f;
_estimatedVelocity += deltaTime * _estimatedAcceleration;
_estimatedPosition += deltaTime * _estimatedVelocity;
_estimatedVelocity *= DECAY_VELOCITY;
// Attempt to fuse gyro position with webcam position
Webcam* webcam = Application::getInstance()->getWebcam();
if (webcam->isActive()) {
const float WEBCAM_POSITION_FUSION = 0.5f;
_estimatedPosition = glm::mix(_estimatedPosition, webcam->getEstimatedPosition(), WEBCAM_POSITION_FUSION);
} else {
_estimatedPosition *= DECAY_POSITION;
}
// Accumulate a set of initial baseline readings for setting gravity
if (totalSamples == 0) {
_gravity = _lastAcceleration;
}
else {
if (totalSamples < GRAVITY_SAMPLES) {
_gravity = glm::mix(_gravity, _lastAcceleration, 1.0f / GRAVITY_SAMPLES);
// North samples start later, because the initial compass readings are screwy
int northSample = totalSamples - (GRAVITY_SAMPLES - NORTH_SAMPLES);
if (northSample == 0) {
_north = _lastCompass;
} else if (northSample > 0) {
_north = glm::mix(_north, _lastCompass, 1.0f / NORTH_SAMPLES);
}
} else {
// Use gravity reading to do sensor fusion on the pitch and roll estimation
estimatedRotation = safeMix(estimatedRotation,
rotationBetween(estimatedRotation * _lastAcceleration, _gravity) * estimatedRotation,
1.0f / ACCELERATION_SENSOR_FUSION_SAMPLES);
// Update the compass extents
_compassMinima = glm::min(_compassMinima, _lastCompass);
_compassMaxima = glm::max(_compassMaxima, _lastCompass);
// Same deal with the compass heading
estimatedRotation = safeMix(estimatedRotation,
rotationBetween(estimatedRotation * recenterCompass(_lastCompass),
recenterCompass(_north)) * estimatedRotation,
1.0f / COMPASS_SENSOR_FUSION_SAMPLES);
}
}
_estimatedRotation = safeEulerAngles(estimatedRotation);
totalSamples++;
}
if (initialSamples == totalSamples) {
timeval now;
gettimeofday(&now, NULL);
if (diffclock(&lastGoodRead, &now) > NO_READ_MAXIMUM_MSECS) {
qDebug("No data - Shutting down SerialInterface.\n");
resetSerial();
}
} else {
gettimeofday(&lastGoodRead, NULL);
}
#endif
}
