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

Merge pull request #476 from PhilipRosedale/master

Browse source 
Cleanup of serial interface code, added translation estimate
This commit is contained in:
ZappoMan 2013-06-05 10:04:27 -07:00
commit 8a08bfd01d
5 changed files with 128 additions and 110 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

64
interface/src/Application.cpp
View file

@ -753,8 +753,8 @@ void Application::timer() {
gettimeofday(&_timerStart, NULL);
// if we haven't detected gyros, check for them now
if (!_serialPort.active) {
_serialPort.pair();
if (!_serialHeadSensor.active) {
_serialHeadSensor.pair();
}
}
@ -892,8 +892,8 @@ void Application::idle() {
}
// Read serial port interface devices
if (_serialPort.active) {
_serialPort.readData(deltaTime);
if (_serialHeadSensor.active) {
_serialHeadSensor.readData(deltaTime);
}
// Update transmitter
@ -1523,26 +1523,40 @@ void Application::init() {
}
void Application::updateAvatar(float deltaTime) {
// Update my avatar's head position from gyros
_myAvatar.updateHeadFromGyros(deltaTime, &_serialPort);
// Grab latest readings from the gyros
float measuredPitchRate = _serialPort.getLastPitchRate();
float measuredYawRate = _serialPort.getLastYawRate();
// Update gyro-based mouse (X,Y on screen)
const float MIN_MOUSE_RATE = 3.0;
const float HORIZONTAL_PIXELS_PER_DEGREE = 2880.f / 45.f;
const float VERTICAL_PIXELS_PER_DEGREE = 1800.f / 30.f;
if (powf(measuredYawRate * measuredYawRate +
measuredPitchRate * measuredPitchRate, 0.5) > MIN_MOUSE_RATE) {
_headMouseX -= measuredYawRate * HORIZONTAL_PIXELS_PER_DEGREE * deltaTime;
_headMouseY -= measuredPitchRate * VERTICAL_PIXELS_PER_DEGREE * deltaTime;
if (_serialHeadSensor.active) {
// Update avatar head translation
if (_gyroLook->isChecked()) {
glm::vec3 headPosition = _serialHeadSensor.getEstimatedPosition();
const float HEAD_OFFSET_SCALING = 3.f;
headPosition *= HEAD_OFFSET_SCALING;
_myCamera.setEyeOffsetPosition(headPosition);
}
// Update my avatar's head position from gyros
_myAvatar.updateHeadFromGyros(deltaTime, &_serialHeadSensor);
// Grab latest readings from the gyros
float measuredPitchRate = _serialHeadSensor.getLastPitchRate();
float measuredYawRate = _serialHeadSensor.getLastYawRate();
// Update gyro-based mouse (X,Y on screen)
const float MIN_MOUSE_RATE = 3.0;
const float HORIZONTAL_PIXELS_PER_DEGREE = 2880.f / 45.f;
const float VERTICAL_PIXELS_PER_DEGREE = 1800.f / 30.f;
if (powf(measuredYawRate * measuredYawRate +
measuredPitchRate * measuredPitchRate, 0.5) > MIN_MOUSE_RATE) {
_headMouseX -= measuredYawRate * HORIZONTAL_PIXELS_PER_DEGREE * deltaTime;
_headMouseY -= measuredPitchRate * VERTICAL_PIXELS_PER_DEGREE * deltaTime;
}
_headMouseX = max(_headMouseX, 0);
_headMouseX = min(_headMouseX, _glWidget->width());
_headMouseY = max(_headMouseY, 0);
_headMouseY = min(_headMouseY, _glWidget->height());
}
_headMouseX = max(_headMouseX, 0);
_headMouseX = min(_headMouseX, _glWidget->width());
_headMouseY = max(_headMouseY, 0);
_headMouseY = min(_headMouseY, _glWidget->height());
if (OculusManager::isConnected()) {
float yaw, pitch, roll;
@ -1954,7 +1968,7 @@ void Application::displayOverlay() {
}
// Show detected levels from the serial I/O ADC channel sensors
if (_displayLevels) _serialPort.renderLevels(_glWidget->width(), _glWidget->height());
if (_displayLevels) _serialHeadSensor.renderLevels(_glWidget->width(), _glWidget->height());
// Show hand transmitter data if detected
if (_myTransmitter.isConnected()) {
@ -2332,8 +2346,8 @@ void Application::resetSensors() {
_headMouseX = _mouseX = _glWidget->width() / 2;
_headMouseY = _mouseY = _glWidget->height() / 2;
if (_serialPort.active) {
_serialPort.resetAverages();
if (_serialHeadSensor.active) {
_serialHeadSensor.resetAverages();
}
QCursor::setPos(_headMouseX, _headMouseY);
_myAvatar.reset();

5
interface/src/Application.h
View file

@ -199,9 +199,9 @@ private:
QAction* _frustumRenderModeAction;
QAction* _settingsAutosave; // Whether settings are saved automatically
SerialInterface _serialHeadSensor;
QNetworkAccessManager* _networkAccessManager;
SerialInterface _serialPort;
QSettings* _settings;
bool _displayLevels;
glm::vec3 _gravity;
@ -292,7 +292,6 @@ private:
int _bytesPerSecond;
int _bytesCount;
QSettings* _settings; // Contain Menu settings and Avatar data
bool _autosave; // True if the autosave is on.
};

2
interface/src/Avatar.cpp
View file

@ -353,7 +353,7 @@ void Avatar::updateFromMouse(int mouseX, int mouseY, int screenWidth, int scree
if (fabs(mouseLocationY) > MOUSE_MOVE_RADIUS) {
float mousePitchAdd = (fabs(mouseLocationY) - MOUSE_MOVE_RADIUS) / (0.5f - MOUSE_MOVE_RADIUS) * MOUSE_PITCH_SPEED;
bool downPitching = (mouseLocationY > 0.f);
_head.setPitch(_head.getPitch() + (downPitching ? mousePitchAdd : -mousePitchAdd));
_head.setPitch(_head.getPitch() + (downPitching ? -mousePitchAdd : mousePitchAdd));
}
}

111
interface/src/SerialInterface.cpp
View file

@ -111,52 +111,61 @@ void SerialInterface::renderLevels(int width, int height) {
const int LEVEL_CORNER_Y = 200;
// Draw the numeric degree/sec values from the gyros
sprintf(val, "Yaw %4.1f", getLastYawRate());
sprintf(val, "Yaw %4.1f", _estimatedRotation.y);
drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y, 0.10, 0, 1.0, 1, val, 0, 1, 0);
sprintf(val, "Pitch %4.1f", getLastPitchRate());
sprintf(val, "Pitch %4.1f", _estimatedRotation.x);
drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 15, 0.10, 0, 1.0, 1, val, 0, 1, 0);
sprintf(val, "Roll %4.1f", getLastRollRate());
sprintf(val, "Roll %4.1f", _estimatedRotation.z);
drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 30, 0.10, 0, 1.0, 1, val, 0, 1, 0);
sprintf(val, "X %4.3f", _lastAccelX);
sprintf(val, "X %4.3f", _lastAcceleration.x - _gravity.x);
drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 45, 0.10, 0, 1.0, 1, val, 0, 1, 0);
sprintf(val, "Y %4.3f", _lastAccelY);
sprintf(val, "Y %4.3f", _lastAcceleration.y - _gravity.y);
drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 60, 0.10, 0, 1.0, 1, val, 0, 1, 0);
sprintf(val, "Z %4.3f", _lastAccelZ);
sprintf(val, "Z %4.3f", _lastAcceleration.z - _gravity.z);
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;
const float ACCEL_VIEW_SCALING = 10.f;
const float POSITION_SCALING = 400.f;
glLineWidth(2.0);
glColor4f(1, 1, 1, 1);
glBegin(GL_LINES);
// Gyro rates
// Rotation rates
glColor4f(1, 1, 1, 1);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y - 3);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + getLastYawRate(), LEVEL_CORNER_Y - 3);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 12);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + getLastPitchRate(), LEVEL_CORNER_Y + 12);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 27);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + getLastRollRate(), LEVEL_CORNER_Y + 27);
// Gyro Estimated Rotation
// Estimated Rotation
glColor4f(0, 1, 1, 1);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y - 1);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.y, LEVEL_CORNER_Y - 1);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 14);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.z, LEVEL_CORNER_Y + 14);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.x, LEVEL_CORNER_Y + 14);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 29);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.x, LEVEL_CORNER_Y + 29);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.z, LEVEL_CORNER_Y + 29);
// Acceleration
// Acceleration rates
glColor4f(1, 1, 1, 1);
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 + (int)((_lastAcceleration.x - _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 + (int)((_lastAcceleration.y - _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);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAcceleration.z - _gravity.z) * ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 72);
// Estimated Position
glColor4f(0, 1, 1, 1);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 44);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedPosition.x * POSITION_SCALING), LEVEL_CORNER_Y + 44);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 59);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedPosition.y * POSITION_SCALING), LEVEL_CORNER_Y + 59);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 74);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedPosition.z * POSITION_SCALING), LEVEL_CORNER_Y + 74);
glEnd();
// Draw green vertical centerline
@ -202,10 +211,8 @@ void SerialInterface::readData(float deltaTime) {
// From MPU-9150 register map, with setting on
// highest resolution = +/- 2G
_lastAccelX = ((float) accelXRate) * LSB_TO_METERS_PER_SECOND2;
_lastAccelY = ((float) accelYRate) * LSB_TO_METERS_PER_SECOND2;
_lastAccelZ = ((float) -accelZRate) * LSB_TO_METERS_PER_SECOND2;
_lastAcceleration = glm::vec3(accelXRate, accelYRate, -accelZRate) * LSB_TO_METERS_PER_SECOND2;
int rollRate, yawRate, pitchRate;
convertHexToInt(sensorBuffer + 22, rollRate);
@ -213,38 +220,41 @@ void SerialInterface::readData(float deltaTime) {
convertHexToInt(sensorBuffer + 30, pitchRate);
// 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.
_lastRollRate = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;
_lastYawRate = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
_lastPitchRate = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND;
const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f; // From MPU-9150 register map, 2000 deg/sec.
_lastRotationRates[0] = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND;
_lastRotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
_lastRotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;
// Update raw rotation estimates
_estimatedRotation += deltaTime * glm::vec3(_lastRollRate - _averageGyroRates[0],
_lastYawRate - _averageGyroRates[1],
_lastPitchRate - _averageGyroRates[2]);
_estimatedRotation += deltaTime * (_lastRotationRates - _averageRotationRates);
// Update estimated position and velocity
float const DECAY_VELOCITY = 0.95f;
float const DECAY_POSITION = 0.95f;
_estimatedVelocity += deltaTime * (_lastAcceleration - _averageAcceleration);
_estimatedPosition += deltaTime * _estimatedVelocity;
_estimatedVelocity *= DECAY_VELOCITY;
_estimatedPosition *= DECAY_POSITION;
//glm::vec3 baseline = glm::normalize(_gravity);
//glm::vec3 current = glm::normalize(_lastAcceleration);
// Accumulate a set of initial baseline readings for setting gravity
if (totalSamples == 0) {
_averageGyroRates[0] = _lastRollRate;
_averageGyroRates[1] = _lastYawRate;
_averageGyroRates[2] = _lastPitchRate;
_gravity.x = _lastAccelX;
_gravity.y = _lastAccelY;
_gravity.z = _lastAccelZ;
_averageRotationRates = _lastRotationRates;
_averageAcceleration = _lastAcceleration;
_gravity = _lastAcceleration;
}
else {
// Cumulate long term average to (hopefully) take DC bias out of rotation rates
_averageGyroRates[0] = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageGyroRates[0] +
1.f/(float)LONG_TERM_RATE_SAMPLES * _lastRollRate;
_averageGyroRates[1] = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageGyroRates[1] +
1.f/(float)LONG_TERM_RATE_SAMPLES * _lastYawRate;
_averageGyroRates[2] = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageGyroRates[2] +
1.f/(float)LONG_TERM_RATE_SAMPLES * _lastPitchRate;
_averageRotationRates = (1.f - 1.f / (float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates
+ 1.f / (float)LONG_TERM_RATE_SAMPLES * _lastRotationRates;
_averageAcceleration = (1.f - 1.f / (float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration
+ 1.f / (float)LONG_TERM_RATE_SAMPLES * _lastAcceleration;
if (totalSamples < GRAVITY_SAMPLES) {
_gravity = (1.f - 1.f/(float)GRAVITY_SAMPLES) * _gravity +
1.f/(float)GRAVITY_SAMPLES * glm::vec3(_lastAccelX, _lastAccelY, _lastAccelZ);
1.f/(float)GRAVITY_SAMPLES * _lastAcceleration;
}
}
@ -268,7 +278,12 @@ void SerialInterface::readData(float deltaTime) {
void SerialInterface::resetAverages() {
totalSamples = 0;
_gravity = glm::vec3(0, 0, 0);
_averageGyroRates = glm::vec3(0, 0, 0);
_averageRotationRates = glm::vec3(0, 0, 0);
_averageAcceleration = glm::vec3(0, 0, 0);
_lastRotationRates = glm::vec3(0, 0, 0);
_estimatedRotation = glm::vec3(0, 0, 0);
_estimatedPosition = glm::vec3(0, 0, 0);
_estimatedVelocity = glm::vec3(0, 0, 0);
}
void SerialInterface::resetSerial() {

56
interface/src/SerialInterface.h
View file

@ -20,41 +20,32 @@
#include <dirent.h>
#endif
#define NUM_CHANNELS 6
// Acceleration sensors, in screen/world coord system (X = left/right, Y = Up/Down, Z = fwd/back)
#define ACCEL_X 3
#define ACCEL_Y 4
#define ACCEL_Z 5
// Gyro sensors, in coodinate system of head/airplane
#define HEAD_PITCH_RATE 1
#define HEAD_YAW_RATE 0
#define HEAD_ROLL_RATE 2
extern const bool USING_INVENSENSE_MPU9150;
class SerialInterface {
public:
SerialInterface() : active(false),
_gravity(0,0,0),
_averageGyroRates(0, 0, 0),
_gravity(0, 0, 0),
_averageRotationRates(0, 0, 0),
_averageAcceleration(0, 0, 0),
_estimatedRotation(0, 0, 0),
_lastAccelX(0),
_lastAccelY(0),
_lastAccelZ(0),
_lastYawRate(0),
_lastPitchRate(0),
_lastRollRate(0) {}
_estimatedPosition(0, 0, 0),
_estimatedVelocity(0, 0, 0),
_lastAcceleration(0, 0, 0),
_lastRotationRates(0, 0, 0)
{}
void pair();
void readData(float deltaTime);
float getLastYawRate() const { return _lastYawRate - _averageGyroRates[1]; }
float getLastPitchRate() const { return _lastPitchRate - _averageGyroRates[2]; }
float getLastRollRate() const { return _lastRollRate - _averageGyroRates[0]; }
glm::vec3 getLastAcceleration() { return glm::vec3(_lastAccelX, _lastAccelY, _lastAccelZ); };
glm::vec3 getGravity() {return _gravity;};
const float getLastPitchRate() const { return _lastRotationRates[0] - _averageRotationRates[0]; }
const float getLastYawRate() const { return _lastRotationRates[1] - _averageRotationRates[1]; }
const float getLastRollRate() const { return _lastRotationRates[2] - _averageRotationRates[2]; }
const glm::vec3& getLastRotationRates() const { return _lastRotationRates; };
const glm::vec3& getEstimatedRotation() const { return _estimatedRotation; };
const glm::vec3& getEstimatedPosition() const { return _estimatedPosition; };
const glm::vec3& getEstimatedVelocity() const { return _estimatedVelocity; };
const glm::vec3& getLastAcceleration() const { return _lastAcceleration; };
const glm::vec3& getGravity() const { return _gravity; };
void renderLevels(int width, int height);
void resetAverages();
@ -68,14 +59,13 @@ private:
int totalSamples;
timeval lastGoodRead;
glm::vec3 _gravity;
glm::vec3 _averageGyroRates;
glm::vec3 _averageRotationRates;
glm::vec3 _averageAcceleration;
glm::vec3 _estimatedRotation;
float _lastAccelX;
float _lastAccelY;
float _lastAccelZ;
float _lastYawRate; // Rates are in degrees per second.
float _lastPitchRate;
float _lastRollRate;
glm::vec3 _estimatedPosition;
glm::vec3 _estimatedVelocity;
glm::vec3 _lastAcceleration;
glm::vec3 _lastRotationRates;
};
#endif