From f171b76f6917c64ae22d37f3c526568ba96733cc Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Wed, 7 Aug 2013 17:54:02 -0700 Subject: [PATCH 1/3] 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 From 55a010a1a001b45901d0cc4c94c110025f78920a Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Wed, 7 Aug 2013 18:02:38 -0700 Subject: [PATCH 2/3] And also, close the file descriptor and print a message. --- interface/src/SerialInterface.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index 20db2c09da..d9c1bd928f 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -226,6 +226,8 @@ void SerialInterface::readData(float deltaTime) { // ask the invensense for raw gyro data short accelData[3]; if (mpu_get_accel_reg(accelData, 0)) { + close(_serialDescriptor); + qDebug("Disconnected SerialUSB.\n"); _active = false; return; // disconnected } From 9d7b22e065cb809a0608ac1421d69ae9e63cd845 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Wed, 7 Aug 2013 18:40:52 -0700 Subject: [PATCH 3/3] Recompiled OS X driver. --- .../MotionDriver/lib/MacOS/libMotionDriver.a | Bin 58976 -> 59312 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/interface/external/MotionDriver/lib/MacOS/libMotionDriver.a b/interface/external/MotionDriver/lib/MacOS/libMotionDriver.a index 9b2eb2f00a84f95f26bc0c1df9c5b5df5790e8c1..5911eb9f9994aa7b4da6b094995ef41ff39a4910 100644 GIT binary patch delta 1838 zcmah}ZA@EL7=F$zg_e(9!lK}y;o{=Q+^)IfGep@s<&x{Nnz&@i{2)RGQbDdd`!4iQr!JKp6f;A?dA3c5E z=ly=qeN)#wLo=R%>Y}FB`lgnq*5OA#^wIyu3*Y*Fh;4$ypMTX+r?eUkqsiK97)HHi z7>2RevYHzk4Wps8wV9VDU#n>JL7py6-IkAkl-yTw*1IJx05DL@uOyo_%g41}U)*HL z7QYwqk>Y)U&&bGN%6PFz-P0w2pO=+1R16g=Nt86o{rqsrNTA=xb@ zBT_#tT*U3Po(SD*o!0w>U+0*ecH*Ioo_LlC{c5TIo^VXsULkSq^qn91Gqc8XZ35+6k_j&aE!<9+>Q@_k6>GVxnofERW;{*Kr*+Vs| zNU|7r`XiF#feJu2+8_u{hB%6+hGi)XpTXo_`mSWnJomCHoBI>p608K0C z^xGMy|76l}!>hdJ<`CcW+hul|&yQ95zY)a(=;DJvRr#p0K}ijH6m^Psk5&1^7YN`0 zXXBgT4c0ctML!uykfE~gi-{`6HTWcHgRB(2=*8e1-(|oYEy6gmi=(^&&cz3GS zKT2qj5BApjgIUF$;=`$0|2aYj_)ERj-ckl!;@49d9cc26%zMZKfP8sE!A&GKjTiCg}c}0hNIHJ_D+NwoaxJIZFC_$o>h!l}sff z)`^A^nBU1iFwgRL79tUg7Qr?KZ1VuQZTgVF49E?>j(C^mwu|`O99xKw6ThGM-0L_< zIQI%(5!j0ea(lK%2~b6w_+7$p68@NQitrbNeN@0(;#**VMbB(jU@tI0`e%rb5`S0n z5nz-QUL^%FE^)4qgCOxch&L3CQR0_qzleT|1ajv*@zcb=07#wO*(LCWx6>=^Z(Smz AN&o-= delta 1574 zcmY*ZZ)jUp6#w0%Nt3kQ)5u(08~UOkOn4<(>yzSEBkql>60w60Q3LJ%wCX2O233iv zR^dIupmIeR44M0;pY})M9NMO~XdSwCU!*Q`)-J=U6+&feskV{(?s>7^54q=ee&^i$ z&b{ZIGqe7M@BCw}jh$WHsm?C*?7gprq@6UAsq?2pAKP_s;Jp+36DlgD>klSVx~}g| z>bkC{l1a0xOV>M1Gbx(qpO038a96)i?F&Ki`g~8+Z1~SD&2(5?nm2t5LE-!HsX7}{ zLHHmj&P96SJ(Q#~<^Pb*9*zKE(bCiz?GGqsgd$3f;y}~=kx~SR0~l)-yRYV0TAaE1 zz`*K0+q!LAx44s8D(sbcOY%o6=2p35&3WLqYkXr>Bw4U3S}}90dqz8+0KgAh6<)|J zX~jLv4J%lcfc$I6s{A%5PLy?VrF@sc9c#v=d11Reg%_f{aBPituW-jIY40`xxZo0_ z(`ixuq*a*LdfFVT%15W~$W9Foc4+PINFIIMxyeVD(_-ven_1guTbs7E!JSO?{{_^z zW6gU7YW^Y9Qg5LzpLDO%~Yxst;*ey-J=gDiY;#MrHlEw$j#VYyYm>h=Xycih zlUsIX$v0ETEwkCQy`~jQd~{mRP7QX5mw&#@*2Ta?EJUy0C)i{ith#^%J}L&rcZBB% z4Y?@ocdr-+1uMlu1opdBwT?cTh>6;fM0jMq=B}t$bPc_QISKEYiVY{9C;3k-o>H(n|`1XFkYfP>jzo^;~AwV ze!~ac0~nwBfSrJG$fx+MDlFdN1A>5|kv~ZO6$Z2c#-Hk<;y*FK55#|9fDbUfS9{)= zCdX%#aE=lL0|G#Ni2R4h|0IL_oDGxqyBV+pFjfG>fW3b}30zcH9ls1E{&B#K@zySh&wqLIFZM6F C3}y!a