L3GD20 Library using FIFO and Interrupt
Fork of L3GD20_SPI by
L3GD20.cpp@0:175bf093daa8, 2014-05-12 (annotated)
- Committer:
- lelect
- Date:
- Mon May 12 05:59:12 2014 +0000
- Revision:
- 0:175bf093daa8
- Child:
- 1:2ebc045424af
test commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
lelect | 0:175bf093daa8 | 1 | #include "mbed.h" |
lelect | 0:175bf093daa8 | 2 | #include "L3GD20.h" |
lelect | 0:175bf093daa8 | 3 | |
lelect | 0:175bf093daa8 | 4 | L3GD20::L3GD20(PinName miso, PinName mosi, PinName scl, PinName cs,PinName interrupt2) |
lelect | 0:175bf093daa8 | 5 | :_spi(miso,mosi,scl) |
lelect | 0:175bf093daa8 | 6 | ,_cs(cs) |
lelect | 0:175bf093daa8 | 7 | ,_int2(interrupt2) |
lelect | 0:175bf093daa8 | 8 | { |
lelect | 0:175bf093daa8 | 9 | _spi.format(8,3); |
lelect | 0:175bf093daa8 | 10 | _spi.frequency(10000000); |
lelect | 0:175bf093daa8 | 11 | int val; |
lelect | 0:175bf093daa8 | 12 | read(null,&val); |
lelect | 0:175bf093daa8 | 13 | read(WhoAmI,&val); |
lelect | 0:175bf093daa8 | 14 | if(val==0x0F) { //Discover L3GD20 |
lelect | 0:175bf093daa8 | 15 | } |
lelect | 0:175bf093daa8 | 16 | configReadOut(); |
lelect | 0:175bf093daa8 | 17 | statusReadOut(); |
lelect | 0:175bf093daa8 | 18 | |
lelect | 0:175bf093daa8 | 19 | } |
lelect | 0:175bf093daa8 | 20 | void L3GD20::write(RESISTER reg,int val) |
lelect | 0:175bf093daa8 | 21 | { |
lelect | 0:175bf093daa8 | 22 | _cs=0; |
lelect | 0:175bf093daa8 | 23 | _spi.write(reg); |
lelect | 0:175bf093daa8 | 24 | _spi.write(val); |
lelect | 0:175bf093daa8 | 25 | _cs=1; |
lelect | 0:175bf093daa8 | 26 | } |
lelect | 0:175bf093daa8 | 27 | void L3GD20::read(RESISTER reg,int* val) |
lelect | 0:175bf093daa8 | 28 | { |
lelect | 0:175bf093daa8 | 29 | _cs=0; |
lelect | 0:175bf093daa8 | 30 | _spi.write(READ|reg); |
lelect | 0:175bf093daa8 | 31 | (*val)=_spi.write(0x00); |
lelect | 0:175bf093daa8 | 32 | _cs=1; |
lelect | 0:175bf093daa8 | 33 | } |
lelect | 0:175bf093daa8 | 34 | |
lelect | 0:175bf093daa8 | 35 | void L3GD20::start(DIRECTION enable) |
lelect | 0:175bf093daa8 | 36 | { |
lelect | 0:175bf093daa8 | 37 | _config.CTRL_REG1.B.Enable=enable; |
lelect | 0:175bf093daa8 | 38 | _config.CTRL_REG1.b.PD=1; |
lelect | 0:175bf093daa8 | 39 | write(CtrlReg1,_config.CTRL_REG1.word); |
lelect | 0:175bf093daa8 | 40 | } |
lelect | 0:175bf093daa8 | 41 | void L3GD20::start(DIRECTION enable,void (*func)(anglerrates*)) |
lelect | 0:175bf093daa8 | 42 | { |
lelect | 0:175bf093daa8 | 43 | reboot(); |
lelect | 0:175bf093daa8 | 44 | userFunction=func; |
lelect | 0:175bf093daa8 | 45 | _int2.rise(this,(&L3GD20::interrupt)); |
lelect | 0:175bf093daa8 | 46 | start(enable); |
lelect | 0:175bf093daa8 | 47 | enableFIFO(L3GD20::BYPASSmode,L3GD20::overrun,30); |
lelect | 0:175bf093daa8 | 48 | enableFIFO(L3GD20::FIFOmode,L3GD20::watermark,30); |
lelect | 0:175bf093daa8 | 49 | } |
lelect | 0:175bf093daa8 | 50 | void L3GD20::interrupt(void) |
lelect | 0:175bf093daa8 | 51 | { |
lelect | 0:175bf093daa8 | 52 | printf("call\r\n"); |
lelect | 0:175bf093daa8 | 53 | updateFIFO(); |
lelect | 0:175bf093daa8 | 54 | if(FIFO.status==L3GD20::watermark) { |
lelect | 0:175bf093daa8 | 55 | while(_status.FIFO_SRC_REG.B.FSS!=0) { |
lelect | 0:175bf093daa8 | 56 | read(&_value,XYZ); |
lelect | 0:175bf093daa8 | 57 | updateFIFO(); |
lelect | 0:175bf093daa8 | 58 | if(NULL!=userFunction) { |
lelect | 0:175bf093daa8 | 59 | userFunction(&_value); |
lelect | 0:175bf093daa8 | 60 | } |
lelect | 0:175bf093daa8 | 61 | } |
lelect | 0:175bf093daa8 | 62 | } |
lelect | 0:175bf093daa8 | 63 | } |
lelect | 0:175bf093daa8 | 64 | void L3GD20::stop() |
lelect | 0:175bf093daa8 | 65 | { |
lelect | 0:175bf093daa8 | 66 | _config.CTRL_REG1.word=_config.CTRL_REG1.word&0xF0; |
lelect | 0:175bf093daa8 | 67 | write(CtrlReg1,_config.CTRL_REG1.word); |
lelect | 0:175bf093daa8 | 68 | } |
lelect | 0:175bf093daa8 | 69 | void L3GD20::sleep() |
lelect | 0:175bf093daa8 | 70 | { |
lelect | 0:175bf093daa8 | 71 | _config.CTRL_REG1.word=_config.CTRL_REG1.word&0xF8; |
lelect | 0:175bf093daa8 | 72 | write(CtrlReg1,_config.CTRL_REG1.word); |
lelect | 0:175bf093daa8 | 73 | } |
lelect | 0:175bf093daa8 | 74 | void L3GD20::reboot() |
lelect | 0:175bf093daa8 | 75 | { |
lelect | 0:175bf093daa8 | 76 | _config.CTRL_REG5.b.BOOT=1; |
lelect | 0:175bf093daa8 | 77 | write(CtrlReg5,_config.CTRL_REG5.word); |
lelect | 0:175bf093daa8 | 78 | } |
lelect | 0:175bf093daa8 | 79 | void L3GD20::read(anglerrates* val,DIRECTION direction) |
lelect | 0:175bf093daa8 | 80 | { |
lelect | 0:175bf093daa8 | 81 | if(direction&X) { |
lelect | 0:175bf093daa8 | 82 | int x_l,x_h; |
lelect | 0:175bf093daa8 | 83 | read(OutXL,&x_l); |
lelect | 0:175bf093daa8 | 84 | read(OutXH,&x_h); |
lelect | 0:175bf093daa8 | 85 | val->X=(x_l)|(x_h<<8); |
lelect | 0:175bf093daa8 | 86 | } |
lelect | 0:175bf093daa8 | 87 | if(direction&Y) { |
lelect | 0:175bf093daa8 | 88 | int y_l,y_h; |
lelect | 0:175bf093daa8 | 89 | read(OutYL,&y_l); |
lelect | 0:175bf093daa8 | 90 | read(OutYH,&y_h); |
lelect | 0:175bf093daa8 | 91 | val->Y=(y_l)|(y_h<<8); |
lelect | 0:175bf093daa8 | 92 | } |
lelect | 0:175bf093daa8 | 93 | if(direction&Z) { |
lelect | 0:175bf093daa8 | 94 | int z_l,z_h; |
lelect | 0:175bf093daa8 | 95 | read(OutZL,&z_l); |
lelect | 0:175bf093daa8 | 96 | read(OutZH,&z_h); |
lelect | 0:175bf093daa8 | 97 | val->Z=(z_l)|(z_h<<8); |
lelect | 0:175bf093daa8 | 98 | } |
lelect | 0:175bf093daa8 | 99 | if(_config.CTRL_REG4.B.FS==0x00) { |
lelect | 0:175bf093daa8 | 100 | val->x=8.75*(float)val->X; |
lelect | 0:175bf093daa8 | 101 | val->y=8.75*(float)val->Y; |
lelect | 0:175bf093daa8 | 102 | val->z=8.75*(float)val->Z; |
lelect | 0:175bf093daa8 | 103 | } else if(_config.CTRL_REG4.B.FS==0x01) { |
lelect | 0:175bf093daa8 | 104 | val->x=17.5*(float)val->X; |
lelect | 0:175bf093daa8 | 105 | val->y=17.5*(float)val->Y; |
lelect | 0:175bf093daa8 | 106 | val->z=17.5*(float)val->Z; |
lelect | 0:175bf093daa8 | 107 | } else if((_config.CTRL_REG4.B.FS==0x10)|(_config.CTRL_REG4.B.FS==0x11)) { |
lelect | 0:175bf093daa8 | 108 | val->x=70.0*(float)val->X; |
lelect | 0:175bf093daa8 | 109 | val->y=70.0*(float)val->Y; |
lelect | 0:175bf093daa8 | 110 | val->z=70.0*(float)val->Z; |
lelect | 0:175bf093daa8 | 111 | } |
lelect | 0:175bf093daa8 | 112 | } |
lelect | 0:175bf093daa8 | 113 | int L3GD20::readTemperature() |
lelect | 0:175bf093daa8 | 114 | { |
lelect | 0:175bf093daa8 | 115 | read(OutTemp,&_status.OUT_TEMP); |
lelect | 0:175bf093daa8 | 116 | return _status.OUT_TEMP; |
lelect | 0:175bf093daa8 | 117 | } |
lelect | 0:175bf093daa8 | 118 | void L3GD20::enableFIFO(FIFO_mode mode,FIFOstatus interrupt,const int threshold) |
lelect | 0:175bf093daa8 | 119 | { |
lelect | 0:175bf093daa8 | 120 | _config.CTRL_REG5.b.FIFO_EN=1; |
lelect | 0:175bf093daa8 | 121 | if(interrupt&empty) { |
lelect | 0:175bf093daa8 | 122 | _config.CTRL_REG3.b.I2_Empty=1; |
lelect | 0:175bf093daa8 | 123 | } else if(interrupt&watermark) { |
lelect | 0:175bf093daa8 | 124 | _config.CTRL_REG3.b.I2_WTM=1; |
lelect | 0:175bf093daa8 | 125 | } else if(interrupt&overrun) { |
lelect | 0:175bf093daa8 | 126 | _config.CTRL_REG3.b.I2_ORun=1; |
lelect | 0:175bf093daa8 | 127 | } |
lelect | 0:175bf093daa8 | 128 | _config.FIFO_CTRL_REG.B.FM=mode; |
lelect | 0:175bf093daa8 | 129 | if((0<threshold)&&(threshold<31)) { |
lelect | 0:175bf093daa8 | 130 | _config.FIFO_CTRL_REG.B.WTM=threshold; |
lelect | 0:175bf093daa8 | 131 | } else { |
lelect | 0:175bf093daa8 | 132 | _config.FIFO_CTRL_REG.B.WTM=30; |
lelect | 0:175bf093daa8 | 133 | } |
lelect | 0:175bf093daa8 | 134 | write(CtrlReg3,_config.CTRL_REG3.word); |
lelect | 0:175bf093daa8 | 135 | write(CtrlReg5,_config.CTRL_REG5.word); |
lelect | 0:175bf093daa8 | 136 | write(FIFOCtrlReg,_config.FIFO_CTRL_REG.word); |
lelect | 0:175bf093daa8 | 137 | } |
lelect | 0:175bf093daa8 | 138 | int L3GD20::updateFIFO(void) |
lelect | 0:175bf093daa8 | 139 | { |
lelect | 0:175bf093daa8 | 140 | read(FIFOSrcReg,&_status.FIFO_SRC_REG.word); |
lelect | 0:175bf093daa8 | 141 | read(FIFOCtrlReg,&_config.FIFO_CTRL_REG.word); |
lelect | 0:175bf093daa8 | 142 | |
lelect | 0:175bf093daa8 | 143 | if(_status.FIFO_SRC_REG.b.WTM) { |
lelect | 0:175bf093daa8 | 144 | FIFO.status=watermark; |
lelect | 0:175bf093daa8 | 145 | } else if(_status.FIFO_SRC_REG.b.EMPTY) { |
lelect | 0:175bf093daa8 | 146 | FIFO.status=empty; |
lelect | 0:175bf093daa8 | 147 | } else if(_status.FIFO_SRC_REG.b.OVR) { |
lelect | 0:175bf093daa8 | 148 | FIFO.status=overrun; |
lelect | 0:175bf093daa8 | 149 | } else { |
lelect | 0:175bf093daa8 | 150 | FIFO.status=none; |
lelect | 0:175bf093daa8 | 151 | } |
lelect | 0:175bf093daa8 | 152 | FIFO.level=_status.FIFO_SRC_REG.B.FSS; |
lelect | 0:175bf093daa8 | 153 | return FIFO.level; |
lelect | 0:175bf093daa8 | 154 | } |
lelect | 0:175bf093daa8 | 155 | |
lelect | 0:175bf093daa8 | 156 | |
lelect | 0:175bf093daa8 | 157 | |
lelect | 0:175bf093daa8 | 158 | |
lelect | 0:175bf093daa8 | 159 | |
lelect | 0:175bf093daa8 | 160 | void L3GD20::configReadOut(void) |
lelect | 0:175bf093daa8 | 161 | { |
lelect | 0:175bf093daa8 | 162 | read(CtrlReg1,&_config.CTRL_REG1.word); |
lelect | 0:175bf093daa8 | 163 | read(CtrlReg2,&_config.CTRL_REG2.word); |
lelect | 0:175bf093daa8 | 164 | read(CtrlReg3,&_config.CTRL_REG3.word); |
lelect | 0:175bf093daa8 | 165 | read(CtrlReg4,&_config.CTRL_REG4.word); |
lelect | 0:175bf093daa8 | 166 | read(CtrlReg5,&_config.CTRL_REG5.word); |
lelect | 0:175bf093daa8 | 167 | read(Reference,&_config.REF_DATACAP.word); |
lelect | 0:175bf093daa8 | 168 | read(FIFOCtrlReg,&_config.FIFO_CTRL_REG.word); |
lelect | 0:175bf093daa8 | 169 | read(INT1Cfg,&_config.INT1_CFG.word); |
lelect | 0:175bf093daa8 | 170 | read(INT1ThsXH,&_config.INT1_TSH_XH.word); |
lelect | 0:175bf093daa8 | 171 | read(INT1ThsXL,&_config.INT1_TSH_XL.word); |
lelect | 0:175bf093daa8 | 172 | read(INT1ThsYH,&_config.INT1_TSH_YH.word); |
lelect | 0:175bf093daa8 | 173 | read(INT1ThsYL,&_config.INT1_TSH_YL.word); |
lelect | 0:175bf093daa8 | 174 | read(INT1ThsZH,&_config.INT1_TSH_ZH.word); |
lelect | 0:175bf093daa8 | 175 | read(INT1ThsZL,&_config.INT1_TSH_ZL.word); |
lelect | 0:175bf093daa8 | 176 | read(INT1Duration,&_config.INT1_DURATION.word); |
lelect | 0:175bf093daa8 | 177 | } |
lelect | 0:175bf093daa8 | 178 | |
lelect | 0:175bf093daa8 | 179 | void L3GD20::statusReadOut(void) |
lelect | 0:175bf093daa8 | 180 | { |
lelect | 0:175bf093daa8 | 181 | read(OutTemp,&_status.OUT_TEMP); |
lelect | 0:175bf093daa8 | 182 | read(StatusReg,&_status.STATUS_REG); |
lelect | 0:175bf093daa8 | 183 | read(FIFOSrcReg,&_status.FIFO_SRC_REG.word); |
lelect | 0:175bf093daa8 | 184 | read(INT1Src,&_status.INT1_SRC); |
lelect | 0:175bf093daa8 | 185 | } |
lelect | 0:175bf093daa8 | 186 | |
lelect | 0:175bf093daa8 | 187 | |
lelect | 0:175bf093daa8 | 188 | |
lelect | 0:175bf093daa8 | 189 |