Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-06-20 07:00:32 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

also read accelerometer data from invensense boards

Browse source
This commit is contained in:
Stephen Birarda 2013-05-09 10:34:40 -07:00
parent 507e4241b1
commit f95ba24f21
2 changed files with 49 additions and 33 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

71
interface/src/SerialInterface.cpp
View file

@ -42,28 +42,23 @@ void SerialInterface::pair() {
int matchStatus; int matchStatus;
regex_t regex; regex_t regex;
if (_failedOpenAttempts < 2) { // for now this only works on OS X, where the usb serial shows up as /dev/tty.usb*
// if we've already failed to open the detected interface twice then don't try again if((devDir = opendir("/dev"))) {
while((entry = readdir(devDir))) {
// for now this only works on OS X, where the usb serial shows up as /dev/tty.usb* regcomp(&regex, "tty\\.usb", REG_EXTENDED|REG_NOSUB);
if((devDir = opendir("/dev"))) { matchStatus = regexec(&regex, entry->d_name, (size_t) 0, NULL, 0);
while((entry = readdir(devDir))) { if (matchStatus == 0) {
regcomp(&regex, "tty\\.usb", REG_EXTENDED|REG_NOSUB); char *serialPortname = new char[100];
matchStatus = regexec(&regex, entry->d_name, (size_t) 0, NULL, 0); sprintf(serialPortname, "/dev/%s", entry->d_name);
if (matchStatus == 0) {
char *serialPortname = new char[100]; initializePort(serialPortname, 115200);
sprintf(serialPortname, "/dev/%s", entry->d_name);
delete [] serialPortname;
initializePort(serialPortname, 115200);
delete [] serialPortname;
}
regfree(&regex);
} }
closedir(devDir); regfree(&regex);
} }
} closedir(devDir);
}
#endif #endif
} }
@ -76,7 +71,6 @@ void SerialInterface::initializePort(char* portname, int baud) {
if (_serialDescriptor == -1) { if (_serialDescriptor == -1) {
printLog("Failed.\n"); printLog("Failed.\n");
_failedOpenAttempts++;
return; return;
} }
struct termios options; struct termios options;
@ -123,6 +117,9 @@ void SerialInterface::initializePort(char* portname, int baud) {
// this disables streaming so there's no garbage data on reads // this disables streaming so there's no garbage data on reads
write(_serialDescriptor, "SD\n", 3); write(_serialDescriptor, "SD\n", 3);
// delay after disabling streaming
usleep(10000);
// flush whatever was produced by the last two commands // flush whatever was produced by the last two commands
tcflush(_serialDescriptor, TCIOFLUSH); tcflush(_serialDescriptor, TCIOFLUSH);
} }
@ -232,26 +229,38 @@ void SerialInterface::readData() {
int initialSamples = totalSamples; int initialSamples = totalSamples;
if (USING_INVENSENSE_MPU9150) { if (USING_INVENSENSE_MPU9150) {
unsigned char gyroBuffer[20]; unsigned char sensorBuffer[36];
// ask the invensense for raw gyro data // ask the invensense for raw gyro data
write(_serialDescriptor, "RD684306\n", 9); write(_serialDescriptor, "RD683B0E\n", 9);
read(_serialDescriptor, gyroBuffer, 20); read(_serialDescriptor, sensorBuffer, 36);
int accelXRate, accelYRate, accelZRate;
convertHexToInt(sensorBuffer + 6, accelXRate);
convertHexToInt(sensorBuffer + 10, accelYRate);
convertHexToInt(sensorBuffer + 14, accelZRate);
const float LSB_TO_METERS_PER_SECOND = 1.f / 2048.f;
_lastAccelX = ((float) accelXRate) * LSB_TO_METERS_PER_SECOND;
_lastAccelY = ((float) accelYRate) * LSB_TO_METERS_PER_SECOND;
_lastAccelZ = ((float) accelZRate) * LSB_TO_METERS_PER_SECOND;
int rollRate, yawRate, pitchRate; int rollRate, yawRate, pitchRate;
convertHexToInt(gyroBuffer + 6, rollRate); convertHexToInt(sensorBuffer + 22, rollRate);
convertHexToInt(gyroBuffer + 10, yawRate); convertHexToInt(sensorBuffer + 26, yawRate);
convertHexToInt(gyroBuffer + 14, pitchRate); convertHexToInt(sensorBuffer + 30, pitchRate);
// Convert the integer rates to floats // Convert the integer rates to floats
const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f; // From MPU-9150 register map, 2000 deg/sec. const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f; // From MPU-9150 register map, 2000 deg/sec.
const float PITCH_BIAS = 2.0; // Strangely, there is a small DC bias in the const float PITCH_BIAS = 2.0; // Strangely, there is a small DC bias in the
// invensense pitch reading. Gravity? // invensense pitch reading. Gravity?
_lastRollRate = (float) rollRate * LSB_TO_DEGREES_PER_SECOND; _lastRollRate = ((float) rollRate) * LSB_TO_DEGREES_PER_SECOND;
_lastYawRate = (float) yawRate * LSB_TO_DEGREES_PER_SECOND; _lastYawRate = ((float) yawRate) * LSB_TO_DEGREES_PER_SECOND;
_lastPitchRate = (float) -pitchRate * LSB_TO_DEGREES_PER_SECOND + PITCH_BIAS; _lastPitchRate = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND + PITCH_BIAS;
totalSamples++; totalSamples++;
} else { } else {

11
interface/src/SerialInterface.h
View file

@ -37,7 +37,12 @@ extern const bool USING_INVENSENSE_MPU9150;
class SerialInterface { class SerialInterface {
public: public:
SerialInterface() : active(false), SerialInterface() : active(false),
_failedOpenAttempts(0) {} _lastAccelX(0),
_lastAccelY(0),
_lastAccelZ(0),
_lastYawRate(0),
_lastPitchRate(0),
_lastRollRate(0) {}
void pair(); void pair();
void readData(); void readData();
@ -69,10 +74,12 @@ private:
int totalSamples; int totalSamples;
timeval lastGoodRead; timeval lastGoodRead;
glm::vec3 gravity; glm::vec3 gravity;
float _lastAccelX;
float _lastAccelY;
float _lastAccelZ;
float _lastYawRate; // Rates are in degrees per second. float _lastYawRate; // Rates are in degrees per second.
float _lastPitchRate; float _lastPitchRate;
float _lastRollRate; float _lastRollRate;
int _failedOpenAttempts;
}; };
#endif #endif