Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-08-07 21:04:58 +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

38
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);
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);
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);
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))
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);
}
float Avatar::getAbsoluteHeadYaw() const {

69
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,6 +95,7 @@ int SerialInterface::initializePort(char* portname, int baud)
cfsetospeed(&options,B9600);
break;
}
options.c_cflag |= (CLOCAL | CREAD);
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
@ -101,9 +103,29 @@ int SerialInterface::initializePort(char* portname, int baud)
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();
active = true;
#endif
@ -157,17 +179,43 @@ 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__
int initialSamples = totalSamples;
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];
int initialSamples = totalSamples;
while (read(serialFd, &bufchar, 1) > 0) {
serialBuffer[serialBufferPos] = bufchar[0];
serialBufferPos++;
@ -215,6 +263,7 @@ void SerialInterface::readData() {
serialBufferPos = 0;
}
}
}
if (initialSamples == totalSamples) {
timeval now;
@ -234,10 +283,12 @@ void SerialInterface::resetSerial() {
#ifdef __APPLE__
active = false;
totalSamples = 0;
gravity = glm::vec3(0,-1,0);
gettimeofday(&lastGoodRead, NULL);
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;
@ -247,6 +298,8 @@ void SerialInterface::resetSerial() {
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();
}
}