src/Energia/libraries/ZumoCC3200/utility/L3G.cpp

   1 /*
   2  *  ======== L3G.cpp ========
   3  */
   4
   5 #include <Wire.h>
   6 #include <math.h>
   7
   8 #include "L3G.h"
   9
  10 // Defines ////////////////////////////////////////////////////////////////
  11
  12 /* The Arduino two-wire interface uses a 7-bit number for the address,
  13  * and sets the last bit correctly based on reads and writes
  14  */
  15 #define D20_SA0_HIGH_ADDRESS      0b1101011 // also applies to D20H
  16 #define D20_SA0_LOW_ADDRESS       0b1101010 // also applies to D20H
  17 #define L3G4200D_SA0_HIGH_ADDRESS 0b1101001
  18 #define L3G4200D_SA0_LOW_ADDRESS  0b1101000
  19
  20 #define TEST_REG_ERROR -1
  21
  22 #define D20H_WHO_ID     0xD7
  23 #define D20_WHO_ID      0xD4
  24 #define L3G4200D_WHO_ID 0xD3
  25
  26 static int testReg(byte address, L3G::regAddr reg);
  27
  28 /*
  29  *  ======== L3G ========
  30  *  Constructors for L3G gyro
  31  */
  32 L3G::L3G(void)
  33 {
  34     _device = device_auto;
  35
  36     io_timeout = 0;  // 0 = no timeout
  37     did_timeout = false;
  38 }
  39
  40 /*
  41  *  ======== timeoutOccurred ========
  42  *  Did a timeout occur in read() since the last call to timeoutOccurred()?
  43  */
  44 bool L3G::timeoutOccurred()
  45 {
  46     bool tmp = did_timeout;
  47     did_timeout = false;
  48     return tmp;
  49 }
  50
  51 /*
  52  *  ======== setTimeout ========
  53  */
  54 void L3G::setTimeout(unsigned int timeout)
  55 {
  56     io_timeout = timeout;
  57 }
  58
  59 /*
  60  *  ======== getTimeout ========
  61  */
  62 unsigned int L3G::getTimeout()
  63 {
  64     return io_timeout;
  65 }
  66
  67 /*
  68  *  ======== init ========
  69  */
  70 bool L3G::init(deviceType device, sa0State sa0)
  71 {
  72     int id;
  73
  74     /* perform auto-detection unless device type and SA0 state were both
  75      * specified
  76      */
  77     if (device == device_auto || sa0 == sa0_auto) {
  78         /* check for L3GD20H, D20 if device is unidentified or was specified
  79          * to be one of these types
  80          */
  81         if (device == device_auto || device == device_D20H
  82                 || device == device_D20) {
  83             // check SA0 high address unless SA0 was specified to be low
  84             if (sa0
  85                     != sa0_low && (id = testReg(D20_SA0_HIGH_ADDRESS, WHO_AM_I)) != TEST_REG_ERROR) {
  86                 /* device responds to address 1101011; it's a D20H or D20
  87                  * with SA0 high
  88                  */
  89                 sa0 = sa0_high;
  90                 if (device == device_auto) {
  91                     // use ID from WHO_AM_I register to determine device type
  92                     device = (id == D20H_WHO_ID) ? device_D20H : device_D20;
  93                 }
  94             }
  95             // check SA0 low address unless SA0 was specified to be high
  96             else if (sa0
  97                     != sa0_high && (id = testReg(D20_SA0_LOW_ADDRESS, WHO_AM_I)) != TEST_REG_ERROR) {
  98                 /* device responds to address 1101010; it's a D20H or D20
  99                  * with SA0 low
 100                  */
 101                 sa0 = sa0_low;
 102                 if (device == device_auto) {
 103                     // use ID from WHO_AM_I register to determine device type
 104                     device = (id == D20H_WHO_ID) ? device_D20H : device_D20;
 105                 }
 106             }
 107         }
 108
 109         /* check for L3G4200D if device is still unidentified or was specified
 110          * to be this type
 111          */
 112         if (device == device_auto || device == device_4200D) {
 113             if (sa0
 114                     != sa0_low && testReg(L3G4200D_SA0_HIGH_ADDRESS, WHO_AM_I) == L3G4200D_WHO_ID) {
 115                 /* device responds to address 1101001; it's a 4200D with SA0
 116                  * high
 117                  */
 118                 device = device_4200D;
 119                 sa0 = sa0_high;
 120             }
 121             else if (sa0
 122                     != sa0_high && testReg(L3G4200D_SA0_LOW_ADDRESS, WHO_AM_I) == L3G4200D_WHO_ID) {
 123                 /* device responds to address 1101000; it's a 4200D with SA0
 124                  * low
 125                  */
 126                 device = device_4200D;
 127                 sa0 = sa0_low;
 128             }
 129         }
 130
 131         /* make sure device and SA0 were successfully detected; otherwise,
 132          * indicate failure
 133          */
 134         if (device == device_auto || sa0 == sa0_auto) {
 135             return false;
 136         }
 137     }
 138
 139     _device = device;
 140
 141     // set device address
 142     switch (device) {
 143         case device_D20H:
 144         case device_D20:
 145             address =
 146                     (sa0 == sa0_high) ?
 147                             D20_SA0_HIGH_ADDRESS : D20_SA0_LOW_ADDRESS;
 148             break;
 149
 150         case device_4200D:
 151             address =
 152                     (sa0 == sa0_high) ?
 153                             L3G4200D_SA0_HIGH_ADDRESS :
 154                             L3G4200D_SA0_LOW_ADDRESS;
 155             break;
 156     }
 157
 158     return true;
 159 }
 160
 161 /*
 162  *  ======== enableDefault ========
 163  *  Enables the L3G's gyro
 164  *
 165  *  Also:
 166  *      - Sets gyro full scale (gain) to default power-on value of +/- 250 dps
 167  *       (specified as +/- 245 dps for L3GD20H).
 168  *      - Selects 380 Hz ODR (output data rate). (Exact rate is specified as
 169  *        189.4 Hz for L3GD20H and 190 Hz for L3GD20.)
 170  *
 171  *  Note that this function will also reset other settings controlled by
 172  *  the registers it writes to.
 173  */
 174 void L3G::enableDefault(void)
 175 {
 176     if (_device == device_D20H) {
 177         // 0x00 = 0b00000000
 178         //     Low_ODR = 0 (low speed ODR disabled)
 179         writeReg(LOW_ODR, 0x00);
 180     }
 181
 182     // 0x00 = 0b00000000
 183     //     FS = 00 (+/- 250 dps full scale)
 184     writeReg(CTRL_REG4, 0x00);
 185
 186     // 0xAF = 0b10101111
 187     //     DR = 10 (380 Hz ODR);
 188     //     BW = 10 (50 Hz bandwidth);
 189     //     PD = 1 (normal mode);
 190     //     Zen = Yen = Xen = 1 (all axes enabled)
 191     writeReg(CTRL_REG1, 0xAF);
 192 }
 193
 194 /*
 195  *  ======== writeReg ========
 196  *  Writes a gyro register
 197  */
 198 void L3G::writeReg(byte reg, byte value)
 199 {
 200     Wire.beginTransmission(address);
 201     Wire.write(reg);
 202     Wire.write(value);
 203     last_status = Wire.endTransmission();
 204 }
 205
 206 /*
 207  *  ======== readReg ========
 208  *  Reads a gyro register
 209  */
 210 byte L3G::readReg(byte reg)
 211 {
 212     byte value;
 213
 214     Wire.beginTransmission(address);
 215     Wire.write(reg);
 216     last_status = Wire.endTransmission();
 217     Wire.requestFrom(address, (byte) 1);
 218     value = Wire.read();
 219     Wire.endTransmission();
 220
 221     return value;
 222 }
 223
 224 /*
 225  *  ======== read ========
 226  *  Reads the 3 gyro channels and stores them in vector g
 227  */
 228 void L3G::read()
 229 {
 230     Wire.beginTransmission(address);
 231     // assert the MSB of the address to get the gyro
 232     // to do slave-transmit subaddress updating.
 233     Wire.write(OUT_X_L | (1 << 7));
 234     Wire.endTransmission();
 235     Wire.requestFrom(address, (byte) 6);
 236
 237     unsigned int millis_start = millis();
 238     while (Wire.available() < 6) {
 239         if (io_timeout > 0
 240                 && ((unsigned int) millis() - millis_start) > io_timeout) {
 241             did_timeout = true;
 242             return;
 243         }
 244     }
 245
 246     uint8_t xlg = Wire.read();
 247     uint8_t xhg = Wire.read();
 248     uint8_t ylg = Wire.read();
 249     uint8_t yhg = Wire.read();
 250     uint8_t zlg = Wire.read();
 251     uint8_t zhg = Wire.read();
 252
 253     // combine high and low bytes
 254     g.x = (int16_t)(xhg << 8 | xlg);
 255     g.y = (int16_t)(yhg << 8 | ylg);
 256     g.z = (int16_t)(zhg << 8 | zlg);
 257 }
 258
 259 /*
 260  *  ======== vector_normalize ========
 261  */
 262 void L3G::vector_normalize(vector<float> *a)
 263 {
 264     float mag = sqrt(vector_dot(a, a));
 265     a->x /= mag;
 266     a->y /= mag;
 267     a->z /= mag;
 268 }
 269
 270 /*
 271  *  ======== testReg ========
 272  */
 273 static int testReg(byte address, L3G::regAddr reg)
 274 {
 275     Wire.beginTransmission(address);
 276     Wire.write((byte) reg);
 277     if (Wire.endTransmission() != 0) {
 278         return TEST_REG_ERROR;
 279     }
 280
 281     Wire.requestFrom(address, (byte) 1);
 282     if (Wire.available()) {
 283         return Wire.read();
 284     }
 285     else {
 286         return TEST_REG_ERROR;
 287     }
 288 }