From f171b76f6917c64ae22d37f3c526568ba96733cc Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Wed, 7 Aug 2013 17:54:02 -0700 Subject: [PATCH] Fix for gyros' freaking out when disconnected. --- .../MotionDriver/lib/UNIX/libMotionDriver.a | Bin 61894 -> 62086 bytes interface/external/MotionDriver/src/inv_tty.c | 69 ++++++++++++------ interface/src/SerialInterface.cpp | 5 +- 3 files changed, 50 insertions(+), 24 deletions(-) diff --git a/interface/external/MotionDriver/lib/UNIX/libMotionDriver.a b/interface/external/MotionDriver/lib/UNIX/libMotionDriver.a index c29bf3ef8ec5645ce6c8962daa20bcad16293b0a..82ca139b508839f9a7f2efba4610ef1ba522f72e 100644 GIT binary patch delta 1581 zcmc&!ZD>y#s0bFz9o%bzt8_`&Bgc|fY?V|Uw-)E7+#_zj|q3moZLPdtyRf= zZO<51hN^=M z-B~^)0Km%zDMcSiBWX>~uQm)ryY~ANhm@lK@KUp37)NG{Y9Afzbq@gmsEJG+hC<8Z zrijZFd43rHq!i6*dj8pehH)Tm1IIWprs?^c9I%f4MW+QO_wtKAT2N1_A{~`dAf=d( zb5lAy6SN>mDSB@yEyVxsD`Tu-0q|jHhF48Y3bx|s98!<0nHa8MMzvd-p68Y)EBG*Q z!KQh$+O^xNwq_{UyC)m$?HTQ*8Qd0HxUM+Q3vN5B>G^yA7U#Lcfl*D*U!4N5*+?mx z(e(V;DZm*3z}um!t;Jq<7kPD-dUe`~qB_l)GgA6!&YaPZs_FSdT;Cd`H$ii3Jej35 z7k|L6*Y)o5Szes;bq3`{wdf3QT>lNz`)Y>!AXzcpNg|cy6#xnVoMLqV=wdG$U!M@k zYO@7C1%Nbffm!Ukv3tbn0_$M-Biwyve)4g+LLe>&4xVozyDG7^BS1n9?5L4(_hkVmF6>8d=l9iSWB^zR00acMWX0%0y?H)vtZ`tgZzax& zw$Gf+f`Jtf0OA;KsjBH`6%b${;b_?EI&IZ^M7$EMCrL3m9t{X&7;tcvOl|_Em&qg` zuwA2Saa;iAvL9QENlO6m*5J=yoOZv2CR)6lRWGHzGHHYm^K6A+Kb1*4F)hR_`?dx1 zcLV_6S@lw_KR7L0LbEO0eGl!KNh7bhg@9-dqeCsVA*^*N;DK*uJBuesg_ygQ6(P;wC@)Qx*L_ N|Is#2@`qUL#Q%+y(Aoe1 delta 1451 zcmc&!YiJZ#6h3n&YeIZXDl*wWbZQ5mJxfgSJwzVFf8Gn=EMzF(tuKStNp#&_;-;Fr^z)D1tvEx7<6kB@G1p=fKR| z^PT&B=ey_5+;cD0d(YLbW%<79^_5kA|F_!4+SAVBd7qR405hnCKZi;pTAB_RrqwVlo%9)|MeSjaVOkreIiI7D4YLcOiT-&~ z?~&ucq#3!0(m2hqE6f;X>b^vufUKHn&SPqnX2ddjHquPwkDC8nvTda2G zTOvIlamgjJ=*aH2efZms40c{G!+YWVk&uwzyr}3L*}8BZVGHsL07?PiJAng0y?EsE zd!p*01^ERcKVYXpI?m;ZDfu1%7Zb;mLTXj)==PJo`S_wS zKw>I3dUZV6U4(T_ZV{fUw8Qgu81(6QGEsyNo7|*J#bi~0+;KRGBCKn67Y?a*`JjTy z#7B6(*-e6r@J4fhfD4np07(?!%AJ8itI(d80ZiUJi+}GtP$C-w0NVh7fC4|-GoT*L z>gBlKn}>C4x8d(C4IfN88QB5=h+x=X?Ck=8#h}11;`;)y8vp>`JNRvaM|PEyDU!Oe zE1-~DfXP~&+yxxkq?2MrL{%Rg0tj^=g)s{AB>(^&C>AL>m88ICD%3<2eUw9DKHMpQMpmB3IvOM;Gj< zn@B@{5}dfBlL8eRdUf=+b}Y^9$dFx2iVERLz)_|*UN%c08<{g-<#7SBH*$ ecCh1U`4{D1S9m-gPo;02b0mv1ZFSq;%l`rav4CO# diff --git a/interface/external/MotionDriver/src/inv_tty.c b/interface/external/MotionDriver/src/inv_tty.c index 3fa2bbe17e..30a3fceb55 100644 --- a/interface/external/MotionDriver/src/inv_tty.c +++ b/interface/external/MotionDriver/src/inv_tty.c @@ -26,51 +26,74 @@ static unsigned char from_hex_digit(char digit) { 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) }; - 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]; - read(ttyFileDescriptor, chars, 2); - return from_hex_digit(chars[0]) * 16 + from_hex_digit(chars[1]); + if (read(ttyFileDescriptor, chars, 2) != 2) { + 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) { - write(ttyFileDescriptor, "WR", 2); - write_byte(slave_addr); - write_byte(reg_addr); + if (write(ttyFileDescriptor, "WR", 2) != 2) { + return 1; + } + if (write_byte(slave_addr)) { + return 1; + } + if (write_byte(reg_addr)) { + return 1; + } int 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]; - read(ttyFileDescriptor, response, 8); - - return 0; + return read(ttyFileDescriptor, response, 8) != 8; } int tty_i2c_read(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data) { - write(ttyFileDescriptor, "RD", 2); - write_byte(slave_addr); - write_byte(reg_addr); - write_byte(length); - write(ttyFileDescriptor, "\n", 1); + if (write(ttyFileDescriptor, "RD", 2) != 2) { + return 1; + } + if (write_byte(slave_addr)) { + 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]; - read(ttyFileDescriptor, prefix, 6); + if (read(ttyFileDescriptor, prefix, 6) != 6) { + return 1; + } int i; for (i = 0; i < length; i++) { - data[i] = read_byte(); + if (read_byte(data + i)) { + return 1; + } } char suffix[2]; - read(ttyFileDescriptor, suffix, 2); - - return 0; + return read(ttyFileDescriptor, suffix, 2) != 2; } void tty_delay_ms(unsigned long num_ms) { diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index 0f90ee8dd7..20db2c09da 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -225,7 +225,10 @@ void SerialInterface::readData(float deltaTime) { // ask the invensense for raw gyro data 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; // From MPU-9150 register map, with setting on