Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-08-10 11:42:55 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

hook up the invensense via existing SerialInterface class

Browse source
This commit is contained in:
Stephen Birarda 2013-05-02 12:26:12 -07:00
parent 8087d2e0f4
commit 9e3b862c67
4 changed files with 152 additions and 90 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

46
interface/src/Avatar.cpp
View file

@ -226,45 +226,39 @@ void Avatar::reset() {
_head.leanForward = _head.leanSideways = 0; _head.leanForward = _head.leanSideways = 0;
} }
// this pertains to moving the head with the glasses
//this pertains to moving the head with the glasses
void Avatar::UpdateGyros(float frametime, SerialInterface * serialInterface, glm::vec3 * gravity)
// Using serial data, update avatar/render position and angles // Using serial data, update avatar/render position and angles
{ void Avatar::UpdateGyros(float frametime, SerialInterface* serialInterface, glm::vec3* gravity) {
const float PITCH_ACCEL_COUPLING = 0.5; float measured_pitch_rate = 0.0f;
const float ROLL_ACCEL_COUPLING = -1.0; float measured_roll_rate = 0.0f;
float measured_pitch_rate = serialInterface->getRelativeValue(HEAD_PITCH_RATE); if (serialInterface->active && USING_INVENSENSE_MPU9150) {
_head.yawRate = serialInterface->getRelativeValue(HEAD_YAW_RATE); measured_pitch_rate = serialInterface->getLastPitch();
float measured_lateral_accel = serialInterface->getRelativeValue(ACCEL_X) - _head.yawRate = serialInterface->getLastYaw();
ROLL_ACCEL_COUPLING*serialInterface->getRelativeValue(HEAD_ROLL_RATE); measured_roll_rate = -1 * serialInterface->getLastRoll();
float measured_fwd_accel = serialInterface->getRelativeValue(ACCEL_Z) - } else {
PITCH_ACCEL_COUPLING*serialInterface->getRelativeValue(HEAD_PITCH_RATE); measured_pitch_rate = serialInterface->getRelativeValue(HEAD_PITCH_RATE);
float measured_roll_rate = serialInterface->getRelativeValue(HEAD_ROLL_RATE); _head.yawRate = serialInterface->getRelativeValue(HEAD_YAW_RATE);
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);
// Update avatar head position based on measured gyro rates // Update avatar head position based on measured gyro rates
const float HEAD_ROTATION_SCALE = 0.70; const float HEAD_ROTATION_SCALE = 0.70;
const float HEAD_ROLL_SCALE = 0.40; const float HEAD_ROLL_SCALE = 0.40;
const float HEAD_LEAN_SCALE = 0.01;
const float MAX_PITCH = 45; const float MAX_PITCH = 45;
const float MIN_PITCH = -45; const float MIN_PITCH = -45;
const float MAX_YAW = 85; const float MAX_YAW = 85;
const float MIN_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); addHeadPitch(measured_pitch_rate * -HEAD_ROTATION_SCALE * frametime);
}
addHeadRoll(measured_roll_rate * HEAD_ROLL_SCALE * frametime); addHeadRoll(measured_roll_rate * HEAD_ROLL_SCALE * frametime);
if ((_headYaw < MAX_YAW) && (_headYaw > MIN_YAW)) if ((_headYaw < MAX_YAW) && (_headYaw > MIN_YAW)) {
addHeadYaw(_head.yawRate * HEAD_ROTATION_SCALE * frametime); addHeadYaw(_head.yawRate * HEAD_ROTATION_SCALE * frametime);
}
addLean(-measured_lateral_accel * frametime * HEAD_LEAN_SCALE, -measured_fwd_accel*frametime * HEAD_LEAN_SCALE);
} }
float Avatar::getAbsoluteHeadYaw() const { float Avatar::getAbsoluteHeadYaw() const {

179
interface/src/SerialInterface.cpp
View file

@ -32,6 +32,8 @@ const short NO_READ_MAXIMUM_MSECS = 3000;
const short SAMPLES_TO_DISCARD = 100; // Throw out the first few samples 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 int GRAVITY_SAMPLES = 200; // Use the first samples to compute gravity vector
const bool USING_INVENSENSE_MPU9150 = 1;
void SerialInterface::pair() { void SerialInterface::pair() {
#ifdef __APPLE__ #ifdef __APPLE__
@ -62,9 +64,8 @@ void SerialInterface::pair() {
} }
// Connect to the serial port // connect to the serial port
int SerialInterface::initializePort(char* portname, int baud) int SerialInterface::initializePort(char* portname, int baud) {
{
#ifdef __APPLE__ #ifdef __APPLE__
serialFd = open(portname, O_RDWR | O_NOCTTY | O_NDELAY); serialFd = open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
@ -76,8 +77,8 @@ int SerialInterface::initializePort(char* portname, int baud)
} }
struct termios options; struct termios options;
tcgetattr(serialFd,&options); tcgetattr(serialFd,&options);
switch(baud)
{ switch(baud) {
case 9600: cfsetispeed(&options,B9600); case 9600: cfsetispeed(&options,B9600);
cfsetospeed(&options,B9600); cfsetospeed(&options,B9600);
break; break;
@ -94,16 +95,37 @@ int SerialInterface::initializePort(char* portname, int baud)
cfsetospeed(&options,B9600); cfsetospeed(&options,B9600);
break; break;
} }
options.c_cflag |= (CLOCAL | CREAD); options.c_cflag |= (CLOCAL | CREAD);
options.c_cflag &= ~PARENB; options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB; options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE; options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8; options.c_cflag |= CS8;
tcsetattr(serialFd,TCSANOW,&options); 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"); printLog("Connected.\n");
resetSerial(); resetSerial();
active = true; active = true;
#endif #endif
@ -157,62 +179,89 @@ void SerialInterface::renderLevels(int width, int height) {
} }
glEnd(); 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() { void SerialInterface::readData() {
#ifdef __APPLE__ #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; int initialSamples = totalSamples;
while (read(serialFd, &bufchar, 1) > 0) { if (USING_INVENSENSE_MPU9150) {
serialBuffer[serialBufferPos] = bufchar[0]; unsigned char gyroBuffer[20];
serialBufferPos++;
// Have we reached end of a line of input? // ask the invensense for raw gyro data
if ((bufchar[0] == '\n') || (serialBufferPos >= MAX_BUFFER)) { write(serialFd, "RD684306\n", 9);
std::string serialLine(serialBuffer, serialBufferPos-1); read(serialFd, gyroBuffer, 20);
//printLog("%s\n", serialLine.c_str());
int spot; convertHexToInt(gyroBuffer + 6, _lastYaw);
//int channel = 0; convertHexToInt(gyroBuffer + 10, _lastRoll);
std::string val; convertHexToInt(gyroBuffer + 14, _lastPitch);
for (int i = 0; i < NUM_CHANNELS + 2; i++) {
spot = serialLine.find_first_of(" ", 0); totalSamples++;
if (spot != std::string::npos) { } else {
val = serialLine.substr(0,spot); // This array sets the rate of trailing averaging for each channel:
//printLog("%s\n", val.c_str()); // If the sensor rate is 100Hz, 0.001 will make the long term average a 10-second average
if (i < NUM_CHANNELS) lastMeasured[i] = atoi(val.c_str()); const float AVG_RATE[] = {0.002, 0.002, 0.002, 0.002, 0.002, 0.002};
else samplesAveraged = atoi(val.c_str()); char bufchar[1];
} else LED = atoi(serialLine.c_str());
serialLine = serialLine.substr(spot+1, serialLine.length() - spot - 1); while (read(serialFd, &bufchar, 1) > 0) {
} serialBuffer[serialBufferPos] = bufchar[0];
serialBufferPos++;
// Update Trailing Averages // Have we reached end of a line of input?
for (int i = 0; i < NUM_CHANNELS; i++) { if ((bufchar[0] == '\n') || (serialBufferPos >= MAX_BUFFER)) {
if (totalSamples > SAMPLES_TO_DISCARD) { std::string serialLine(serialBuffer, serialBufferPos-1);
trailingAverage[i] = (1.f - AVG_RATE[i])*trailingAverage[i] + //printLog("%s\n", serialLine.c_str());
AVG_RATE[i]*(float)lastMeasured[i]; int spot;
} else { //int channel = 0;
trailingAverage[i] = (float)lastMeasured[i]; 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__ #ifdef __APPLE__
active = false; active = false;
totalSamples = 0; totalSamples = 0;
gravity = glm::vec3(0,-1,0);
gettimeofday(&lastGoodRead, NULL); gettimeofday(&lastGoodRead, NULL);
// Clear the measured and average channel data if (!USING_INVENSENSE_MPU9150) {
for (int i = 0; i < NUM_CHANNELS; i++) { gravity = glm::vec3(0,-1,0);
lastMeasured[i] = 0;
trailingAverage[i] = 0.0; // Clear the measured and average channel data
} for (int i = 0; i < NUM_CHANNELS; i++) {
// Clear serial input buffer lastMeasured[i] = 0;
for (int i = 1; i < MAX_BUFFER; i++) { trailingAverage[i] = 0.0;
serialBuffer[i] = ' '; }
// Clear serial input buffer
for (int i = 1; i < MAX_BUFFER; i++) {
serialBuffer[i] = ' ';
}
} }
#endif #endif
} }

11
interface/src/SerialInterface.h
View file

@ -32,16 +32,24 @@
#define HEAD_YAW_RATE 0 #define HEAD_YAW_RATE 0
#define HEAD_ROLL_RATE 2 #define HEAD_ROLL_RATE 2
extern const bool USING_INVENSENSE_MPU9150;
class SerialInterface { class SerialInterface {
public: public:
SerialInterface() { active = false; }; SerialInterface() { active = false; };
void pair(); void pair();
void readData(); void readData();
int getLastYaw() const { return _lastYaw; }
int getLastPitch() const { return _lastPitch; }
int getLastRoll() const { return _lastRoll; }
int getLED() {return LED;}; int getLED() {return LED;};
int getNumSamples() {return samplesAveraged;}; int getNumSamples() {return samplesAveraged;};
int getValue(int num) {return lastMeasured[num];}; int getValue(int num) {return lastMeasured[num];};
int getRelativeValue(int num) {return static_cast<int>(lastMeasured[num] - trailingAverage[num]);}; int getRelativeValue(int num) {return static_cast<int>(lastMeasured[num] - trailingAverage[num]);};
float getTrailingValue(int num) {return trailingAverage[num];}; float getTrailingValue(int num) {return trailingAverage[num];};
void resetTrailingAverages(); void resetTrailingAverages();
void renderLevels(int width, int height); void renderLevels(int width, int height);
bool active; bool active;
@ -57,6 +65,9 @@ private:
int totalSamples; int totalSamples;
timeval lastGoodRead; timeval lastGoodRead;
glm::vec3 gravity; glm::vec3 gravity;
int _lastYaw;
int _lastPitch;
int _lastRoll;
}; };
#endif #endif

6
interface/src/main.cpp
View file

@ -1476,6 +1476,10 @@ void idle(void) {
handControl.stop(); handControl.stop();
} }
if (serialPort.active && USING_INVENSENSE_MPU9150) {
serialPort.readData();
}
// //
// Sample hardware, update view frustum if needed, Lsend avatar data to mixer/agents // Sample hardware, update view frustum if needed, Lsend avatar data to mixer/agents
// //
@ -1504,7 +1508,7 @@ void idle(void) {
} }
// Read serial data // Read serial data
if (serialPort.active) { if (serialPort.active && !USING_INVENSENSE_MPU9150) {
serialPort.readData(); serialPort.readData();
} }
} }