From 4ea14a51fc2cb6f105997d6ab42062d5c21a9d61 Mon Sep 17 00:00:00 2001 From: Stephen Birarda Date: Tue, 14 Jan 2014 13:14:15 -0800 Subject: [PATCH] remove the SerialInterface --- interface/src/Application.cpp | 58 +-- interface/src/Application.h | 4 - interface/src/avatar/Avatar.h | 1 - interface/src/avatar/Hand.h | 1 - interface/src/avatar/Head.h | 1 - interface/src/avatar/MyAvatar.cpp | 4 - interface/src/devices/SerialInterface.cpp | 422 ---------------------- interface/src/devices/SerialInterface.h | 76 ---- 8 files changed, 2 insertions(+), 565 deletions(-) delete mode 100644 interface/src/devices/SerialInterface.cpp delete mode 100644 interface/src/devices/SerialInterface.h diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 2c1efdfa8e..7b56e22dd2 100644 --- a/interface/src/Application.cpp +++ b/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) { - _displayLevels = !_displayLevels; - } else if (isShifted) { + 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) - && USING_INVENSENSE_MPU9150) { + if (Menu::getInstance()->isOptionChecked(MenuOption::HeadMouse)) { // 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(); diff --git a/interface/src/Application.h b/interface/src/Application.h index 1da7de0224..0123cd5c60 100644 --- a/interface/src/Application.h +++ b/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; diff --git a/interface/src/avatar/Avatar.h b/interface/src/avatar/Avatar.h index 1a4511275a..0b0b7ebb1f 100755 --- a/interface/src/avatar/Avatar.h +++ b/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; diff --git a/interface/src/avatar/Hand.h b/interface/src/avatar/Hand.h index 8c368a4c37..8a672fc1d9 100755 --- a/interface/src/avatar/Hand.h +++ b/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" diff --git a/interface/src/avatar/Head.h b/interface/src/avatar/Head.h index 55c3dffc50..6d0f69d8ed 100644 --- a/interface/src/avatar/Head.h +++ b/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 { diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index dabbb7812d..ea1dd557ff 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/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(); diff --git a/interface/src/devices/SerialInterface.cpp b/interface/src/devices/SerialInterface.cpp deleted file mode 100644 index 4082bc87ef..0000000000 --- a/interface/src/devices/SerialInterface.cpp +++ /dev/null @@ -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 -#include -#include -#endif - -#include - -#include - -extern "C" { - #include - #include -} - -#include - -#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(®ex, "tty\\.usb", REG_EXTENDED|REG_NOSUB); -#else - regcomp(®ex, "ttyACM", REG_EXTENDED|REG_NOSUB); -#endif - matchStatus = regexec(®ex, 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(®ex); - } - 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); -} - - diff --git a/interface/src/devices/SerialInterface.h b/interface/src/devices/SerialInterface.h deleted file mode 100644 index 711ff56757..0000000000 --- a/interface/src/devices/SerialInterface.h +++ /dev/null @@ -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 -#include -#include -#include -#endif - -#include - -#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