diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 6583af5484..dd181b63e5 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -269,16 +269,10 @@ void Avatar::updateHeadFromGyros(float frametime, SerialInterface* serialInterfa float measuredRollRate = 0.0f; float measuredYawRate = 0.0f; - if (serialInterface->active && USING_INVENSENSE_MPU9150) { - measuredPitchRate = serialInterface->getLastPitchRate(); - measuredYawRate = serialInterface->getLastYawRate(); - measuredRollRate = serialInterface->getLastRollRate(); - } else { - measuredPitchRate = serialInterface->getRelativeValue(HEAD_PITCH_RATE); - measuredYawRate = serialInterface->getRelativeValue(HEAD_YAW_RATE); - measuredRollRate = serialInterface->getRelativeValue(HEAD_ROLL_RATE); - } - + measuredPitchRate = serialInterface->getLastPitchRate(); + measuredYawRate = serialInterface->getLastYawRate(); + measuredRollRate = serialInterface->getLastRollRate(); + // Update avatar head position based on measured gyro rates const float MAX_PITCH = 45; const float MIN_PITCH = -45; @@ -521,14 +515,12 @@ void Avatar::updateHead(float deltaTime) { } // For invensense gyro, decay only slightly when roughly centered - if (USING_INVENSENSE_MPU9150) { - const float RETURN_RANGE = 5.0; - const float RETURN_STRENGTH = 1.0; - if (fabs(_headPitch) < RETURN_RANGE) { _headPitch *= (1.0f - RETURN_STRENGTH * deltaTime); } - if (fabs(_headYaw) < RETURN_RANGE) { _headYaw *= (1.0f - RETURN_STRENGTH * deltaTime); } - if (fabs(_headRoll) < RETURN_RANGE) { _headRoll *= (1.0f - RETURN_STRENGTH * deltaTime); } + const float RETURN_RANGE = 15.0; + const float RETURN_STRENGTH = 2.0; + if (fabs(_headPitch) < RETURN_RANGE) { _headPitch *= (1.0f - RETURN_STRENGTH * deltaTime); } + if (fabs(_headYaw) < RETURN_RANGE) { _headYaw *= (1.0f - RETURN_STRENGTH * deltaTime); } + if (fabs(_headRoll) < RETURN_RANGE) { _headRoll *= (1.0f - RETURN_STRENGTH * deltaTime); } - } if (_head.noise) { // Move toward new target diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index 2b21f5062a..7aea1a46d9 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -131,43 +131,10 @@ void SerialInterface::initializePort(char* portname, int baud) { #endif } -// Reset Trailing averages to the current measurement -void SerialInterface::resetTrailingAverages() { - for (int i = 1; i < NUM_CHANNELS; i++) trailingAverage[i] = lastMeasured[i]; -} - // Render the serial interface channel values onscreen as vertical lines void SerialInterface::renderLevels(int width, int height) { - int i; - int displayX = 10; - const int GAP = 16; char val[40]; - if (!USING_INVENSENSE_MPU9150) { - // Legacy - Maple ADC gyros - for(i = 0; i < NUM_CHANNELS; i++) { - // Actual value - glLineWidth(2.0); - glColor4f(1, 1, 1, 1); - glBegin(GL_LINES); - glVertex2f(displayX, height * 0.95); - glVertex2f(displayX, height * (0.25 + 0.75f * getValue(i) / 4096)); - glColor4f(1, 0, 0, 1); - glVertex2f(displayX - 3, height * (0.25 + 0.75f * getValue(i) / 4096)); - glVertex2f(displayX, height * (0.25 + 0.75f * getValue(i) / 4096)); - glEnd(); - // Trailing Average value - glBegin(GL_LINES); - glColor4f(1, 1, 1, 1); - glVertex2f(displayX, height * (0.25 + 0.75f * getTrailingValue(i) / 4096)); - glVertex2f(displayX + 4, height * (0.25 + 0.75f * getTrailingValue(i) / 4096)); - glEnd(); - - sprintf(val, "%d", getValue(i)); - drawtext(displayX - GAP / 2, (height * 0.95) + 2, 0.08, 90, 1.0, 0, val, 0, 1, 0); - - displayX += GAP; - } - } else { + if (USING_INVENSENSE_MPU9150) { // For invensense gyros, render as horizontal bars const int LEVEL_CORNER_X = 10; const int LEVEL_CORNER_Y = 200; @@ -218,18 +185,6 @@ void SerialInterface::renderLevels(int width, int height) { glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 30); glEnd(); } - - // Display Serial latency block - if (LED) { - glColor3f(1,0,0); - glBegin(GL_QUADS); { - glVertex2f(width - 100, height - 100); - glVertex2f(width, height - 100); - glVertex2f(width, height); - glVertex2f(width - 100, height); - } - glEnd(); - } } void convertHexToInt(unsigned char* sourceBuffer, int& destinationInt) { @@ -263,6 +218,8 @@ void SerialInterface::readData() { convertHexToInt(sensorBuffer + 14, accelXRate); const float LSB_TO_METERS_PER_SECOND2 = 1.f / 16384.f * 9.80665f; + // From MPU-9150 register map, with setting on + // highest resolution = +/- 2G _lastAccelX = ((float) accelXRate) * LSB_TO_METERS_PER_SECOND2; _lastAccelY = ((float) accelYRate) * LSB_TO_METERS_PER_SECOND2; @@ -296,60 +253,7 @@ void SerialInterface::readData() { } totalSamples++; - } else { - // This array sets the rate of trailing averaging for each channel: - // If the sensor rate is 100Hz, 0.001 will make the long term average a 10-second average - const float AVG_RATE[] = {0.002, 0.002, 0.002, 0.002, 0.002, 0.002}; - char bufchar[1]; - - while (read(_serialDescriptor, &bufchar, 1) > 0) { - serialBuffer[serialBufferPos] = bufchar[0]; - serialBufferPos++; - // Have we reached end of a line of input? - if ((bufchar[0] == '\n') || (serialBufferPos >= MAX_BUFFER)) { - std::string serialLine(serialBuffer, serialBufferPos-1); - //printLog("%s\n", serialLine.c_str()); - int spot; - //int channel = 0; - std::string val; - for (int i = 0; i < NUM_CHANNELS + 2; i++) { - spot = serialLine.find_first_of(" ", 0); - if (spot != std::string::npos) { - val = serialLine.substr(0,spot); - //printLog("%s\n", val.c_str()); - if (i < NUM_CHANNELS) lastMeasured[i] = atoi(val.c_str()); - else samplesAveraged = atoi(val.c_str()); - } else LED = atoi(serialLine.c_str()); - serialLine = serialLine.substr(spot+1, serialLine.length() - spot - 1); - } - - // Update Trailing Averages - for (int i = 0; i < NUM_CHANNELS; i++) { - if (totalSamples > SAMPLES_TO_DISCARD) { - trailingAverage[i] = (1.f - AVG_RATE[i])*trailingAverage[i] + - AVG_RATE[i]*(float)lastMeasured[i]; - } else { - trailingAverage[i] = (float)lastMeasured[i]; - } - - } - /* - // Use a set of initial samples to compute gravity - if (totalSamples < GRAVITY_SAMPLES) { - gravity.x += lastMeasured[ACCEL_X]; - gravity.y += lastMeasured[ACCEL_Y]; - gravity.z += lastMeasured[ACCEL_Z]; - } - if (totalSamples == GRAVITY_SAMPLES) { - gravity = glm::normalize(gravity); - printLog("gravity: %f,%f,%f\n", gravity.x, gravity.y, gravity.z); - } - */ - totalSamples++; - serialBufferPos = 0; - } - } - } + } if (initialSamples == totalSamples) { timeval now; @@ -373,18 +277,6 @@ void SerialInterface::resetSerial() { gettimeofday(&lastGoodRead, NULL); - if (!USING_INVENSENSE_MPU9150) { - // Clear the measured and average channel data - for (int i = 0; i < NUM_CHANNELS; i++) { - lastMeasured[i] = 0; - trailingAverage[i] = 0.0; - } - // Clear serial input buffer - for (int i = 1; i < MAX_BUFFER; i++) { - serialBuffer[i] = ' '; - } - } - #endif } diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h index 0de8de7ff0..2858d2aee7 100644 --- a/interface/src/SerialInterface.h +++ b/interface/src/SerialInterface.h @@ -32,7 +32,7 @@ #define HEAD_YAW_RATE 0 #define HEAD_ROLL_RATE 2 -extern const bool USING_INVENSENSE_MPU9150; +//const bool USING_INVENSENSE_MPU9150; class SerialInterface { public: @@ -51,13 +51,12 @@ public: float getLastPitchRate() const { return _lastPitchRate; } float getLastRollRate() const { return _lastRollRate; } - int getLED() {return LED;}; - int getNumSamples() {return samplesAveraged;}; - int getValue(int num) {return lastMeasured[num];}; - int getRelativeValue(int num) {return static_cast(lastMeasured[num] - trailingAverage[num]);}; - float getTrailingValue(int num) {return trailingAverage[num];}; + //int getLED() {return LED;}; + //int getNumSamples() {return samplesAveraged;}; + //int getValue(int num) {return lastMeasured[num];}; + //int getRelativeValue(int num) {return static_cast(lastMeasured[num] - trailingAverage[num]);}; + //float getTrailingValue(int num) {return trailingAverage[num];}; - void resetTrailingAverages(); void renderLevels(int width, int height); bool active; glm::vec3 getGravity() {return _gravity;}; @@ -67,10 +66,6 @@ private: void resetSerial(); int _serialDescriptor; - int lastMeasured[NUM_CHANNELS]; - float trailingAverage[NUM_CHANNELS]; - int samplesAveraged; - int LED; int totalSamples; timeval lastGoodRead; glm::vec3 _gravity; diff --git a/interface/src/main.cpp b/interface/src/main.cpp index 957c418b16..654f9143f8 100644 --- a/interface/src/main.cpp +++ b/interface/src/main.cpp @@ -365,10 +365,6 @@ void reset_sensors() { headMouseY = HEIGHT/2; myAvatar.reset(); - - if (serialPort.active) { - serialPort.resetTrailingAverages(); - } } // @@ -380,15 +376,9 @@ void updateAvatar(float deltaTime) { myAvatar.updateHeadFromGyros(deltaTime, &serialPort, &gravity); // Grab latest readings from the gyros - float measuredYawRate, measuredPitchRate; - if (USING_INVENSENSE_MPU9150) { - measuredPitchRate = serialPort.getLastPitchRate(); - measuredYawRate = serialPort.getLastYawRate(); - } else { - measuredPitchRate = serialPort.getRelativeValue(HEAD_PITCH_RATE); - measuredYawRate = serialPort.getRelativeValue(HEAD_YAW_RATE); - } - + float measuredPitchRate = serialPort.getLastPitchRate(); + float measuredYawRate = serialPort.getLastYawRate(); + // Update gyro-based mouse (X,Y on screen) const float MIN_MOUSE_RATE = 30.0; const float MOUSE_SENSITIVITY = 0.1f; @@ -1714,7 +1704,7 @@ void idle(void) { } // Read serial port interface devices - if (serialPort.active && USING_INVENSENSE_MPU9150) { + if (serialPort.active) { serialPort.readData(); } @@ -1748,10 +1738,6 @@ void idle(void) { lastTimeIdle = check; } - // Read serial data - if (serialPort.active && !USING_INVENSENSE_MPU9150) { - serialPort.readData(); - } } void reshape(int width, int height) {