Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-04-21 09:44:21 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions 2

Invensense gyros now correctly controlling head motion, code cleanups.

Browse source
This commit is contained in:
Philip Rosedale 2013-05-08 14:39:41 -07:00
parent 131377aaf9
commit a9882d054f
2 changed files with 34 additions and 21 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

50
interface/src/Avatar.cpp
View file

@ -260,38 +260,38 @@ void Avatar::reset() {
_head.leanForward = _head.leanSideways = 0;
}
// this pertains to moving the head with the glasses
// Using serial data, update avatar/render position and angles
// Update avatar head rotation with sensor data
void Avatar::UpdateGyros(float frametime, SerialInterface* serialInterface, glm::vec3* gravity) {
float measured_pitch_rate = 0.0f;
float measured_roll_rate = 0.0f;
float measuredPitchRate = 0.0f;
float measuredRollRate = 0.0f;
float measuredYawRate = 0.0f;
if (serialInterface->active && USING_INVENSENSE_MPU9150) {
measured_pitch_rate = serialInterface->getLastPitchRate();
_head.yawRate = serialInterface->getLastYawRate();
measured_roll_rate = -1 * serialInterface->getLastRollRate();
measuredPitchRate = serialInterface->getLastPitchRate();
measuredYawRate = serialInterface->getLastYawRate();
measuredRollRate = serialInterface->getLastRollRate();
} else {
measured_pitch_rate = serialInterface->getRelativeValue(HEAD_PITCH_RATE);
_head.yawRate = serialInterface->getRelativeValue(HEAD_YAW_RATE);
measured_roll_rate = serialInterface->getRelativeValue(HEAD_ROLL_RATE);
measuredPitchRate = serialInterface->getRelativeValue(HEAD_PITCH_RATE);
measuredYawRate = serialInterface->getRelativeValue(HEAD_YAW_RATE);
measuredRollRate = serialInterface->getRelativeValue(HEAD_ROLL_RATE);
}
// Update avatar head position based on measured gyro rates
const float HEAD_ROTATION_SCALE = 0.70;
const float HEAD_ROLL_SCALE = 0.40;
const float MAX_PITCH = 45;
const float MIN_PITCH = -45;
const float MAX_YAW = 85;
const float MIN_YAW = -85;
const float MAX_ROLL = 50;
const float MIN_ROLL = -50;
if ((_headPitch < MAX_PITCH) && (_headPitch > MIN_PITCH)) {
addHeadPitch(measured_pitch_rate * -HEAD_ROTATION_SCALE * frametime);
}
addHeadRoll(measured_roll_rate * HEAD_ROLL_SCALE * frametime);
addHeadPitch(measuredPitchRate * frametime);
addHeadYaw(measuredYawRate * frametime);
addHeadRoll(measuredRollRate * frametime);
if ((_headYaw < MAX_YAW) && (_headYaw > MIN_YAW)) {
addHeadYaw(_head.yawRate * HEAD_ROTATION_SCALE * frametime);
}
setHeadPitch(glm::clamp(getHeadPitch(), MIN_PITCH, MAX_PITCH));
setHeadYaw(glm::clamp(getHeadYaw(), MIN_YAW, MAX_YAW));
setHeadRoll(glm::clamp(getHeadRoll(), MIN_ROLL, MAX_ROLL));
}
float Avatar::getAbsoluteHeadYaw() const {
@ -518,6 +518,16 @@ void Avatar::updateHead(float deltaTime) {
_headRoll *= (1.0f - DECAY * _head.returnSpringScale * 2 * deltaTime);
}
// For invensense gyro, decay only slightly when roughly centered
if (USING_INVENSENSE_MPU9150) {
const float RETURN_RANGE = 5.0;
const float RETURN_STRENGTH = 1.0;
if (fabs(_headPitch) < RETURN_RANGE) { _headPitch *= (1.0f - RETURN_STRENGTH * deltaTime); }
if (fabs(_headYaw) < RETURN_RANGE) { _headYaw *= (1.0f - RETURN_STRENGTH * deltaTime); }
if (fabs(_headRoll) < RETURN_RANGE) { _headRoll *= (1.0f - RETURN_STRENGTH * deltaTime); }
}
if (_head.noise) {
// Move toward new target
_headPitch += (_head.pitchTarget - _headPitch) * 10 * deltaTime; // (1.f - DECAY*deltaTime)*Pitch + ;

5
interface/src/SerialInterface.cpp
View file

@ -246,9 +246,12 @@ void SerialInterface::readData() {
// 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.
const float PITCH_BIAS = 2.0; // Strangely, there is a small DC bias in the
// invensense pitch reading. Gravity?
_lastRollRate = (float) rollRate * LSB_TO_DEGREES_PER_SECOND;
_lastYawRate = (float) yawRate * LSB_TO_DEGREES_PER_SECOND;
_lastPitchRate = (float) pitchRate * LSB_TO_DEGREES_PER_SECOND;
_lastPitchRate = (float) -pitchRate * LSB_TO_DEGREES_PER_SECOND + PITCH_BIAS;
totalSamples++;
} else {