mirror of
https://github.com/lubosz/overte.git
synced 2025-04-24 09:23:17 +02:00
remove the SerialInterface
This commit is contained in:
Stephen Birarda
parent
987c639e36
commit
4ea14a51fc
8 changed files with 2 additions and 565 deletions
|
|
@ -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();
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -24,7 +24,6 @@
|
|||
#include "BuckyBalls.h"
|
||||
#include "InterfaceConfig.h"
|
||||
#include "world.h"
|
||||
#include "devices/SerialInterface.h"
|
||||
#include "VoxelSystem.h"
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -21,7 +21,6 @@
|
|||
#include "InterfaceConfig.h"
|
||||
#include "VideoFace.h"
|
||||
#include "world.h"
|
||||
#include "devices/SerialInterface.h"
|
||||
#include "renderer/TextureCache.h"
|
||||
|
||||
enum eyeContactTargets {
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
||||
|
|
|
|||
|
|
@ -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(®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);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
Loading…
Reference in a new issue