Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-08-09 00:36:47 +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; _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) { void Avatar::UpdateGyros(float frametime, SerialInterface* serialInterface, glm::vec3* gravity) {
float measured_pitch_rate = 0.0f; float measuredPitchRate = 0.0f;
float measured_roll_rate = 0.0f; float measuredRollRate = 0.0f;
float measuredYawRate = 0.0f;
if (serialInterface->active && USING_INVENSENSE_MPU9150) { if (serialInterface->active && USING_INVENSENSE_MPU9150) {
measured_pitch_rate = serialInterface->getLastPitchRate(); measuredPitchRate = serialInterface->getLastPitchRate();
_head.yawRate = serialInterface->getLastYawRate(); measuredYawRate = serialInterface->getLastYawRate();
measured_roll_rate = -1 * serialInterface->getLastRollRate(); measuredRollRate = serialInterface->getLastRollRate();
} else { } else {
measured_pitch_rate = serialInterface->getRelativeValue(HEAD_PITCH_RATE); measuredPitchRate = serialInterface->getRelativeValue(HEAD_PITCH_RATE);
_head.yawRate = serialInterface->getRelativeValue(HEAD_YAW_RATE); measuredYawRate = serialInterface->getRelativeValue(HEAD_YAW_RATE);
measured_roll_rate = serialInterface->getRelativeValue(HEAD_ROLL_RATE); measuredRollRate = serialInterface->getRelativeValue(HEAD_ROLL_RATE);
} }
// Update avatar head position based on measured gyro rates // 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 MAX_PITCH = 45;
const float MIN_PITCH = -45; const float MIN_PITCH = -45;
const float MAX_YAW = 85; const float MAX_YAW = 85;
const float MIN_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(measuredPitchRate * frametime);
addHeadPitch(measured_pitch_rate * -HEAD_ROTATION_SCALE * frametime); addHeadYaw(measuredYawRate * frametime);
} addHeadRoll(measuredRollRate * frametime);
addHeadRoll(measured_roll_rate * HEAD_ROLL_SCALE * frametime);
if ((_headYaw < MAX_YAW) && (_headYaw > MIN_YAW)) { setHeadPitch(glm::clamp(getHeadPitch(), MIN_PITCH, MAX_PITCH));
addHeadYaw(_head.yawRate * HEAD_ROTATION_SCALE * frametime); setHeadYaw(glm::clamp(getHeadYaw(), MIN_YAW, MAX_YAW));
} setHeadRoll(glm::clamp(getHeadRoll(), MIN_ROLL, MAX_ROLL));
} }
float Avatar::getAbsoluteHeadYaw() const { float Avatar::getAbsoluteHeadYaw() const {
@ -518,6 +518,16 @@ void Avatar::updateHead(float deltaTime) {
_headRoll *= (1.0f - DECAY * _head.returnSpringScale * 2 * 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) { if (_head.noise) {
// Move toward new target // Move toward new target
_headPitch += (_head.pitchTarget - _headPitch) * 10 * deltaTime; // (1.f - DECAY*deltaTime)*Pitch + ; _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 // 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 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; _lastRollRate = (float) rollRate * LSB_TO_DEGREES_PER_SECOND;
_lastYawRate = (float) yawRate * 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++; totalSamples++;
} else { } else {