work nice now
Fork of LSM303DLHC by
LSM303DLHC.cpp@7:ddd9717cdb71, 2013-11-17 (annotated)
- Committer:
- patsteph
- Date:
- Sun Nov 17 17:58:43 2013 +0000
- Revision:
- 7:ddd9717cdb71
- Parent:
- 6:fcf7e9b8ce21
mbed working with arduino program
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
patsteph | 7:ddd9717cdb71 | 1 | |
bclaus | 3:4d9465e7e10e | 2 | #include "mbed.h" |
bclaus | 3:4d9465e7e10e | 3 | #include "LSM303DLHC.h" |
bclaus | 3:4d9465e7e10e | 4 | |
bclaus | 3:4d9465e7e10e | 5 | |
bclaus | 3:4d9465e7e10e | 6 | const int addr_acc = 0x32; |
bclaus | 3:4d9465e7e10e | 7 | const int addr_mag = 0x3c; |
bclaus | 3:4d9465e7e10e | 8 | |
bclaus | 3:4d9465e7e10e | 9 | bool LSM303DLHC::write_reg(int addr_i2c,int addr_reg, char v) |
bclaus | 3:4d9465e7e10e | 10 | { |
bclaus | 3:4d9465e7e10e | 11 | char data[2] = {addr_reg, v}; |
bclaus | 3:4d9465e7e10e | 12 | return LSM303DLHC::_LSM303.write(addr_i2c, data, 2) == 0; |
bclaus | 3:4d9465e7e10e | 13 | } |
bclaus | 3:4d9465e7e10e | 14 | |
bclaus | 3:4d9465e7e10e | 15 | bool LSM303DLHC::read_reg(int addr_i2c,int addr_reg, char *v) |
bclaus | 3:4d9465e7e10e | 16 | { |
bclaus | 3:4d9465e7e10e | 17 | char data = addr_reg; |
bclaus | 3:4d9465e7e10e | 18 | bool result = false; |
bclaus | 3:4d9465e7e10e | 19 | |
bclaus | 3:4d9465e7e10e | 20 | __disable_irq(); |
bclaus | 3:4d9465e7e10e | 21 | if ((_LSM303.write(addr_i2c, &data, 1) == 0) && (_LSM303.read(addr_i2c, &data, 1) == 0)){ |
bclaus | 3:4d9465e7e10e | 22 | *v = data; |
bclaus | 3:4d9465e7e10e | 23 | result = true; |
bclaus | 3:4d9465e7e10e | 24 | } |
bclaus | 3:4d9465e7e10e | 25 | __enable_irq(); |
bclaus | 3:4d9465e7e10e | 26 | return result; |
bclaus | 3:4d9465e7e10e | 27 | } |
bclaus | 3:4d9465e7e10e | 28 | |
bclaus | 3:4d9465e7e10e | 29 | |
bclaus | 3:4d9465e7e10e | 30 | LSM303DLHC::LSM303DLHC(PinName sda, PinName scl): |
bclaus | 3:4d9465e7e10e | 31 | _LSM303(sda, scl) |
bclaus | 3:4d9465e7e10e | 32 | { |
bclaus | 3:4d9465e7e10e | 33 | char reg_v; |
bclaus | 3:4d9465e7e10e | 34 | _LSM303.frequency(100000); |
bclaus | 3:4d9465e7e10e | 35 | |
bclaus | 3:4d9465e7e10e | 36 | reg_v = 0; |
bclaus | 3:4d9465e7e10e | 37 | |
bclaus | 3:4d9465e7e10e | 38 | reg_v |= 0x27; /* X/Y/Z axis enable. */ |
bclaus | 3:4d9465e7e10e | 39 | write_reg(addr_acc,CTRL_REG1_A,reg_v); |
bclaus | 3:4d9465e7e10e | 40 | |
bclaus | 3:4d9465e7e10e | 41 | reg_v = 0; |
bclaus | 3:4d9465e7e10e | 42 | // reg_v |= 0x01 << 6; /* 1: data MSB @ lower address */ |
bclaus | 3:4d9465e7e10e | 43 | reg_v = 0x01 << 4; /* +/- 4g */ |
bclaus | 3:4d9465e7e10e | 44 | write_reg(addr_acc,CTRL_REG4_A,reg_v); |
bclaus | 3:4d9465e7e10e | 45 | |
bclaus | 3:4d9465e7e10e | 46 | /* -- mag --- */ |
bclaus | 3:4d9465e7e10e | 47 | reg_v = 0; |
bclaus | 3:4d9465e7e10e | 48 | reg_v |= 0x04 << 2; /* Minimum data output rate = 15Hz */ |
bclaus | 3:4d9465e7e10e | 49 | write_reg(addr_mag,CRA_REG_M,reg_v); |
bclaus | 3:4d9465e7e10e | 50 | |
bclaus | 3:4d9465e7e10e | 51 | reg_v = 0; |
bclaus | 3:4d9465e7e10e | 52 | reg_v |= 0x01 << 5; /* +-1.3Gauss */ |
bclaus | 3:4d9465e7e10e | 53 | //reg_v |= 0x07 << 5; /* +-8.1Gauss */ |
bclaus | 3:4d9465e7e10e | 54 | write_reg(addr_mag,CRB_REG_M,reg_v); |
bclaus | 3:4d9465e7e10e | 55 | |
bclaus | 3:4d9465e7e10e | 56 | reg_v = 0; /* Continuous-conversion mode */ |
bclaus | 3:4d9465e7e10e | 57 | write_reg(addr_mag,MR_REG_M,reg_v); |
bclaus | 3:4d9465e7e10e | 58 | } |
bclaus | 3:4d9465e7e10e | 59 | |
bclaus | 3:4d9465e7e10e | 60 | |
patsteph | 7:ddd9717cdb71 | 61 | bool LSM303DLHC::readcomp(float *mx, float *my, float *mz) |
patsteph | 7:ddd9717cdb71 | 62 | { |
patsteph | 7:ddd9717cdb71 | 63 | char mag[6]; |
patsteph | 6:fcf7e9b8ce21 | 64 | char temp; |
patsteph | 7:ddd9717cdb71 | 65 | |
patsteph | 7:ddd9717cdb71 | 66 | if (recv(addr_mag, OUT_X_M, mag, 6)) |
patsteph | 7:ddd9717cdb71 | 67 | { |
patsteph | 7:ddd9717cdb71 | 68 | for(int i=0; i<6; i+=2) |
patsteph | 7:ddd9717cdb71 | 69 | { |
patsteph | 7:ddd9717cdb71 | 70 | temp = mag[i]; |
patsteph | 7:ddd9717cdb71 | 71 | mag[i] = mag[i+1]; //la boucle inverse les valeurs du table |
patsteph | 7:ddd9717cdb71 | 72 | mag[i+1] = temp; |
patsteph | 7:ddd9717cdb71 | 73 | } |
patsteph | 7:ddd9717cdb71 | 74 | |
patsteph | 7:ddd9717cdb71 | 75 | |
patsteph | 7:ddd9717cdb71 | 76 | *mx = *((short*)(mag));///1100.0; |
patsteph | 7:ddd9717cdb71 | 77 | *mz = *((short*)(mag+2));///980.0; //ATTENTION: le z vient avant le y, c'est normal!!!!! |
patsteph | 7:ddd9717cdb71 | 78 | *my = *((short*)(mag+4));///1100.0; |
patsteph | 7:ddd9717cdb71 | 79 | |
bclaus | 3:4d9465e7e10e | 80 | |
patsteph | 7:ddd9717cdb71 | 81 | return true; |
patsteph | 7:ddd9717cdb71 | 82 | } |
patsteph | 7:ddd9717cdb71 | 83 | |
patsteph | 7:ddd9717cdb71 | 84 | return false; |
patsteph | 7:ddd9717cdb71 | 85 | } |
patsteph | 7:ddd9717cdb71 | 86 | bool LSM303DLHC::readacc(float *ax, float *ay, float *az) |
patsteph | 7:ddd9717cdb71 | 87 | { |
patsteph | 7:ddd9717cdb71 | 88 | char acc[6]; |
patsteph | 7:ddd9717cdb71 | 89 | |
patsteph | 7:ddd9717cdb71 | 90 | if (recv(addr_acc, OUT_X_A, acc, 6)) |
patsteph | 7:ddd9717cdb71 | 91 | { |
patsteph | 7:ddd9717cdb71 | 92 | *ax = *((short*)(acc)) >>4;///8192.0; //32768/4=8192 |
patsteph | 7:ddd9717cdb71 | 93 | *ay = *((short*)(acc+2)) >>4;///8192.0; |
patsteph | 7:ddd9717cdb71 | 94 | *az = *((short*)(acc+4)) >>4;///8192.0; |
bclaus | 3:4d9465e7e10e | 95 | //full scale magnetic readings are from -2048 to 2047 |
bclaus | 3:4d9465e7e10e | 96 | //gain is x,y =1100; z = 980 LSB/gauss |
patsteph | 7:ddd9717cdb71 | 97 | |
patsteph | 7:ddd9717cdb71 | 98 | |
bclaus | 3:4d9465e7e10e | 99 | |
bclaus | 3:4d9465e7e10e | 100 | return true; |
bclaus | 3:4d9465e7e10e | 101 | } |
bclaus | 3:4d9465e7e10e | 102 | |
bclaus | 3:4d9465e7e10e | 103 | return false; |
bclaus | 3:4d9465e7e10e | 104 | } |
bclaus | 3:4d9465e7e10e | 105 | |
bclaus | 3:4d9465e7e10e | 106 | bool LSM303DLHC::recv(char sad, char sub, char *buf, int length) { |
bclaus | 3:4d9465e7e10e | 107 | if (length > 1) sub |= 0x80; |
bclaus | 3:4d9465e7e10e | 108 | |
bclaus | 3:4d9465e7e10e | 109 | return _LSM303.write(sad, &sub, 1, true) == 0 && _LSM303.read(sad, buf, length) == 0; |
bclaus | 3:4d9465e7e10e | 110 | } |