Diff: IMU.cpp

Revision:

0:b4479b51578c

Child:

1:a2b5b17c949d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU.cpp	Thu Nov 28 15:14:40 2013 +0000
@@ -0,0 +1,74 @@
+#include "IMU.h"
+
+IMU::IMU(PinName tx, PinName rx){
+    Serial *s = new Serial(tx, rx);
+    init(s);
+}
+
+void IMU::init(Serial *s){
+    faulty = false;
+    s_ = s;
+    s_->baud(BAUDRATE);
+    wait(0.8);
+    while(!isReady()){
+        wait(0.1);
+    }
+    faulty = isFaulty();
+}
+
+IMU::~IMU(){
+    delete s_;
+}
+
+signed int IMU::getParam(unsigned char par){
+    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 IMU::getScaledParam(unsigned char par){
+    float val =(float) getParam(par);
+    
+    if(val>=0){
+        return val/2147483647;
+    } else{
+        return val/2147483648;
+    }
+
+}
+
+void IMU::goToStartOfSentence(unsigned char par){
+    s_->putc(par);
+    s_->putc(0x00);
+    s_->putc(CR);
+    
+    while(s_->getc()!=par);
+}
+
+bool IMU::isReady(){
+    unsigned int param = getParam(0x3E);
+    return (param & 0x0400) == 0;
+}
+
+bool IMU::isFaulty(){
+    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;
+    
+    
+}
\ No newline at end of file