Mirrors/overte-JulianGro
3
0
Fork 0
mirror of https://github.com/JulianGro/overte.git synced 2025-08-04 05:44:48 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Fix for gyros' freaking out when disconnected.

Browse source
This commit is contained in:
Andrzej Kapolka 2013-08-07 17:54:02 -07:00
parent 7a24a56670
commit f171b76f69
3 changed files with 50 additions and 24 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

BIN
interface/external/MotionDriver/lib/UNIX/libMotionDriver.a vendored
View file

Binary file not shown.

69
interface/external/MotionDriver/src/inv_tty.c vendored
View file

@ -26,51 +26,74 @@ static unsigned char from_hex_digit(char digit) {
return (digit < 'A') ? digit - '0' : (digit - 'A') + 10; return (digit < 'A') ? digit - '0' : (digit - 'A') + 10;
} }
static void write_byte(unsigned char value) { static int write_byte(unsigned char value) {
char chars[] = { to_hex_digit(value / 16), to_hex_digit(value % 16) }; char chars[] = { to_hex_digit(value / 16), to_hex_digit(value % 16) };
write(ttyFileDescriptor, chars, 2); return write(ttyFileDescriptor, chars, 2) != 2;
} }
static unsigned char read_byte() { static int read_byte(unsigned char* value) {
char chars[2]; char chars[2];
read(ttyFileDescriptor, chars, 2); if (read(ttyFileDescriptor, chars, 2) != 2) {
return from_hex_digit(chars[0]) * 16 + from_hex_digit(chars[1]); return 1;
}
*value = from_hex_digit(chars[0]) * 16 + from_hex_digit(chars[1]);
return 0;
} }
int tty_i2c_write(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data) { int tty_i2c_write(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data) {
write(ttyFileDescriptor, "WR", 2); if (write(ttyFileDescriptor, "WR", 2) != 2) {
write_byte(slave_addr); return 1;
write_byte(reg_addr); }
if (write_byte(slave_addr)) {
return 1;
}
if (write_byte(reg_addr)) {
return 1;
}
int i; int i;
for (i = 0; i < length; i++) { for (i = 0; i < length; i++) {
write_byte(data[i]); if (write_byte(data[i])) {
return 1;
}
}
if (write(ttyFileDescriptor, "\n", 1) != 1) {
return 1;
} }
write(ttyFileDescriptor, "\n", 1);
char response[8]; char response[8];
read(ttyFileDescriptor, response, 8); return read(ttyFileDescriptor, response, 8) != 8;
return 0;
} }
int tty_i2c_read(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data) { int tty_i2c_read(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data) {
write(ttyFileDescriptor, "RD", 2); if (write(ttyFileDescriptor, "RD", 2) != 2) {
write_byte(slave_addr); return 1;
write_byte(reg_addr); }
write_byte(length); if (write_byte(slave_addr)) {
write(ttyFileDescriptor, "\n", 1); return 1;
}
if (write_byte(reg_addr)) {
return 1;
}
if (write_byte(length)) {
return 1;
}
if (write(ttyFileDescriptor, "\n", 1) != 1) {
return 1;
}
char prefix[6]; char prefix[6];
read(ttyFileDescriptor, prefix, 6); if (read(ttyFileDescriptor, prefix, 6) != 6) {
return 1;
}
int i; int i;
for (i = 0; i < length; i++) { for (i = 0; i < length; i++) {
data[i] = read_byte(); if (read_byte(data + i)) {
return 1;
}
} }
char suffix[2]; char suffix[2];
read(ttyFileDescriptor, suffix, 2); return read(ttyFileDescriptor, suffix, 2) != 2;
return 0;
} }
void tty_delay_ms(unsigned long num_ms) { void tty_delay_ms(unsigned long num_ms) {

5
interface/src/SerialInterface.cpp
View file

@ -225,7 +225,10 @@ void SerialInterface::readData(float deltaTime) {
// ask the invensense for raw gyro data // ask the invensense for raw gyro data
short accelData[3]; short accelData[3];
mpu_get_accel_reg(accelData, 0); if (mpu_get_accel_reg(accelData, 0)) {
_active = false;
return; // disconnected
}
const float LSB_TO_METERS_PER_SECOND2 = 1.f / 16384.f * GRAVITY_EARTH; const float LSB_TO_METERS_PER_SECOND2 = 1.f / 16384.f * GRAVITY_EARTH;
// From MPU-9150 register map, with setting on // From MPU-9150 register map, with setting on