L3GD20.cpp

00001 #include "mbed.h"
00002 #include "L3GD20.h"
00003 
00004 L3GD20::L3GD20(PinName mosi, PinName miso, PinName scl, PinName cs,PinName interrupt2)
00005     :_spi(mosi,miso,scl)
00006     ,_cs(cs)
00007     ,_int2(interrupt2)
00008 {
00009     _spi.format(8,3);
00010     _spi.frequency(10000000);
00011     int val;
00012     read(null,&val);
00013     read(WhoAmI,&val);
00014     if(val==0x0F) { //Discover L3GD20
00015     }
00016     configReadOut();
00017     statusReadOut();
00018 
00019 }
00020 void L3GD20::write(RESISTER reg,int val)
00021 {
00022     _cs=0;
00023     _spi.write(reg);
00024     _spi.write(val);
00025     _cs=1;
00026 }
00027 void L3GD20::read(RESISTER reg,int* val)
00028 {
00029     _cs=0;
00030     _spi.write(READ|reg);
00031     (*val)=_spi.write(0x00);
00032     _cs=1;
00033 }
00034 
00035 void L3GD20::start(DIRECTION enable)
00036 {
00037     _config.CTRL_REG1.B.Enable=enable;
00038     _config.CTRL_REG1.b.PD=1;
00039     write(CtrlReg1,_config.CTRL_REG1.word);
00040     allReadOut();
00041 }
00042 void L3GD20::resetFIFO()
00043 {
00044     enableFIFO(L3GD20::BYPASSmode,L3GD20::none,20);
00045     enableFIFO(L3GD20::STREAMmode,L3GD20::none);
00046 }
00047 void L3GD20::start(DIRECTION enable,void (*func)(anglerrates*))
00048 {
00049     reboot();
00050     userFunction=func;
00051     _int2.rise(this,(&L3GD20::interrupt));
00052     start(enable);
00053     resetFIFO();
00054     enableFIFO(L3GD20::BYPASSmode,L3GD20::none,20);
00055     enableFIFO(L3GD20::STREAMmode,L3GD20::none);
00056 }
00057 void L3GD20::interrupt(void)
00058 {
00059     read(&value,XYZ);
00060     if(NULL!=userFunction) {
00061         userFunction(&value);
00062     }
00063 }
00064 void L3GD20::stop()
00065 {
00066     _config.CTRL_REG1.word=_config.CTRL_REG1.word&0xF0;
00067     write(CtrlReg1,_config.CTRL_REG1.word);
00068 }
00069 void L3GD20::sleep()
00070 {
00071     _config.CTRL_REG1.word=_config.CTRL_REG1.word&0xF8;
00072     write(CtrlReg1,_config.CTRL_REG1.word);
00073 }
00074 void L3GD20::reboot()
00075 {
00076     _config.CTRL_REG5.b.BOOT=1;
00077     write(CtrlReg5,_config.CTRL_REG5.word);
00078 }
00079 void L3GD20::read(anglerrates* val,DIRECTION direction)
00080 {
00081     if(direction&X) {
00082         int x_l,x_h;
00083         read(OutXL,&x_l);
00084         read(OutXH,&x_h);
00085         val->X=(x_l)|(x_h<<8);
00086     }
00087     if(direction&Y) {
00088         int y_l,y_h;
00089         read(OutYL,&y_l);
00090         read(OutYH,&y_h);
00091         val->Y=(y_l)|(y_h<<8);
00092     }
00093     if(direction&Z) {
00094         int z_l,z_h;
00095         read(OutZL,&z_l);
00096         read(OutZH,&z_h);
00097         val->Z=(z_l)|(z_h<<8);
00098     }
00099     if(_config.CTRL_REG4.B.FS==0x00) {
00100         val->x=8.75*(float)val->X;
00101         val->y=8.75*(float)val->Y;
00102         val->z=8.75*(float)val->Z;
00103     } else if(_config.CTRL_REG4.B.FS==0x01) {
00104         val->x=17.5*(float)val->X;
00105         val->y=17.5*(float)val->Y;
00106         val->z=17.5*(float)val->Z;
00107     } else if((_config.CTRL_REG4.B.FS==0x10)|(_config.CTRL_REG4.B.FS==0x11)) {
00108         val->x=70.0*(float)val->X;
00109         val->y=70.0*(float)val->Y;
00110         val->z=70.0*(float)val->Z;
00111     }
00112 }
00113 int L3GD20::readTemperature()
00114 {
00115     read(OutTemp,&_status.OUT_TEMP);
00116     return _status.OUT_TEMP;
00117 }
00118 void L3GD20::enableFIFO(FIFO_mode mode,FIFOstatus interrupt,const int threshold)
00119 {
00120     _config.CTRL_REG5.b.FIFO_EN=1;
00121     if(interrupt==empty) {
00122         _config.CTRL_REG3.b.I2_Empty=1;
00123     } else if(interrupt==watermark) {
00124         _config.CTRL_REG3.b.I2_WTM=1;
00125     } else if(interrupt==overrun) {
00126         _config.CTRL_REG3.b.I2_ORun=1;
00127     } else if(interrupt==none) {
00128         _config.CTRL_REG3.b.I2_DRDY=1;
00129     }
00130     _config.FIFO_CTRL_REG.B.FM=mode;
00131     if(threshold>0) {
00132         if(threshold<30) {
00133             _config.FIFO_CTRL_REG.B.WTM=31;
00134         } else {
00135             _config.FIFO_CTRL_REG.B.WTM=threshold;
00136         }
00137     }
00138     write(CtrlReg3,_config.CTRL_REG3.word);
00139     write(CtrlReg5,_config.CTRL_REG5.word);
00140     write(FIFOCtrlReg,_config.FIFO_CTRL_REG.word);
00141 }
00142 int L3GD20::updateFIFO(void)
00143 {
00144     read(FIFOSrcReg,&_status.FIFO_SRC_REG.word);
00145     read(FIFOCtrlReg,&_config.FIFO_CTRL_REG.word);
00146     if(_status.FIFO_SRC_REG.b.WTM) {
00147         FIFO.status=watermark;
00148     } else if(_status.FIFO_SRC_REG.b.EMPTY) {
00149         FIFO.status=empty;
00150     } else if(_status.FIFO_SRC_REG.b.OVR) {
00151         FIFO.status=overrun;
00152     } else {
00153         FIFO.status=none;
00154     }
00155     FIFO.level=_status.FIFO_SRC_REG.B.FSS;
00156     return FIFO.level;
00157 }
00158 void L3GD20::allReadOut()
00159 {
00160     configReadOut();
00161     statusReadOut();
00162 }
00163 void L3GD20::configReadOut(void)
00164 {
00165     read(CtrlReg1,&_config.CTRL_REG1.word);
00166     read(CtrlReg2,&_config.CTRL_REG2.word);
00167     read(CtrlReg3,&_config.CTRL_REG3.word);
00168     read(CtrlReg4,&_config.CTRL_REG4.word);
00169     read(CtrlReg5,&_config.CTRL_REG5.word);
00170     read(Reference,&_config.REF_DATACAP.word);
00171     read(FIFOCtrlReg,&_config.FIFO_CTRL_REG.word);
00172     read(INT1Cfg,&_config.INT1_CFG.word);
00173     read(INT1ThsXH,&_config.INT1_TSH_XH.word);
00174     read(INT1ThsXL,&_config.INT1_TSH_XL.word);
00175     read(INT1ThsYH,&_config.INT1_TSH_YH.word);
00176     read(INT1ThsYL,&_config.INT1_TSH_YL.word);
00177     read(INT1ThsZH,&_config.INT1_TSH_ZH.word);
00178     read(INT1ThsZL,&_config.INT1_TSH_ZL.word);
00179     read(INT1Duration,&_config.INT1_DURATION.word);
00180 }
00181 
00182 void L3GD20::statusReadOut(void)
00183 {
00184     read(OutTemp,&_status.OUT_TEMP);
00185     read(StatusReg,&_status.STATUS_REG);
00186     read(FIFOSrcReg,&_status.FIFO_SRC_REG.word);
00187     read(INT1Src,&_status.INT1_SRC);
00188 }