Revision 0:b4479b51578c, committed 2013-11-28

Comitter:

GijsB

Date:

Thu Nov 28 15:14:40 2013 +0000

Child:

1:a2b5b17c949d

Commit message:

eerste;

Changed in this revision

--- /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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU.h	Thu Nov 28 15:14:40 2013 +0000
@@ -0,0 +1,43 @@
+#ifndef IMU_H
+#define IMU_H
+
+#include "mbed.h"
+
+
+#define TEMP_OUT 0x02
+#define XGYRO_OUT 0x04
+#define YGYRO_OUT 0x06
+#define ZGYRO_OUT 0x08
+#define XACCL_OUT 0x0A
+#define YACCL_OUT 0x0C
+#define ZACCL_OUT 0x0E
+
+#define CR 0x0D
+
+#define BAUDRATE 57600
+
+
+class IMU {
+
+    public:
+        IMU(PinName tx, PinName rx);
+        ~IMU();
+        signed int getParam(unsigned char par); 
+        float getScaledParam(unsigned char par); 
+        
+        
+    private:
+        Serial *s_;
+        bool faulty;
+        
+        void init(Serial *s);
+        bool isReady();
+        bool isFaulty();
+        void goToStartOfSentence(unsigned char par);
+        
+        
+
+
+};
+
+#endif
\ No newline at end of file