Diff: USBIMU.cpp

Revision:

1:a2b5b17c949d

diff -r b4479b51578c -r a2b5b17c949d USBIMU.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBIMU.cpp	Fri Dec 13 10:58:02 2013 +0000
@@ -0,0 +1,89 @@
+#include "USBIMU.h"
+
+USBIMU::USBIMU(){
+    // TODO
+    USBHostSerial *s = new USBHostSerial();
+    init(s);
+}
+
+void USBIMU::init(USBHostSerial *s){
+    // TODO
+    
+    while (!s->connect()){
+        wait(50);
+    }
+    
+    faulty = false;
+    s_ = s;
+    wait(0.8);
+    while(!isReady()){
+        wait(0.1);
+    }
+    faulty = isFaulty();
+}
+
+USBIMU::~USBIMU(){
+    delete s_;
+}
+
+signed int USBIMU::getParam(unsigned char par){
+    // TODO
+    if(faulty){
+        return 0;
+    }
+    
+    goToStartOfSentence(par);
+    
+    unsigned char MSByte = s_->getc(); /*The most significant byte */
+    unsigned char LSByte = s_->getc(); /*The least significant byte */
+    s_->getc();                        /*The CR */
+    
+    signed int res = (MSByte<<8) & LSByte;
+    return res;
+}
+
+
+
+float USBIMU::getScaledParam(unsigned char par){
+    // TODO
+    float val =(float) getParam(par);
+    
+    if(val>=0){
+        return val/2147483647;
+    } else{
+        return val/2147483648;
+    }
+
+}
+
+void USBIMU::goToStartOfSentence(unsigned char par){
+    // TODO
+    s_->putc(par);
+    s_->putc(0x00);
+    s_->putc(CR);
+    
+    while(s_->getc()!=par);
+}
+
+bool USBIMU::isReady(){
+    // TODO
+    unsigned int param = getParam(0x3E);
+    return (param & 0x0400) == 0;
+}
+
+bool USBIMU::isFaulty(){
+    // TODO
+    goToStartOfSentence(0x3C);
+    
+    s_->getc();                         /*The most significant byte is nog interesting here*/
+    unsigned char LSByte = s_->getc(); /*The least significant byte */
+    s_->getc();                        /*The CR */
+    
+    return (LSByte & 0x60) != 0;
+    
+    
+}
+
+bool USBIMU::getFaulty(){
+    return faulty;
+}
\ No newline at end of file