remove the SerialInterface

interface/src/Application.cpp

@@ -95,7 +95,6 @@ Application::Application(int& argc, char** argv, timeval &startup_time) :
         QApplication(argc, argv),
         _window(new QMainWindow(desktop())),
         _glWidget(new GLCanvas()),
-        _displayLevels(false),
         _frameCount(0),
         _fps(120.0f),
         _justStarted(true),
@@ -670,9 +669,7 @@ void Application::keyPressEvent(QKeyEvent* event) {
                 _audioScope.inputPaused = !_audioScope.inputPaused;
                 break;
             case Qt::Key_L:
-                if (!isShifted && !isMeta) {
+                if (isShifted) {
-                    _displayLevels = !_displayLevels;
-                } else if (isShifted) {
                     Menu::getInstance()->triggerOption(MenuOption::LodTools);
                 } else if (isMeta) {
                     Menu::getInstance()->triggerOption(MenuOption::Log);
@@ -1338,11 +1335,6 @@ void Application::timer() {
 
     gettimeofday(&_timerStart, NULL);
 
-    // if we haven't detected gyros, check for them now
-    if (!_serialHeadSensor.isActive()) {
-        _serialHeadSensor.pair();
-    }
-
     // ask the node list to check in with the domain server
     NodeList::getInstance()->sendDomainServerCheckIn();
 
@@ -2379,10 +2371,6 @@ void Application::updateSixense(float deltaTime) {
 void Application::updateSerialDevices(float deltaTime) {
     bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
     PerformanceWarning warn(showWarnings, "Application::updateSerialDevices()");
-
-    if (_serialHeadSensor.isActive()) {
-        _serialHeadSensor.readData(deltaTime);
-    }
 }
 
 void Application::updateThreads(float deltaTime) {
@@ -2626,41 +2614,6 @@ void Application::updateAvatar(float deltaTime) {
         _headMouseY -= headVelocity.x * HEADMOUSE_FACESHIFT_PITCH_SCALE;
     }
 
-    if (_serialHeadSensor.isActive()) {
-
-        //  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;
-        }
-
-        const float MIDPOINT_OF_SCREEN = 0.5;
-
-        // Only use gyro to set lookAt if mouse hasn't selected an avatar
-        if (!_lookatTargetAvatar) {
-
-            // Set lookAtPosition if an avatar is at the center of the screen
-            glm::vec3 screenCenterRayOrigin, screenCenterRayDirection;
-            _viewFrustum.computePickRay(MIDPOINT_OF_SCREEN, MIDPOINT_OF_SCREEN, screenCenterRayOrigin, screenCenterRayDirection);
-
-            glm::vec3 eyePosition;
-            updateLookatTargetAvatar(screenCenterRayOrigin, screenCenterRayDirection, eyePosition);
-            if (_lookatTargetAvatar) {
-                glm::vec3 myLookAtFromMouse(eyePosition);
-                _myAvatar.getHead().setLookAtPosition(myLookAtFromMouse);
-            }
-        }
-
-    }
-
     //  Constrain head-driven mouse to edges of screen
     _headMouseX = glm::clamp(_headMouseX, 0, _glWidget->width());
     _headMouseY = glm::clamp(_headMouseY, 0, _glWidget->height());
@@ -3327,8 +3280,7 @@ void Application::displayOverlay() {
 
        //noiseTest(_glWidget->width(), _glWidget->height());
 
-    if (Menu::getInstance()->isOptionChecked(MenuOption::HeadMouse)
+    if (Menu::getInstance()->isOptionChecked(MenuOption::HeadMouse)) {
-        && USING_INVENSENSE_MPU9150) {
         //  Display small target box at center or head mouse target that can also be used to measure LOD
         glColor3f(1.0, 1.0, 1.0);
         glDisable(GL_LINE_SMOOTH);
@@ -3364,9 +3316,6 @@ void Application::displayOverlay() {
         }
     }
 
-    //  Show detected levels from the serial I/O ADC channel sensors
-    if (_displayLevels) _serialHeadSensor.renderLevels(_glWidget->width(), _glWidget->height());
-
     //  Show hand transmitter data if detected
     if (_myTransmitter.isConnected()) {
         _myTransmitter.renderLevels(_glWidget->width(), _glWidget->height());
@@ -4119,9 +4068,6 @@ void Application::resetSensors() {
     _headMouseX = _mouseX = _glWidget->width() / 2;
     _headMouseY = _mouseY = _glWidget->height() / 2;
 
-    if (_serialHeadSensor.isActive()) {
-        _serialHeadSensor.resetAverages();
-    }
     _webcam.reset();
     _faceshift.reset();
 

interface/src/Application.h

@@ -54,7 +54,6 @@
 #include "avatar/MyAvatar.h"
 #include "avatar/Profile.h"
 #include "devices/Faceshift.h"
-#include "devices/SerialInterface.h"
 #include "devices/SixenseManager.h"
 #include "devices/Webcam.h"
 #include "renderer/AmbientOcclusionEffect.h"
@@ -155,7 +154,6 @@ public:
     VoxelTree* getClipboard() { return &_clipboard; }
     Environment* getEnvironment() { return &_environment; }
     bool isMouseHidden() const { return _mouseHidden; }
-    SerialInterface* getSerialHeadSensor() { return &_serialHeadSensor; }
     Webcam* getWebcam() { return &_webcam; }
     Faceshift* getFaceshift() { return &_faceshift; }
     SixenseManager* getSixenseManager() { return &_sixenseManager; }
@@ -339,10 +337,8 @@ private:
 
     BandwidthMeter _bandwidthMeter;
 
-    SerialInterface _serialHeadSensor;
     QNetworkAccessManager* _networkAccessManager;
     QSettings* _settings;
-    bool _displayLevels;
 
     glm::vec3 _gravity;
 

interface/src/avatar/Avatar.h

@@ -21,7 +21,6 @@
 #include "Skeleton.h"
 #include "SkeletonModel.h"
 #include "world.h"
-#include "devices/SerialInterface.h"
 #include "devices/Transmitter.h"
 
 static const float SCALING_RATIO = .05f;

interface/src/avatar/Hand.h

@@ -24,7 +24,6 @@
 #include "BuckyBalls.h"
 #include "InterfaceConfig.h"
 #include "world.h"
-#include "devices/SerialInterface.h"
 #include "VoxelSystem.h"
 
 

interface/src/avatar/Head.h

@@ -21,7 +21,6 @@
 #include "InterfaceConfig.h"
 #include "VideoFace.h"
 #include "world.h"
-#include "devices/SerialInterface.h"
 #include "renderer/TextureCache.h"
 
 enum eyeContactTargets {

interface/src/avatar/MyAvatar.cpp

@@ -299,7 +299,6 @@ const float MAX_PITCH = 90.0f;
 //  Update avatar head rotation with sensor data
 void MyAvatar::updateFromGyrosAndOrWebcam(bool turnWithHead) {
     Faceshift* faceshift = Application::getInstance()->getFaceshift();
-    SerialInterface* gyros = Application::getInstance()->getSerialHeadSensor();
     Webcam* webcam = Application::getInstance()->getWebcam();
     glm::vec3 estimatedPosition, estimatedRotation;
     
@@ -320,9 +319,6 @@ void MyAvatar::updateFromGyrosAndOrWebcam(bool turnWithHead) {
                 }
             }
         }
-    } else if (gyros->isActive()) {
-        estimatedRotation = gyros->getEstimatedRotation();
-    
     } else if (webcam->isActive()) {
         estimatedRotation = webcam->getEstimatedRotation();
     

interface/src/devices/SerialInterface.cpp

@@ -1,422 +0,0 @@
-//
-//  SerialInterface.cpp
-//  2012 by Philip Rosedale for High Fidelity Inc.
-//
-//  Read interface data from the gyros/accelerometer Invensense board using the SerialUSB
-//
-
-#ifndef _WIN32
-#include <regex.h>
-#include <sys/time.h>
-#include <string>
-#endif
-
-#include <math.h>
-
-#include <glm/gtx/vector_angle.hpp>
-
-extern "C" {
-    #include <inv_tty.h>
-    #include <inv_mpu.h>
-}
-
-#include <SharedUtil.h>
-
-#include "Application.h"
-#include "SerialInterface.h"
-#include "Util.h"
-#include "Webcam.h"
-
-const short NO_READ_MAXIMUM_MSECS = 3000;
-const int GRAVITY_SAMPLES = 60;                     //  Use the first few samples to baseline values
-const int NORTH_SAMPLES = 30;
-const int ACCELERATION_SENSOR_FUSION_SAMPLES = 20;
-const int COMPASS_SENSOR_FUSION_SAMPLES = 100;
-const int LONG_TERM_RATE_SAMPLES = 1000;            
-
-const bool USING_INVENSENSE_MPU9150 = 1;
-
-SerialInterface::SerialInterface() :
-    _active(false),
-    _gravity(0, 0, 0),
-    _averageRotationRates(0, 0, 0),
-    _averageAcceleration(0, 0, 0),
-    _estimatedRotation(0, 0, 0),
-    _estimatedPosition(0, 0, 0),
-    _estimatedVelocity(0, 0, 0),
-    _lastAcceleration(0, 0, 0),
-    _lastRotationRates(0, 0, 0),
-    _compassMinima(-211, -132, -186),
-    _compassMaxima(89, 95, 98),
-    _angularVelocityToLinearAccel(0.003f, -0.001f, -0.006f,
-                                  -0.005f, -0.001f, -0.006f,
-                                  0.010f, 0.004f, 0.007f),
-    _angularAccelToLinearAccel(0.0f, 0.0f, 0.002f,
-                               0.0f, 0.0f, 0.001f,
-                               -0.002f, -0.002f, 0.0f)
-{
-
-}
-
-void SerialInterface::pair() {
-    
-#ifndef _WIN32
-    // look for a matching gyro setup
-    DIR *devDir;
-    struct dirent *entry;
-    int matchStatus;
-    regex_t regex;
-    
-    // for now this only works on OS X, where the usb serial shows up as /dev/tty.usb*,
-    // and (possibly just Ubuntu) Linux, where it shows up as /dev/ttyACM*
-    if((devDir = opendir("/dev"))) {
-        while((entry = readdir(devDir))) {
-#ifdef __APPLE__
-            regcomp(&regex, "tty\\.usb", REG_EXTENDED|REG_NOSUB);
-#else
-            regcomp(&regex, "ttyACM", REG_EXTENDED|REG_NOSUB);
-#endif
-            matchStatus = regexec(&regex, entry->d_name, (size_t) 0, NULL, 0);
-            if (matchStatus == 0) {
-                char *serialPortname = new char[100];
-                sprintf(serialPortname, "/dev/%s", entry->d_name);
-                
-                initializePort(serialPortname);
-                
-                delete [] serialPortname;
-            }
-            regfree(&regex);
-        }
-        closedir(devDir);
-    }    
-#endif
-}
-
-//  connect to the serial port
-void SerialInterface::initializePort(char* portname) {
-#ifndef _WIN32
-    _serialDescriptor = open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
-    
-    qDebug("Opening SerialUSB %s: ", portname);
-    
-    if (_serialDescriptor == -1) {
-        qDebug("Failed.\n");
-        return;
-    }
-    
-    struct termios options;
-    tcgetattr(_serialDescriptor, &options);
-        
-    options.c_cflag |= (CLOCAL | CREAD | CS8);
-    options.c_cflag &= ~PARENB;
-    options.c_cflag &= ~CSTOPB;
-    options.c_cflag &= ~CSIZE;
-    tcsetattr(_serialDescriptor, TCSANOW, &options);
-    
-    cfsetispeed(&options,B115200);
-    cfsetospeed(&options,B115200);
-    
-    if (USING_INVENSENSE_MPU9150) {
-        // block on invensense reads until there is data to read
-        int currentFlags = fcntl(_serialDescriptor, F_GETFL);
-        fcntl(_serialDescriptor, F_SETFL, currentFlags & ~O_NONBLOCK);
-        
-        // make sure there's nothing queued up to be read
-        tcflush(_serialDescriptor, TCIOFLUSH);
-        
-        // this disables streaming so there's no garbage data on reads
-        if (write(_serialDescriptor, "SD\n", 3) != 3) {
-            qDebug("Failed.\n");
-            return;
-        }
-        char result[4];
-        if (read(_serialDescriptor, result, 4) != 4) {
-            qDebug("Failed.\n");
-            return;
-        }
-        
-        tty_set_file_descriptor(_serialDescriptor);
-        mpu_init(0);
-        mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
-    }
-    
-    qDebug("Connected.\n");
-    resetSerial();
-    
-    _active = true;
- #endif
-}
-
-//  Render the serial interface channel values onscreen as vertical lines
-void SerialInterface::renderLevels(int width, int height) {
-    char val[40];
-    if (USING_INVENSENSE_MPU9150) {
-        //  For invensense gyros, render as horizontal bars
-        const int LEVEL_CORNER_X = 10;
-        const int LEVEL_CORNER_Y = 200;
-        
-        // Draw the numeric degree/sec values from the gyros
-        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", _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", _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", _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", _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", _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 = 10.f;
-        const float POSITION_SCALING = 400.f;
-
-        glLineWidth(2.0);
-        glBegin(GL_LINES);
-        // 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);
-        // 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.x, LEVEL_CORNER_Y + 14);
-        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 29);
-        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.z, LEVEL_CORNER_Y + 29);
-
-        // Acceleration rates
-        glColor4f(1, 1, 1, 1);
-        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 42);
-        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedAcceleration.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)(_estimatedAcceleration.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)(_estimatedAcceleration.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
-        glColor4f(0, 1, 0, 0.5);
-        glBegin(GL_LINES);
-        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y - 6);
-        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 30);
-        glEnd();
-    }
-}
-
-void SerialInterface::readData(float deltaTime) {
-#ifndef _WIN32
-    
-    int initialSamples = totalSamples;
-    
-    if (USING_INVENSENSE_MPU9150) { 
-
-        // ask the invensense for raw gyro data
-        short accelData[3];
-        if (mpu_get_accel_reg(accelData, 0)) {
-            close(_serialDescriptor);
-            qDebug("Disconnected SerialUSB.\n");
-            _active = false;
-            return; // disconnected
-        }
-        
-        const float LSB_TO_METERS_PER_SECOND2 = 1.f / 16384.f * GRAVITY_EARTH;
-                                                                //  From MPU-9150 register map, with setting on
-                                                                //  highest resolution = +/- 2G
-        
-        _lastAcceleration = glm::vec3(-accelData[2], -accelData[1], -accelData[0]) * LSB_TO_METERS_PER_SECOND2;
-          
-        short gyroData[3];
-        mpu_get_gyro_reg(gyroData, 0);
-        
-        //  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.
-        glm::vec3 rotationRates;
-        rotationRates[0] = ((float) -gyroData[2]) * LSB_TO_DEGREES_PER_SECOND;
-        rotationRates[1] = ((float) -gyroData[1]) * LSB_TO_DEGREES_PER_SECOND;
-        rotationRates[2] = ((float) -gyroData[0]) * LSB_TO_DEGREES_PER_SECOND;
-      
-        short compassData[3];
-        mpu_get_compass_reg(compassData, 0);
-      
-        // Convert integer values to floats, update extents
-        _lastCompass = glm::vec3(compassData[2], -compassData[0], -compassData[1]);
-        
-        // update and subtract the long term average
-        _averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates +
-                1.f/(float)LONG_TERM_RATE_SAMPLES * rotationRates;
-        rotationRates -= _averageRotationRates;
-
-        // compute the angular acceleration
-        glm::vec3 angularAcceleration = (deltaTime < EPSILON) ? glm::vec3() : (rotationRates - _lastRotationRates) / deltaTime;
-        _lastRotationRates = rotationRates;
-        
-        //  Update raw rotation estimates
-        glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
-            glm::quat(glm::radians(deltaTime * _lastRotationRates));
-        
-        //  Update acceleration estimate: first, subtract gravity as rotated into current frame
-        _estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() :
-            _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
-        
-        // update and subtract the long term average
-        _averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration +
-                1.f/(float)LONG_TERM_RATE_SAMPLES * _estimatedAcceleration;
-        _estimatedAcceleration -= _averageAcceleration;
-        
-        //  Consider updating our angular velocity/acceleration to linear acceleration mapping
-        if (glm::length(_estimatedAcceleration) > EPSILON &&
-                (glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON)) {
-            // compute predicted linear acceleration, find error between actual and predicted
-            glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates +
-                _angularAccelToLinearAccel * angularAcceleration;
-            glm::vec3 error = _estimatedAcceleration - predictedAcceleration;
-            
-            // the "error" is actually what we want: the linear acceleration minus rotational influences
-            _estimatedAcceleration = error;
-            
-            // adjust according to error in each dimension, in proportion to input magnitudes
-            for (int i = 0; i < 3; i++) {
-                if (fabsf(error[i]) < EPSILON) {
-                    continue;
-                }
-                const float LEARNING_RATE = 0.001f;
-                float rateSum = fabsf(_lastRotationRates.x) + fabsf(_lastRotationRates.y) + fabsf(_lastRotationRates.z);
-                if (rateSum > EPSILON) {
-                    for (int j = 0; j < 3; j++) {
-                        float proportion = LEARNING_RATE * fabsf(_lastRotationRates[j]) / rateSum;
-                        if (proportion > EPSILON) {
-                            _angularVelocityToLinearAccel[j][i] += error[i] * proportion / _lastRotationRates[j];
-                        }
-                    }
-                }
-                float accelSum = fabsf(angularAcceleration.x) + fabsf(angularAcceleration.y) + fabsf(angularAcceleration.z);
-                if (accelSum > EPSILON) {
-                    for (int j = 0; j < 3; j++) {
-                        float proportion = LEARNING_RATE * fabsf(angularAcceleration[j]) / accelSum;
-                        if (proportion > EPSILON) {
-                            _angularAccelToLinearAccel[j][i] += error[i] * proportion / angularAcceleration[j];
-                        }
-                    }                
-                }
-            }
-        }
-        
-        // rotate estimated acceleration into global rotation frame
-        _estimatedAcceleration = estimatedRotation * _estimatedAcceleration;
-        
-        //  Update estimated position and velocity
-        float const DECAY_VELOCITY = 0.975f;
-        float const DECAY_POSITION = 0.975f;
-        _estimatedVelocity += deltaTime * _estimatedAcceleration;
-        _estimatedPosition += deltaTime * _estimatedVelocity;
-        _estimatedVelocity *= DECAY_VELOCITY;
-        
-        //  Attempt to fuse gyro position with webcam position
-        Webcam* webcam = Application::getInstance()->getWebcam();
-        if (webcam->isActive()) {
-            const float WEBCAM_POSITION_FUSION = 0.5f;
-            _estimatedPosition = glm::mix(_estimatedPosition, webcam->getEstimatedPosition(), WEBCAM_POSITION_FUSION);
-               
-        } else {
-            _estimatedPosition *= DECAY_POSITION;
-        }
-            
-        //  Accumulate a set of initial baseline readings for setting gravity
-        if (totalSamples == 0) {
-            _gravity = _lastAcceleration;
-        } 
-        else {
-            if (totalSamples < GRAVITY_SAMPLES) {
-                _gravity = glm::mix(_gravity, _lastAcceleration, 1.0f / GRAVITY_SAMPLES);
-                
-                //  North samples start later, because the initial compass readings are screwy
-                int northSample = totalSamples - (GRAVITY_SAMPLES - NORTH_SAMPLES);
-                if (northSample == 0) {
-                    _north = _lastCompass;
-                    
-                } else if (northSample > 0) {
-                    _north = glm::mix(_north, _lastCompass, 1.0f / NORTH_SAMPLES);
-                }
-            } else {
-                //  Use gravity reading to do sensor fusion on the pitch and roll estimation
-                estimatedRotation = safeMix(estimatedRotation,
-                    rotationBetween(estimatedRotation * _lastAcceleration, _gravity) * estimatedRotation,
-                    1.0f / ACCELERATION_SENSOR_FUSION_SAMPLES);
-                
-                //  Update the compass extents
-                _compassMinima = glm::min(_compassMinima, _lastCompass);
-                _compassMaxima = glm::max(_compassMaxima, _lastCompass);
-        
-                //  Same deal with the compass heading
-                estimatedRotation = safeMix(estimatedRotation,
-                    rotationBetween(estimatedRotation * recenterCompass(_lastCompass),
-                        recenterCompass(_north)) * estimatedRotation,
-                    1.0f / COMPASS_SENSOR_FUSION_SAMPLES);
-            }
-        }
-        
-        _estimatedRotation = safeEulerAngles(estimatedRotation); 
-        
-        totalSamples++;
-    } 
-    
-    if (initialSamples == totalSamples) {        
-        timeval now;
-        gettimeofday(&now, NULL);
-        
-        if (diffclock(&lastGoodRead, &now) > NO_READ_MAXIMUM_MSECS) {
-            qDebug("No data - Shutting down SerialInterface.\n");
-            resetSerial();
-        }
-    } else {
-        gettimeofday(&lastGoodRead, NULL);
-    }
-#endif
-}
-
-void SerialInterface::resetAverages() {
-    totalSamples = 0;
-    _gravity = 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);
-    _estimatedAcceleration = glm::vec3(0, 0, 0);
-}
-
-void SerialInterface::resetSerial() {
-#ifndef _WIN32
-    resetAverages();
-    _active = false;
-    gettimeofday(&lastGoodRead, NULL);
-#endif
-}
-
-glm::vec3 SerialInterface::recenterCompass(const glm::vec3& compass) {
-    // compensate for "hard iron" distortion by subtracting the midpoint on each axis; see
-    // http://www.sensorsmag.com/sensors/motion-velocity-displacement/compensating-tilt-hard-iron-and-soft-iron-effects-6475
-    return (compass - (_compassMinima + _compassMaxima) * 0.5f) / (_compassMaxima - _compassMinima);
-}
-
-

interface/src/devices/SerialInterface.h

@@ -1,76 +0,0 @@
-//
-//  SerialInterface.h
-//  hifi
-//
-//  Created by Stephen Birarda on 2/15/13.
-//  Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
-//
-
-#ifndef __interface__SerialInterface__
-#define __interface__SerialInterface__
-
-// These includes are for serial port reading/writing
-#ifndef _WIN32
-#include <unistd.h>
-#include <fcntl.h>
-#include <termios.h>
-#include <dirent.h>
-#endif
-
-#include <glm/glm.hpp>
-
-#include "InterfaceConfig.h"
-#include "Util.h"
-
-extern const bool USING_INVENSENSE_MPU9150;
-
-class SerialInterface {
-public:
-    SerialInterface();
-    
-    void pair();
-    void readData(float deltaTime);
-    const float getLastPitchRate() const { return _lastRotationRates[0]; }
-    const float getLastYawRate() const { return _lastRotationRates[1]; }
-    const float getLastRollRate() const { return _lastRotationRates[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& getEstimatedAcceleration() const { return _estimatedAcceleration; };
-    const glm::vec3& getLastAcceleration() const { return _lastAcceleration; };
-    const glm::vec3& getGravity() const { return _gravity; };
-    
-    void renderLevels(int width, int height);
-    void resetAverages();
-    bool isActive() const { return _active; }
-    
-private:
-    void initializePort(char* portname);
-    void resetSerial();
-
-    glm::vec3 recenterCompass(const glm::vec3& compass);
-
-    bool _active;
-    int _serialDescriptor;
-    int totalSamples;
-    timeval lastGoodRead;
-    glm::vec3 _gravity;
-    glm::vec3 _north;
-    glm::vec3 _averageRotationRates;
-    glm::vec3 _averageAcceleration;
-    glm::vec3 _estimatedRotation;
-    glm::vec3 _estimatedPosition;
-    glm::vec3 _estimatedVelocity;
-    glm::vec3 _estimatedAcceleration;
-    glm::vec3 _lastAcceleration;
-    glm::vec3 _lastRotationRates;
-    glm::vec3 _lastCompass;
-    glm::vec3 _compassMinima;
-    glm::vec3 _compassMaxima;
-    
-    glm::mat3 _angularVelocityToLinearAccel;
-    glm::mat3 _angularAccelToLinearAccel;
-};
-
-#endif