Merge pull request #476 from PhilipRosedale/master

Cleanup of serial interface code, added translation estimate

interface/src/Application.cpp

@@ -753,8 +753,8 @@ void Application::timer() {
     gettimeofday(&_timerStart, NULL);
     
     // if we haven't detected gyros, check for them now
-    if (!_serialPort.active) {
+    if (!_serialHeadSensor.active) {
-        _serialPort.pair();
+        _serialHeadSensor.pair();
     }
 }
 
@@ -892,8 +892,8 @@ void Application::idle() {
         }
        
         //  Read serial port interface devices
-        if (_serialPort.active) {
+        if (_serialHeadSensor.active) {
-            _serialPort.readData(deltaTime);
+            _serialHeadSensor.readData(deltaTime);
         }
         
         //  Update transmitter
@@ -1523,12 +1523,23 @@ void Application::init() {
 }
 
 void Application::updateAvatar(float 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, &_serialPort);
+        _myAvatar.updateHeadFromGyros(deltaTime, &_serialHeadSensor);
         
         //  Grab latest readings from the gyros
-    float measuredPitchRate = _serialPort.getLastPitchRate();
+        float measuredPitchRate = _serialHeadSensor.getLastPitchRate();
-    float measuredYawRate = _serialPort.getLastYawRate();
+        float measuredYawRate = _serialHeadSensor.getLastYawRate();
         
         //  Update gyro-based mouse (X,Y on screen)
         const float MIN_MOUSE_RATE = 3.0;
@@ -1544,6 +1555,9 @@ void Application::updateAvatar(float deltaTime) {
         _headMouseY = max(_headMouseY, 0);
         _headMouseY = min(_headMouseY, _glWidget->height());
 
+    }
+
+    
     if (OculusManager::isConnected()) {
         float yaw, pitch, roll;
         OculusManager::getEulerAngles(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) {
+    if (_serialHeadSensor.active) {
-        _serialPort.resetAverages();
+        _serialHeadSensor.resetAverages();
     }
     QCursor::setPos(_headMouseX, _headMouseY);
     _myAvatar.reset();

interface/src/Application.h

@@ -199,9 +199,9 @@ private:
     QAction* _frustumRenderModeAction;
     QAction* _settingsAutosave;      // Whether settings are saved automatically
     
+    SerialInterface _serialHeadSensor;
     QNetworkAccessManager* _networkAccessManager;
-    
+    QSettings* _settings;
-    SerialInterface _serialPort;
     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.
 };
 

interface/src/Avatar.cpp

@@ -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));
         }
         
     }

interface/src/SerialInterface.cpp

@@ -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 rates
-        // Acceleration
+        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),
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAcceleration.x - _gravity.x) *ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 42);
-                   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),
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAcceleration.y - _gravity.y) *ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 57);
-                   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),
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAcceleration.z - _gravity.z) * ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 72);
-                   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,9 +211,7 @@ void SerialInterface::readData(float deltaTime) {
                                                                 //  From MPU-9150 register map, with setting on
                                                                 //  highest resolution = +/- 2G
         
-        _lastAccelX = ((float) accelXRate) * LSB_TO_METERS_PER_SECOND2;
+        _lastAcceleration = glm::vec3(accelXRate, accelYRate, -accelZRate) * 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;
         
@@ -214,37 +221,40 @@ void SerialInterface::readData(float deltaTime) {
         
         //  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;
+        _lastRotationRates[0] = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND;
-        _lastYawRate = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
+        _lastRotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
-        _lastPitchRate = ((float) -pitchRate) * 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],
+        _estimatedRotation += deltaTime * (_lastRotationRates - _averageRotationRates);
-                                                    _lastYawRate - _averageGyroRates[1],
+        
-                                                    _lastPitchRate - _averageGyroRates[2]);
+        //  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;
+            _averageRotationRates = _lastRotationRates;
-            _averageGyroRates[1] = _lastYawRate;
+            _averageAcceleration = _lastAcceleration;
-            _averageGyroRates[2] = _lastPitchRate;
+            _gravity = _lastAcceleration;
-            _gravity.x = _lastAccelX;
-            _gravity.y = _lastAccelY;
-            _gravity.z = _lastAccelZ;
-
         } 
         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] +
+            _averageRotationRates = (1.f - 1.f / (float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates
-            1.f/(float)LONG_TERM_RATE_SAMPLES * _lastRollRate;
+                                    + 1.f / (float)LONG_TERM_RATE_SAMPLES * _lastRotationRates;
-            _averageGyroRates[1] =  (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageGyroRates[1] +
+            _averageAcceleration = (1.f - 1.f / (float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration
-            1.f/(float)LONG_TERM_RATE_SAMPLES * _lastYawRate;
+                                    + 1.f / (float)LONG_TERM_RATE_SAMPLES * _lastAcceleration;
-            _averageGyroRates[2] =  (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageGyroRates[2] +
-            1.f/(float)LONG_TERM_RATE_SAMPLES * _lastPitchRate;
             
             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() {

interface/src/SerialInterface.h

@@ -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),
+                        _gravity(0, 0, 0),
-                        _averageGyroRates(0, 0, 0),
+                        _averageRotationRates(0, 0, 0),
+                        _averageAcceleration(0, 0, 0),
                         _estimatedRotation(0, 0, 0),
-                        _lastAccelX(0),
+                        _estimatedPosition(0, 0, 0),
-                        _lastAccelY(0),
+                        _estimatedVelocity(0, 0, 0),
-                        _lastAccelZ(0),
+                        _lastAcceleration(0, 0, 0),
-                        _lastYawRate(0),
+                        _lastRotationRates(0, 0, 0)
-                        _lastPitchRate(0),
+                    {}
-                        _lastRollRate(0) {}
     
     void pair();
     void readData(float deltaTime);
-    
+    const float getLastPitchRate() const { return _lastRotationRates[0] - _averageRotationRates[0]; }
-    float getLastYawRate() const { return _lastYawRate - _averageGyroRates[1]; }
+    const float getLastYawRate() const { return _lastRotationRates[1] - _averageRotationRates[1]; }
-    float getLastPitchRate() const { return _lastPitchRate - _averageGyroRates[2]; }
+    const float getLastRollRate() const { return _lastRotationRates[2] - _averageRotationRates[2]; }
-    float getLastRollRate() const { return _lastRollRate - _averageGyroRates[0]; }
+    const glm::vec3& getLastRotationRates() const { return _lastRotationRates; };
-    glm::vec3 getLastAcceleration() { return glm::vec3(_lastAccelX, _lastAccelY, _lastAccelZ); };
+    const glm::vec3& getEstimatedRotation() const { return _estimatedRotation; };
-    glm::vec3 getGravity() {return _gravity;};
+    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;
+    glm::vec3 _estimatedPosition;
-    float _lastAccelY;
+    glm::vec3 _estimatedVelocity;
-    float _lastAccelZ;
+    glm::vec3 _lastAcceleration;
-    float _lastYawRate;         //  Rates are in degrees per second. 
+    glm::vec3 _lastRotationRates;
-    float _lastPitchRate;
-    float _lastRollRate;
 };
 
 #endif