Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-04-22 16:13:28 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions 2

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;
}
//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 {

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 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
}

11
interface/src/SerialInterface.h
View file

@ -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<int>(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

6
interface/src/main.cpp
View file

@ -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();
}
}