From 9e3b862c6715c4c38135a6a29db29602eba10448 Mon Sep 17 00:00:00 2001 From: Stephen Birarda Date: Thu, 2 May 2013 12:26:12 -0700 Subject: [PATCH] hook up the invensense via existing SerialInterface class --- interface/src/Avatar.cpp | 46 ++++---- interface/src/SerialInterface.cpp | 179 +++++++++++++++++++----------- interface/src/SerialInterface.h | 11 ++ interface/src/main.cpp | 6 +- 4 files changed, 152 insertions(+), 90 deletions(-) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 21fa44e651..1f4f3cd1a7 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -226,45 +226,39 @@ void Avatar::reset() { _head.leanForward = _head.leanSideways = 0; } - -//this pertains to moving the head with the glasses -void Avatar::UpdateGyros(float frametime, SerialInterface * serialInterface, glm::vec3 * gravity) +// this pertains to moving the head with the glasses // Using serial data, update avatar/render position and angles -{ - const float PITCH_ACCEL_COUPLING = 0.5; - const float ROLL_ACCEL_COUPLING = -1.0; - float measured_pitch_rate = serialInterface->getRelativeValue(HEAD_PITCH_RATE); - _head.yawRate = serialInterface->getRelativeValue(HEAD_YAW_RATE); - float measured_lateral_accel = serialInterface->getRelativeValue(ACCEL_X) - - ROLL_ACCEL_COUPLING*serialInterface->getRelativeValue(HEAD_ROLL_RATE); - float measured_fwd_accel = serialInterface->getRelativeValue(ACCEL_Z) - - PITCH_ACCEL_COUPLING*serialInterface->getRelativeValue(HEAD_PITCH_RATE); - float measured_roll_rate = serialInterface->getRelativeValue(HEAD_ROLL_RATE); - - //printLog("Pitch Rate: %d ACCEL_Z: %d\n", serialInterface->getRelativeValue(PITCH_RATE), - // serialInterface->getRelativeValue(ACCEL_Z)); - //printLog("Pitch Rate: %d ACCEL_X: %d\n", serialInterface->getRelativeValue(PITCH_RATE), - // serialInterface->getRelativeValue(ACCEL_Z)); - //printLog("Pitch: %f\n", Pitch); +void Avatar::UpdateGyros(float frametime, SerialInterface* serialInterface, glm::vec3* gravity) { + float measured_pitch_rate = 0.0f; + float measured_roll_rate = 0.0f; + if (serialInterface->active && USING_INVENSENSE_MPU9150) { + measured_pitch_rate = serialInterface->getLastPitch(); + _head.yawRate = serialInterface->getLastYaw(); + measured_roll_rate = -1 * serialInterface->getLastRoll(); + } else { + measured_pitch_rate = serialInterface->getRelativeValue(HEAD_PITCH_RATE); + _head.yawRate = serialInterface->getRelativeValue(HEAD_YAW_RATE); + measured_roll_rate = serialInterface->getRelativeValue(HEAD_ROLL_RATE); + } // Update avatar head position based on measured gyro rates const float HEAD_ROTATION_SCALE = 0.70; const float HEAD_ROLL_SCALE = 0.40; - const float HEAD_LEAN_SCALE = 0.01; const float MAX_PITCH = 45; const float MIN_PITCH = -45; const float MAX_YAW = 85; const float MIN_YAW = -85; - if ((_headPitch < MAX_PITCH) && (_headPitch > MIN_PITCH)) + if ((_headPitch < MAX_PITCH) && (_headPitch > MIN_PITCH)) { addHeadPitch(measured_pitch_rate * -HEAD_ROTATION_SCALE * frametime); - + } + addHeadRoll(measured_roll_rate * HEAD_ROLL_SCALE * frametime); - if ((_headYaw < MAX_YAW) && (_headYaw > MIN_YAW)) - addHeadYaw(_head.yawRate * HEAD_ROTATION_SCALE * frametime); - - addLean(-measured_lateral_accel * frametime * HEAD_LEAN_SCALE, -measured_fwd_accel*frametime * HEAD_LEAN_SCALE); + if ((_headYaw < MAX_YAW) && (_headYaw > MIN_YAW)) { + addHeadYaw(_head.yawRate * HEAD_ROTATION_SCALE * frametime); + } + } float Avatar::getAbsoluteHeadYaw() const { diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index 9fe57ecaaf..44f256cd01 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -32,6 +32,8 @@ const short NO_READ_MAXIMUM_MSECS = 3000; const short SAMPLES_TO_DISCARD = 100; // Throw out the first few samples const int GRAVITY_SAMPLES = 200; // Use the first samples to compute gravity vector +const bool USING_INVENSENSE_MPU9150 = 1; + void SerialInterface::pair() { #ifdef __APPLE__ @@ -62,9 +64,8 @@ void SerialInterface::pair() { } -// Connect to the serial port -int SerialInterface::initializePort(char* portname, int baud) -{ +// connect to the serial port +int SerialInterface::initializePort(char* portname, int baud) { #ifdef __APPLE__ serialFd = open(portname, O_RDWR | O_NOCTTY | O_NDELAY); @@ -76,8 +77,8 @@ int SerialInterface::initializePort(char* portname, int baud) } struct termios options; tcgetattr(serialFd,&options); - switch(baud) - { + + switch(baud) { case 9600: cfsetispeed(&options,B9600); cfsetospeed(&options,B9600); break; @@ -94,16 +95,37 @@ int SerialInterface::initializePort(char* portname, int baud) cfsetospeed(&options,B9600); break; } + options.c_cflag |= (CLOCAL | CREAD); options.c_cflag &= ~PARENB; options.c_cflag &= ~CSTOPB; options.c_cflag &= ~CSIZE; options.c_cflag |= CS8; tcsetattr(serialFd,TCSANOW,&options); - + + if (USING_INVENSENSE_MPU9150) { + // block on invensense reads until there is data to read + int currentFlags = fcntl(serialFd, F_GETFL); + fcntl(serialFd, F_SETFL, currentFlags & ~O_NONBLOCK); + + // there are extra commands to send to the invensense when it fires up + + // this takes it out of SLEEP + write(serialFd, "WR686B01\n", 9); + + // delay after the wakeup + usleep(10000); + + // this disables streaming so there's no garbage data on reads + write(serialFd, "SD\n", 3); + + // flush whatever was produced by the last two commands + tcflush(serialFd, TCIOFLUSH); + } printLog("Connected.\n"); - resetSerial(); + resetSerial(); + active = true; #endif @@ -157,62 +179,89 @@ void SerialInterface::renderLevels(int width, int height) { } glEnd(); } +} +void convertHexToInt(unsigned char* sourceBuffer, int& destinationInt) { + unsigned int byte[2]; + + for(int i = 0; i < 2; i++) { + sscanf((char*) sourceBuffer + 2 * i, "%2x", &byte[i]); + } + + int16_t result = (byte[0] << 8); + result += byte[1]; + + destinationInt = result; } void SerialInterface::readData() { #ifdef __APPLE__ - // 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]; int initialSamples = totalSamples; - while (read(serialFd, &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]; + if (USING_INVENSENSE_MPU9150) { + unsigned char gyroBuffer[20]; + + // ask the invensense for raw gyro data + write(serialFd, "RD684306\n", 9); + read(serialFd, gyroBuffer, 20); + + convertHexToInt(gyroBuffer + 6, _lastYaw); + convertHexToInt(gyroBuffer + 10, _lastRoll); + convertHexToInt(gyroBuffer + 14, _lastPitch); + + 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(serialFd, &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; } - - // 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; } } @@ -234,19 +283,23 @@ void SerialInterface::resetSerial() { #ifdef __APPLE__ active = false; totalSamples = 0; - gravity = glm::vec3(0,-1,0); gettimeofday(&lastGoodRead, NULL); - // 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] = ' '; + if (!USING_INVENSENSE_MPU9150) { + gravity = glm::vec3(0,-1,0); + + // 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 150c15fb65..9558a1601f 100644 --- a/interface/src/SerialInterface.h +++ b/interface/src/SerialInterface.h @@ -32,16 +32,24 @@ #define HEAD_YAW_RATE 0 #define HEAD_ROLL_RATE 2 +extern const bool USING_INVENSENSE_MPU9150; + class SerialInterface { public: SerialInterface() { active = false; }; void pair(); void readData(); + + int getLastYaw() const { return _lastYaw; } + int getLastPitch() const { return _lastPitch; } + int getLastRoll() const { return _lastRoll; } + 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; @@ -57,6 +65,9 @@ private: int totalSamples; timeval lastGoodRead; glm::vec3 gravity; + int _lastYaw; + int _lastPitch; + int _lastRoll; }; #endif diff --git a/interface/src/main.cpp b/interface/src/main.cpp index 7ef1c491f8..26cb2d0e53 100644 --- a/interface/src/main.cpp +++ b/interface/src/main.cpp @@ -1476,6 +1476,10 @@ void idle(void) { handControl.stop(); } + if (serialPort.active && USING_INVENSENSE_MPU9150) { + serialPort.readData(); + } + // // Sample hardware, update view frustum if needed, Lsend avatar data to mixer/agents // @@ -1504,7 +1508,7 @@ void idle(void) { } // Read serial data - if (serialPort.active) { + if (serialPort.active && !USING_INVENSENSE_MPU9150) { serialPort.readData(); } }