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

Merge pull request #263 from PhilipRosedale/master

Browse source 
Removed old maple serial code (we are now using Invensense), added acceleration and gravity
This commit is contained in:
ZappoMan 2013-05-09 17:20:06 -07:00
commit e28c246efb
6 changed files with 118 additions and 181 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

70
interface/src/Avatar.cpp
View file

@ -266,21 +266,15 @@ void Avatar::reset() {
// Update avatar head rotation with sensor data
void Avatar::updateHeadFromGyros(float frametime, SerialInterface* serialInterface, glm::vec3* gravity) {
void Avatar::updateHeadFromGyros(float deltaTime, SerialInterface* serialInterface, glm::vec3* gravity) {
float measuredPitchRate = 0.0f;
float measuredRollRate = 0.0f;
float measuredYawRate = 0.0f;
if (serialInterface->active && USING_INVENSENSE_MPU9150) {
measuredPitchRate = serialInterface->getLastPitchRate();
measuredYawRate = serialInterface->getLastYawRate();
measuredRollRate = serialInterface->getLastRollRate();
} else {
measuredPitchRate = serialInterface->getRelativeValue(HEAD_PITCH_RATE);
measuredYawRate = serialInterface->getRelativeValue(HEAD_YAW_RATE);
measuredRollRate = serialInterface->getRelativeValue(HEAD_ROLL_RATE);
}
measuredPitchRate = serialInterface->getLastPitchRate();
measuredYawRate = serialInterface->getLastYawRate();
measuredRollRate = serialInterface->getLastRollRate();
// Update avatar head position based on measured gyro rates
const float MAX_PITCH = 45;
const float MIN_PITCH = -45;
@ -289,13 +283,34 @@ void Avatar::updateHeadFromGyros(float frametime, SerialInterface* serialInterfa
const float MAX_ROLL = 50;
const float MIN_ROLL = -50;
addHeadPitch(measuredPitchRate * frametime);
addHeadYaw(measuredYawRate * frametime);
addHeadRoll(measuredRollRate * frametime);
addHeadPitch(measuredPitchRate * deltaTime);
addHeadYaw(measuredYawRate * deltaTime);
addHeadRoll(measuredRollRate * deltaTime);
setHeadPitch(glm::clamp(getHeadPitch(), MIN_PITCH, MAX_PITCH));
setHeadYaw(glm::clamp(getHeadYaw(), MIN_YAW, MAX_YAW));
setHeadRoll(glm::clamp(getHeadRoll(), MIN_ROLL, MAX_ROLL));
// Update head lean distance based on accelerometer data
const float LEAN_SENSITIVITY = 0.15;
const float LEAN_MAX = 0.45;
const float LEAN_AVERAGING = 10.0;
glm::vec3 headRotationRates(getHeadPitch(), getHeadYaw(), getHeadRoll());
float headRateMax = 50.f;
glm::vec3 leaning = (serialInterface->getLastAcceleration() - serialInterface->getGravity())
* LEAN_SENSITIVITY
* (1.f - fminf(glm::length(headRotationRates), headRateMax) / headRateMax);
leaning.y = 0.f;
if (glm::length(leaning) < LEAN_MAX) {
_head.leanForward = _head.leanForward * (1.f - LEAN_AVERAGING * deltaTime) +
(LEAN_AVERAGING * deltaTime) * leaning.z * LEAN_SENSITIVITY;
_head.leanSideways = _head.leanSideways * (1.f - LEAN_AVERAGING * deltaTime) +
(LEAN_AVERAGING * deltaTime) * leaning.x * LEAN_SENSITIVITY;
}
setHeadLeanSideways(_head.leanSideways);
setHeadLeanForward(_head.leanForward);
}
float Avatar::getAbsoluteHeadYaw() const {
@ -329,6 +344,8 @@ void Avatar::simulate(float deltaTime) {
// update balls
if (_balls) { _balls->simulate(deltaTime); }
// if other avatar, update head position from network data
// update avatar skeleton
updateSkeleton();
@ -517,14 +534,12 @@ void Avatar::updateHandMovementAndTouching(float deltaTime) {
void Avatar::updateHead(float deltaTime) {
// hold on to this - used for testing....
/*
static float test = 0.0f;
test += deltaTime;
_head.leanForward = 0.02 * sin( test * 0.2f );
_head.leanSideways = 0.02 * sin( test * 0.3f );
*/
// Get head position data from network for other people
if (!_isMine) {
_head.leanSideways = getHeadLeanSideways();
_head.leanForward = getHeadLeanForward();
}
//apply the head lean values to the springy position...
if (fabs(_head.leanSideways + _head.leanForward) > 0.0f) {
glm::vec3 headLean =
@ -551,7 +566,7 @@ void Avatar::updateHead(float deltaTime) {
}
// Decay head back to center if turned on
if (_returnHeadToCenter) {
if (_isMine && _returnHeadToCenter) {
// Decay back toward center
_headPitch *= (1.0f - DECAY * _head.returnSpringScale * 2 * deltaTime);
_headYaw *= (1.0f - DECAY * _head.returnSpringScale * 2 * deltaTime);
@ -559,15 +574,14 @@ void Avatar::updateHead(float 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 (_isMine) {
const float RETURN_RANGE = 15.0;
const float RETURN_STRENGTH = 2.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 + ;

164
interface/src/SerialInterface.cpp
View file

@ -29,7 +29,7 @@ int serialBufferPos = 0;
const int ZERO_OFFSET = 2048;
const short NO_READ_MAXIMUM_MSECS = 3000;
const short SAMPLES_TO_DISCARD = 100; // Throw out the first few samples
const int GRAVITY_SAMPLES = 200; // Use the first samples to compute gravity vector
const int GRAVITY_SAMPLES = 60; // Use the first samples to compute gravity vector
const bool USING_INVENSENSE_MPU9150 = 1;
@ -131,42 +131,11 @@ void SerialInterface::initializePort(char* portname, int baud) {
#endif
}
// Reset Trailing averages to the current measurement
void SerialInterface::resetTrailingAverages() {
for (int i = 1; i < NUM_CHANNELS; i++) trailingAverage[i] = lastMeasured[i];
}
// Render the serial interface channel values onscreen as vertical lines
void SerialInterface::renderLevels(int width, int height) {
int i;
int disp_x = 10;
const int GAP = 16;
char val[40];
if (!USING_INVENSENSE_MPU9150) {
for(i = 0; i < NUM_CHANNELS; i++) {
// Actual value
glLineWidth(2.0);
glColor4f(1, 1, 1, 1);
glBegin(GL_LINES);
glVertex2f(disp_x, height * 0.95);
glVertex2f(disp_x, height * (0.25 + 0.75f * getValue(i) / 4096));
glColor4f(1, 0, 0, 1);
glVertex2f(disp_x - 3, height * (0.25 + 0.75f * getValue(i) / 4096));
glVertex2f(disp_x, height * (0.25 + 0.75f * getValue(i) / 4096));
glEnd();
// Trailing Average value
glBegin(GL_LINES);
glColor4f(1, 1, 1, 1);
glVertex2f(disp_x, height * (0.25 + 0.75f * getTrailingValue(i) / 4096));
glVertex2f(disp_x + 4, height * (0.25 + 0.75f * getTrailingValue(i) / 4096));
glEnd();
sprintf(val, "%d", getValue(i));
drawtext(disp_x - GAP / 2, (height * 0.95) + 2, 0.08, 90, 1.0, 0, val, 0, 1, 0);
disp_x += GAP;
}
} else {
if (USING_INVENSENSE_MPU9150) {
// For invensense gyros, render as horizontal bars
const int LEVEL_CORNER_X = 10;
const int LEVEL_CORNER_Y = 200;
@ -177,18 +146,37 @@ void SerialInterface::renderLevels(int width, int height) {
drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 15, 0.10, 0, 1.0, 1, val, 0, 1, 0);
sprintf(val, "Roll %4.1f", _lastRollRate);
drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 30, 0.10, 0, 1.0, 1, val, 0, 1, 0);
sprintf(val, "X %4.3f", _lastAccelX);
drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 45, 0.10, 0, 1.0, 1, val, 0, 1, 0);
sprintf(val, "Y %4.3f", _lastAccelY);
drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 60, 0.10, 0, 1.0, 1, val, 0, 1, 0);
sprintf(val, "Z %4.3f", _lastAccelZ);
drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 75, 0.10, 0, 1.0, 1, val, 0, 1, 0);
// Draw the levels as horizontal lines
const int LEVEL_CENTER = 150;
const float ACCEL_VIEW_SCALING = 50.f;
glLineWidth(2.0);
glColor4f(1, 1, 1, 1);
glBegin(GL_LINES);
// Gyro rates
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y - 3);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastYawRate, LEVEL_CORNER_Y - 3);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 12);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastPitchRate, LEVEL_CORNER_Y + 12);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 27);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastRollRate, LEVEL_CORNER_Y + 27);
// Acceleration
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 42);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAccelX - _gravity.x)* ACCEL_VIEW_SCALING),
LEVEL_CORNER_Y + 42);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 57);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAccelY - _gravity.y) * ACCEL_VIEW_SCALING),
LEVEL_CORNER_Y + 57);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 72);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAccelZ - _gravity.z) * ACCEL_VIEW_SCALING),
LEVEL_CORNER_Y + 72);
glEnd();
// Draw green vertical centerline
glColor4f(0, 1, 0, 0.5);
@ -197,18 +185,6 @@ void SerialInterface::renderLevels(int width, int height) {
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 30);
glEnd();
}
// Display Serial latency block
if (LED) {
glColor3f(1,0,0);
glBegin(GL_QUADS); {
glVertex2f(width - 100, height - 100);
glVertex2f(width, height - 100);
glVertex2f(width, height);
glVertex2f(width - 100, height);
}
glEnd();
}
}
void convertHexToInt(unsigned char* sourceBuffer, int& destinationInt) {
@ -237,15 +213,17 @@ void SerialInterface::readData() {
int accelXRate, accelYRate, accelZRate;
convertHexToInt(sensorBuffer + 6, accelXRate);
convertHexToInt(sensorBuffer + 6, accelZRate);
convertHexToInt(sensorBuffer + 10, accelYRate);
convertHexToInt(sensorBuffer + 14, accelZRate);
convertHexToInt(sensorBuffer + 14, accelXRate);
const float LSB_TO_METERS_PER_SECOND = 1.f / 16384.f;
const float LSB_TO_METERS_PER_SECOND2 = 1.f / 16384.f * 9.80665f;
// From MPU-9150 register map, with setting on
// highest resolution = +/- 2G
_lastAccelX = ((float) accelXRate) * LSB_TO_METERS_PER_SECOND;
_lastAccelY = ((float) accelYRate) * LSB_TO_METERS_PER_SECOND;
_lastAccelZ = ((float) accelZRate) * LSB_TO_METERS_PER_SECOND;
_lastAccelX = ((float) accelXRate) * LSB_TO_METERS_PER_SECOND2;
_lastAccelY = ((float) accelYRate) * LSB_TO_METERS_PER_SECOND2;
_lastAccelZ = ((float) -accelZRate) * LSB_TO_METERS_PER_SECOND2;
int rollRate, yawRate, pitchRate;
@ -262,61 +240,20 @@ void SerialInterface::readData() {
_lastYawRate = ((float) yawRate) * LSB_TO_DEGREES_PER_SECOND;
_lastPitchRate = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND + PITCH_BIAS;
totalSamples++;
} else {
// This array sets the rate of trailing averaging for each channel:
// If the sensor rate is 100Hz, 0.001 will make the long term average a 10-second average
const float AVG_RATE[] = {0.002, 0.002, 0.002, 0.002, 0.002, 0.002};
char bufchar[1];
while (read(_serialDescriptor, &bufchar, 1) > 0) {
serialBuffer[serialBufferPos] = bufchar[0];
serialBufferPos++;
// Have we reached end of a line of input?
if ((bufchar[0] == '\n') || (serialBufferPos >= MAX_BUFFER)) {
std::string serialLine(serialBuffer, serialBufferPos-1);
//printLog("%s\n", serialLine.c_str());
int spot;
//int channel = 0;
std::string val;
for (int i = 0; i < NUM_CHANNELS + 2; i++) {
spot = serialLine.find_first_of(" ", 0);
if (spot != std::string::npos) {
val = serialLine.substr(0,spot);
//printLog("%s\n", val.c_str());
if (i < NUM_CHANNELS) lastMeasured[i] = atoi(val.c_str());
else samplesAveraged = atoi(val.c_str());
} else LED = atoi(serialLine.c_str());
serialLine = serialLine.substr(spot+1, serialLine.length() - spot - 1);
}
// Update Trailing Averages
for (int i = 0; i < NUM_CHANNELS; i++) {
if (totalSamples > SAMPLES_TO_DISCARD) {
trailingAverage[i] = (1.f - AVG_RATE[i])*trailingAverage[i] +
AVG_RATE[i]*(float)lastMeasured[i];
} else {
trailingAverage[i] = (float)lastMeasured[i];
}
}
// Use a set of initial samples to compute gravity
if (totalSamples < GRAVITY_SAMPLES) {
gravity.x += lastMeasured[ACCEL_X];
gravity.y += lastMeasured[ACCEL_Y];
gravity.z += lastMeasured[ACCEL_Z];
}
if (totalSamples == GRAVITY_SAMPLES) {
gravity = glm::normalize(gravity);
printLog("gravity: %f,%f,%f\n", gravity.x, gravity.y, gravity.z);
}
totalSamples++;
serialBufferPos = 0;
}
// Accumulate an initial reading for gravity
// Use a set of initial samples to compute gravity
if (totalSamples < GRAVITY_SAMPLES) {
_gravity.x += _lastAccelX;
_gravity.y += _lastAccelY;
_gravity.z += _lastAccelZ;
}
}
if (totalSamples == GRAVITY_SAMPLES) {
_gravity /= (float) totalSamples;
printLog("Gravity: %f\n", glm::length(_gravity));
}
totalSamples++;
}
if (initialSamples == totalSamples) {
timeval now;
@ -336,23 +273,10 @@ void SerialInterface::resetSerial() {
#ifdef __APPLE__
active = false;
totalSamples = 0;
_gravity = glm::vec3(0, 0, 0);
gettimeofday(&lastGoodRead, NULL);
if (!USING_INVENSENSE_MPU9150) {
gravity = glm::vec3(0, -1, 0);
// Clear the measured and average channel data
for (int i = 0; i < NUM_CHANNELS; i++) {
lastMeasured[i] = 0;
trailingAverage[i] = 0.0;
}
// Clear serial input buffer
for (int i = 1; i < MAX_BUFFER; i++) {
serialBuffer[i] = ' ';
}
}
#endif
}

18
interface/src/SerialInterface.h
View file

@ -32,7 +32,7 @@
#define HEAD_YAW_RATE 0
#define HEAD_ROLL_RATE 2
extern const bool USING_INVENSENSE_MPU9150;
//const bool USING_INVENSENSE_MPU9150;
class SerialInterface {
public:
@ -50,30 +50,20 @@ public:
float getLastYawRate() const { return _lastYawRate; }
float getLastPitchRate() const { return _lastPitchRate; }
float getLastRollRate() const { return _lastRollRate; }
glm::vec3 getLastAcceleration() { return glm::vec3(_lastAccelX, _lastAccelY, _lastAccelZ); };
glm::vec3 getGravity() {return _gravity;};
int getLED() {return LED;};
int getNumSamples() {return samplesAveraged;};
int getValue(int num) {return lastMeasured[num];};
int getRelativeValue(int num) {return static_cast<int>(lastMeasured[num] - trailingAverage[num]);};
float getTrailingValue(int num) {return trailingAverage[num];};
void resetTrailingAverages();
void renderLevels(int width, int height);
bool active;
glm::vec3 getGravity() {return gravity;};
private:
void initializePort(char* portname, int baud);
void resetSerial();
int _serialDescriptor;
int lastMeasured[NUM_CHANNELS];
float trailingAverage[NUM_CHANNELS];
int samplesAveraged;
int LED;
int totalSamples;
timeval lastGoodRead;
glm::vec3 gravity;
glm::vec3 _gravity;
float _lastAccelX;
float _lastAccelY;
float _lastAccelZ;

22
interface/src/main.cpp
View file

@ -364,10 +364,6 @@ void reset_sensors() {
headMouseY = HEIGHT/2;
myAvatar.reset();
if (serialPort.active) {
serialPort.resetTrailingAverages();
}
}
//
@ -379,15 +375,9 @@ void updateAvatar(float deltaTime) {
myAvatar.updateHeadFromGyros(deltaTime, &serialPort, &gravity);
// Grab latest readings from the gyros
float measuredYawRate, measuredPitchRate;
if (USING_INVENSENSE_MPU9150) {
measuredPitchRate = serialPort.getLastPitchRate();
measuredYawRate = serialPort.getLastYawRate();
} else {
measuredPitchRate = serialPort.getRelativeValue(HEAD_PITCH_RATE);
measuredYawRate = serialPort.getRelativeValue(HEAD_YAW_RATE);
}
float measuredPitchRate = serialPort.getLastPitchRate();
float measuredYawRate = serialPort.getLastYawRate();
// Update gyro-based mouse (X,Y on screen)
const float MIN_MOUSE_RATE = 30.0;
const float MOUSE_SENSITIVITY = 0.1f;
@ -1775,7 +1765,7 @@ void idle(void) {
}
// Read serial port interface devices
if (serialPort.active && USING_INVENSENSE_MPU9150) {
if (serialPort.active) {
serialPort.readData();
}
@ -1809,10 +1799,6 @@ void idle(void) {
lastTimeIdle = check;
}
// Read serial data
if (serialPort.active && !USING_INVENSENSE_MPU9150) {
serialPort.readData();
}
}
void reshape(int width, int height) {

16
libraries/avatars/src/AvatarData.cpp
View file

@ -42,6 +42,8 @@ AvatarData::AvatarData() :
_headYaw(0),
_headPitch(0),
_headRoll(0),
_headLeanSideways(0),
_headLeanForward(0),
_handState(0),
_cameraPosition(0,0,0),
_cameraDirection(0,0,0),
@ -84,7 +86,13 @@ int AvatarData::getBroadcastData(unsigned char* destinationBuffer) {
destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, _headPitch);
destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, _headRoll);
// Hand Position
// Head lean X,Z (head lateral and fwd/back motion relative to torso)
memcpy(destinationBuffer, &_headLeanSideways, sizeof(float));
destinationBuffer += sizeof(float);
memcpy(destinationBuffer, &_headLeanForward, sizeof(float));
destinationBuffer += sizeof(float);
// Hand Position
memcpy(destinationBuffer, &_handPosition, sizeof(float) * 3);
destinationBuffer += sizeof(float) * 3;
@ -150,6 +158,12 @@ int AvatarData::parseData(unsigned char* sourceBuffer, int numBytes) {
sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t *)sourceBuffer, &_headPitch);
sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t *)sourceBuffer, &_headRoll);
// Head position relative to pelvis
memcpy(&_headLeanSideways, sourceBuffer, sizeof(float));
sourceBuffer += sizeof(float);
memcpy(&_headLeanForward, sourceBuffer, sizeof(float));
sourceBuffer += sizeof(float);
// Hand Position
memcpy(&_handPosition, sourceBuffer, sizeof(float) * 3);
sourceBuffer += sizeof(float) * 3;

9
libraries/avatars/src/AvatarData.h
View file

@ -55,6 +55,12 @@ public:
void addHeadYaw(float y){_headYaw -= y; }
void addHeadRoll(float r){_headRoll += r; }
// Head vector deflection from pelvix in X,Z
void setHeadLeanSideways(float s) {_headLeanSideways = s; };
float getHeadLeanSideways() const { return _headLeanSideways; };
void setHeadLeanForward(float f) {_headLeanForward = f; };
float getHeadLeanForward() const { return _headLeanForward; };
// Hand State
void setHandState(char s) { _handState = s; };
char getHandState() const {return _handState; };
@ -104,6 +110,9 @@ protected:
float _headYaw;
float _headPitch;
float _headRoll;
float _headLeanSideways;
float _headLeanForward;
// Audio loudness (used to drive facial animation)
float _audioLoudness;