Diff: L3GD20.cpp

Revision:

3:6e935e7cec72

Parent:

2:be6daa938101

--- a/L3GD20.cpp	Mon May 12 11:47:43 2014 +0000
+++ b/L3GD20.cpp	Tue May 13 09:56:43 2014 +0000
@@ -1,8 +1,8 @@
 #include "mbed.h"
 #include "L3GD20.h"
 
-L3GD20::L3GD20(PinName miso, PinName mosi, PinName scl, PinName cs,PinName interrupt2)
-    :_spi(miso,mosi,scl)
+L3GD20::L3GD20(PinName mosi, PinName miso, PinName scl, PinName cs,PinName interrupt2)
+    :_spi(mosi,miso,scl)
     ,_cs(cs)
     ,_int2(interrupt2)
 {
@@ -37,33 +37,28 @@
     _config.CTRL_REG1.B.Enable=enable;
     _config.CTRL_REG1.b.PD=1;
     write(CtrlReg1,_config.CTRL_REG1.word);
+    allReadOut();
 }
 void L3GD20::resetFIFO()
 {
-    enableFIFO(BYPASSmode,none,20);
-    enableFIFO(STREAMmode,watermark,20);
+    enableFIFO(L3GD20::BYPASSmode,L3GD20::none,20);
+    enableFIFO(L3GD20::STREAMmode,L3GD20::none);
 }
 void L3GD20::start(DIRECTION enable,void (*func)(anglerrates*))
 {
+    reboot();
     userFunction=func;
     _int2.rise(this,(&L3GD20::interrupt));
+    start(enable);
     resetFIFO();
-    start(enable);
+    enableFIFO(L3GD20::BYPASSmode,L3GD20::none,20);
+    enableFIFO(L3GD20::STREAMmode,L3GD20::none);
 }
 void L3GD20::interrupt(void)
 {
-    updateFIFO();
-    if(FIFO.status==L3GD20::watermark) {
-        while(_status.FIFO_SRC_REG.B.FSS!=0) {
-            read(&value,XYZ);
-            if(NULL!=userFunction) {
-                userFunction(&value);
-            }
-            updateFIFO();
-            if(FIFO.level==31) {
-                resetFIFO();
-            }
-        }
+    read(&value,XYZ);
+    if(NULL!=userFunction) {
+        userFunction(&value);
     }
 }
 void L3GD20::stop()
@@ -123,21 +118,22 @@
 void L3GD20::enableFIFO(FIFO_mode mode,FIFOstatus interrupt,const int threshold)
 {
     _config.CTRL_REG5.b.FIFO_EN=1;
-    _config.CTRL_REG3.word=0;
-    if(interrupt&empty) {
+    if(interrupt==empty) {
         _config.CTRL_REG3.b.I2_Empty=1;
-    } else if(interrupt&watermark) {
+    } else if(interrupt==watermark) {
         _config.CTRL_REG3.b.I2_WTM=1;
-    } else if(interrupt&overrun) {
+    } else if(interrupt==overrun) {
         _config.CTRL_REG3.b.I2_ORun=1;
+    } else if(interrupt==none) {
+        _config.CTRL_REG3.b.I2_DRDY=1;
     }
     _config.FIFO_CTRL_REG.B.FM=mode;
-    if(threshold<=0) {
-        _config.FIFO_CTRL_REG.B.WTM=0;
-    } else if(threshold<30) {
-        _config.FIFO_CTRL_REG.B.WTM=31;
-    } else {
-        _config.FIFO_CTRL_REG.B.WTM=threshold;
+    if(threshold>0) {
+        if(threshold<30) {
+            _config.FIFO_CTRL_REG.B.WTM=31;
+        } else {
+            _config.FIFO_CTRL_REG.B.WTM=threshold;
+        }
     }
     write(CtrlReg3,_config.CTRL_REG3.word);
     write(CtrlReg5,_config.CTRL_REG5.word);
@@ -159,6 +155,11 @@
     FIFO.level=_status.FIFO_SRC_REG.B.FSS;
     return FIFO.level;
 }
+void L3GD20::allReadOut()
+{
+    configReadOut();
+    statusReadOut();
+}
 void L3GD20::configReadOut(void)
 {
     read(CtrlReg1,&_config.CTRL_REG1.word);