Added functions to independently get accelerometer and magnetometer data
Fork of LSM303DLHC by
LSM303DLHC.cpp@6:5fe568883921, 2015-04-27 (annotated)
- Committer:
- Spilly
- Date:
- Mon Apr 27 16:49:03 2015 +0000
- Revision:
- 6:5fe568883921
- Parent:
- 5:dd17c7b96e2b
- Child:
- 7:6c74e3e5e105
BoatProject
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Spilly | 5:dd17c7b96e2b | 1 | /************************************************************************************************************************************************************** |
Spilly | 5:dd17c7b96e2b | 2 | // This is a modified version of mbed /users/bclaus/code/LSM303DLHC/ for LSM303DLHC |
Spilly | 5:dd17c7b96e2b | 3 | // |
Spilly | 5:dd17c7b96e2b | 4 | // Changes made by Ryan Spillman: |
Spilly | 5:dd17c7b96e2b | 5 | // Added initialization function to configure ODR, filters, and other settings |
Spilly | 5:dd17c7b96e2b | 6 | // Added seperate functions for reading the magnetometer and acclerometer seperately (they do not operate at the same ODR) |
Spilly | 5:dd17c7b96e2b | 7 | **************************************************************************************************************************************************************/ |
bclaus | 3:4d9465e7e10e | 8 | #include "mbed.h" |
bclaus | 3:4d9465e7e10e | 9 | #include "LSM303DLHC.h" |
bclaus | 3:4d9465e7e10e | 10 | |
bclaus | 3:4d9465e7e10e | 11 | |
bclaus | 3:4d9465e7e10e | 12 | const int addr_acc = 0x32; |
bclaus | 3:4d9465e7e10e | 13 | const int addr_mag = 0x3c; |
bclaus | 3:4d9465e7e10e | 14 | |
bclaus | 3:4d9465e7e10e | 15 | enum REG_ADDRS { |
bclaus | 3:4d9465e7e10e | 16 | /* --- Mag --- */ |
bclaus | 3:4d9465e7e10e | 17 | CRA_REG_M = 0x00, |
bclaus | 3:4d9465e7e10e | 18 | CRB_REG_M = 0x01, |
bclaus | 3:4d9465e7e10e | 19 | MR_REG_M = 0x02, |
bclaus | 3:4d9465e7e10e | 20 | OUT_X_M = 0x03, |
bclaus | 3:4d9465e7e10e | 21 | OUT_Y_M = 0x05, |
bclaus | 3:4d9465e7e10e | 22 | OUT_Z_M = 0x07, |
bclaus | 3:4d9465e7e10e | 23 | /* --- Acc --- */ |
bclaus | 3:4d9465e7e10e | 24 | CTRL_REG1_A = 0x20, |
bclaus | 3:4d9465e7e10e | 25 | CTRL_REG4_A = 0x23, |
bclaus | 3:4d9465e7e10e | 26 | OUT_X_A = 0x28, |
bclaus | 3:4d9465e7e10e | 27 | OUT_Y_A = 0x2A, |
bclaus | 3:4d9465e7e10e | 28 | OUT_Z_A = 0x2C, |
bclaus | 3:4d9465e7e10e | 29 | }; |
bclaus | 3:4d9465e7e10e | 30 | |
bclaus | 3:4d9465e7e10e | 31 | bool LSM303DLHC::write_reg(int addr_i2c,int addr_reg, char v) |
bclaus | 3:4d9465e7e10e | 32 | { |
bclaus | 3:4d9465e7e10e | 33 | char data[2] = {addr_reg, v}; |
bclaus | 3:4d9465e7e10e | 34 | return LSM303DLHC::_LSM303.write(addr_i2c, data, 2) == 0; |
bclaus | 3:4d9465e7e10e | 35 | } |
bclaus | 3:4d9465e7e10e | 36 | |
bclaus | 3:4d9465e7e10e | 37 | bool LSM303DLHC::read_reg(int addr_i2c,int addr_reg, char *v) |
bclaus | 3:4d9465e7e10e | 38 | { |
bclaus | 3:4d9465e7e10e | 39 | char data = addr_reg; |
bclaus | 3:4d9465e7e10e | 40 | bool result = false; |
bclaus | 3:4d9465e7e10e | 41 | |
bclaus | 3:4d9465e7e10e | 42 | __disable_irq(); |
bclaus | 3:4d9465e7e10e | 43 | if ((_LSM303.write(addr_i2c, &data, 1) == 0) && (_LSM303.read(addr_i2c, &data, 1) == 0)){ |
bclaus | 3:4d9465e7e10e | 44 | *v = data; |
bclaus | 3:4d9465e7e10e | 45 | result = true; |
bclaus | 3:4d9465e7e10e | 46 | } |
bclaus | 3:4d9465e7e10e | 47 | __enable_irq(); |
bclaus | 3:4d9465e7e10e | 48 | return result; |
bclaus | 3:4d9465e7e10e | 49 | } |
bclaus | 3:4d9465e7e10e | 50 | |
bclaus | 3:4d9465e7e10e | 51 | |
bclaus | 3:4d9465e7e10e | 52 | LSM303DLHC::LSM303DLHC(PinName sda, PinName scl): |
bclaus | 3:4d9465e7e10e | 53 | _LSM303(sda, scl) |
bclaus | 3:4d9465e7e10e | 54 | { |
bclaus | 3:4d9465e7e10e | 55 | char reg_v; |
bclaus | 3:4d9465e7e10e | 56 | _LSM303.frequency(100000); |
bclaus | 3:4d9465e7e10e | 57 | |
bclaus | 3:4d9465e7e10e | 58 | reg_v = 0; |
bclaus | 3:4d9465e7e10e | 59 | |
bclaus | 3:4d9465e7e10e | 60 | reg_v |= 0x27; /* X/Y/Z axis enable. */ |
bclaus | 3:4d9465e7e10e | 61 | write_reg(addr_acc,CTRL_REG1_A,reg_v); |
bclaus | 3:4d9465e7e10e | 62 | |
bclaus | 3:4d9465e7e10e | 63 | reg_v = 0; |
bclaus | 3:4d9465e7e10e | 64 | // reg_v |= 0x01 << 6; /* 1: data MSB @ lower address */ |
bclaus | 3:4d9465e7e10e | 65 | reg_v = 0x01 << 4; /* +/- 4g */ |
bclaus | 3:4d9465e7e10e | 66 | write_reg(addr_acc,CTRL_REG4_A,reg_v); |
bclaus | 3:4d9465e7e10e | 67 | |
bclaus | 3:4d9465e7e10e | 68 | /* -- mag --- */ |
bclaus | 3:4d9465e7e10e | 69 | reg_v = 0; |
Spilly | 5:dd17c7b96e2b | 70 | //leave line below commented for 0.75Hz data output rate (less noise) |
Spilly | 5:dd17c7b96e2b | 71 | //reg_v |= 0x04 << 2; /* Minimum data output rate = 15Hz */ |
bclaus | 3:4d9465e7e10e | 72 | write_reg(addr_mag,CRA_REG_M,reg_v); |
Spilly | 5:dd17c7b96e2b | 73 | |
bclaus | 3:4d9465e7e10e | 74 | reg_v = 0; |
bclaus | 3:4d9465e7e10e | 75 | reg_v |= 0x01 << 5; /* +-1.3Gauss */ |
bclaus | 3:4d9465e7e10e | 76 | //reg_v |= 0x07 << 5; /* +-8.1Gauss */ |
bclaus | 3:4d9465e7e10e | 77 | write_reg(addr_mag,CRB_REG_M,reg_v); |
bclaus | 3:4d9465e7e10e | 78 | |
bclaus | 3:4d9465e7e10e | 79 | reg_v = 0; /* Continuous-conversion mode */ |
bclaus | 3:4d9465e7e10e | 80 | write_reg(addr_mag,MR_REG_M,reg_v); |
bclaus | 3:4d9465e7e10e | 81 | } |
bclaus | 3:4d9465e7e10e | 82 | |
bclaus | 3:4d9465e7e10e | 83 | |
bclaus | 3:4d9465e7e10e | 84 | bool LSM303DLHC::read(float *ax, float *ay, float *az, float *mx, float *my, float *mz) { |
bclaus | 3:4d9465e7e10e | 85 | char acc[6], mag[6]; |
bclaus | 3:4d9465e7e10e | 86 | |
bclaus | 3:4d9465e7e10e | 87 | if (recv(addr_acc, OUT_X_A, acc, 6) && recv(addr_mag, OUT_X_M, mag, 6)) { |
bclaus | 3:4d9465e7e10e | 88 | *ax = float(short(acc[1] << 8 | acc[0]))/8192; //32768/4=8192 |
Spilly | 5:dd17c7b96e2b | 89 | //*ax = float(short(acc[1] << 8 | acc[0])); |
bclaus | 3:4d9465e7e10e | 90 | *ay = float(short(acc[3] << 8 | acc[2]))/8192; |
Spilly | 5:dd17c7b96e2b | 91 | //*ay = float(short(acc[3] << 8 | acc[2])); |
bclaus | 3:4d9465e7e10e | 92 | *az = float(short(acc[5] << 8 | acc[4]))/8192; |
Spilly | 5:dd17c7b96e2b | 93 | //*az = float(short(acc[5] << 8 | acc[4])); |
bclaus | 3:4d9465e7e10e | 94 | //full scale magnetic readings are from -2048 to 2047 |
bclaus | 3:4d9465e7e10e | 95 | //gain is x,y =1100; z = 980 LSB/gauss |
bclaus | 3:4d9465e7e10e | 96 | *mx = float(short(mag[0] << 8 | mag[1]))/1100; |
Spilly | 5:dd17c7b96e2b | 97 | //*mx = float(short(mag[0] << 8 | mag[1]))/450; |
Spilly | 5:dd17c7b96e2b | 98 | //*mx = float(short(mag[0] << 8 | mag[1])); |
bclaus | 3:4d9465e7e10e | 99 | *mz = float(short(mag[2] << 8 | mag[3]))/980; |
Spilly | 5:dd17c7b96e2b | 100 | //*mz = float(short(mag[2] << 8 | mag[3]))/400; |
Spilly | 5:dd17c7b96e2b | 101 | //*mz = float(short(mag[2] << 8 | mag[3])); |
bclaus | 3:4d9465e7e10e | 102 | *my = float(short(mag[4] << 8 | mag[5]))/1100; |
Spilly | 5:dd17c7b96e2b | 103 | //*my = float(short(mag[4] << 8 | mag[5]))/450; |
Spilly | 5:dd17c7b96e2b | 104 | //*my = float(short(mag[4] << 8 | mag[5])); |
Spilly | 5:dd17c7b96e2b | 105 | |
Spilly | 5:dd17c7b96e2b | 106 | return true; |
Spilly | 5:dd17c7b96e2b | 107 | } |
Spilly | 5:dd17c7b96e2b | 108 | |
Spilly | 5:dd17c7b96e2b | 109 | return false; |
Spilly | 5:dd17c7b96e2b | 110 | } |
Spilly | 5:dd17c7b96e2b | 111 | |
Spilly | 5:dd17c7b96e2b | 112 | bool LSM303DLHC::ReadAccOnly(float *ax, float *ay, float *az) |
Spilly | 5:dd17c7b96e2b | 113 | { |
Spilly | 6:5fe568883921 | 114 | char acc[6]; |
Spilly | 5:dd17c7b96e2b | 115 | |
Spilly | 5:dd17c7b96e2b | 116 | if (recv(addr_acc, OUT_X_A, acc, 6)) |
Spilly | 5:dd17c7b96e2b | 117 | { |
Spilly | 5:dd17c7b96e2b | 118 | *ax = float(short(acc[1] << 8 | acc[0]))/8192; //32768/4=8192 |
Spilly | 5:dd17c7b96e2b | 119 | //*ax = float(short(acc[1] << 8 | acc[0])); |
Spilly | 5:dd17c7b96e2b | 120 | *ay = float(short(acc[3] << 8 | acc[2]))/8192; |
Spilly | 5:dd17c7b96e2b | 121 | //*ay = float(short(acc[3] << 8 | acc[2])); |
Spilly | 5:dd17c7b96e2b | 122 | *az = float(short(acc[5] << 8 | acc[4]))/8192; |
Spilly | 5:dd17c7b96e2b | 123 | //*az = float(short(acc[5] << 8 | acc[4])); |
bclaus | 3:4d9465e7e10e | 124 | |
bclaus | 3:4d9465e7e10e | 125 | return true; |
bclaus | 3:4d9465e7e10e | 126 | } |
bclaus | 3:4d9465e7e10e | 127 | |
bclaus | 3:4d9465e7e10e | 128 | return false; |
bclaus | 3:4d9465e7e10e | 129 | } |
bclaus | 3:4d9465e7e10e | 130 | |
Spilly | 5:dd17c7b96e2b | 131 | bool LSM303DLHC::ReadMagnOnly(float *mx, float *my, float *mz) |
Spilly | 5:dd17c7b96e2b | 132 | { |
Spilly | 5:dd17c7b96e2b | 133 | char mag[6]; |
Spilly | 5:dd17c7b96e2b | 134 | |
Spilly | 5:dd17c7b96e2b | 135 | if (recv(addr_mag, OUT_X_M, mag, 6)) |
Spilly | 5:dd17c7b96e2b | 136 | { |
Spilly | 5:dd17c7b96e2b | 137 | *mx = float(short(mag[0] << 8 | mag[1]))/1100; |
Spilly | 5:dd17c7b96e2b | 138 | //*mx = float(short(mag[0] << 8 | mag[1]))/450; |
Spilly | 5:dd17c7b96e2b | 139 | //*mx = float(short(mag[0] << 8 | mag[1])); |
Spilly | 5:dd17c7b96e2b | 140 | *mz = float(short(mag[2] << 8 | mag[3]))/980; |
Spilly | 5:dd17c7b96e2b | 141 | //*mz = float(short(mag[2] << 8 | mag[3]))/400; |
Spilly | 5:dd17c7b96e2b | 142 | //*mz = float(short(mag[2] << 8 | mag[3])); |
Spilly | 5:dd17c7b96e2b | 143 | *my = float(short(mag[4] << 8 | mag[5]))/1100; |
Spilly | 5:dd17c7b96e2b | 144 | //*my = float(short(mag[4] << 8 | mag[5]))/450; |
Spilly | 5:dd17c7b96e2b | 145 | //*my = float(short(mag[4] << 8 | mag[5])); |
Spilly | 5:dd17c7b96e2b | 146 | |
Spilly | 5:dd17c7b96e2b | 147 | return true; |
Spilly | 5:dd17c7b96e2b | 148 | } |
Spilly | 5:dd17c7b96e2b | 149 | |
Spilly | 5:dd17c7b96e2b | 150 | return false; |
Spilly | 5:dd17c7b96e2b | 151 | } |
bclaus | 3:4d9465e7e10e | 152 | |
bclaus | 3:4d9465e7e10e | 153 | bool LSM303DLHC::recv(char sad, char sub, char *buf, int length) { |
bclaus | 3:4d9465e7e10e | 154 | if (length > 1) sub |= 0x80; |
bclaus | 3:4d9465e7e10e | 155 | |
bclaus | 3:4d9465e7e10e | 156 | return _LSM303.write(sad, &sub, 1, true) == 0 && _LSM303.read(sad, buf, length) == 0; |
bclaus | 3:4d9465e7e10e | 157 | } |
Spilly | 5:dd17c7b96e2b | 158 | |
Spilly | 5:dd17c7b96e2b | 159 | void LSM303DLHC::init(void) |
Spilly | 5:dd17c7b96e2b | 160 | { |
Spilly | 5:dd17c7b96e2b | 161 | char reg_v; |
Spilly | 5:dd17c7b96e2b | 162 | _LSM303.frequency(100000); |
Spilly | 5:dd17c7b96e2b | 163 | |
Spilly | 5:dd17c7b96e2b | 164 | reg_v = 0; |
Spilly | 5:dd17c7b96e2b | 165 | |
Spilly | 5:dd17c7b96e2b | 166 | reg_v |= 0x47; // X/Y/Z axis enable. ODR set to 50 Hz |
Spilly | 5:dd17c7b96e2b | 167 | write_reg(addr_acc,CTRL_REG1_A,reg_v); |
Spilly | 5:dd17c7b96e2b | 168 | |
Spilly | 5:dd17c7b96e2b | 169 | reg_v = 0; |
Spilly | 5:dd17c7b96e2b | 170 | // reg_v |= 0x01 << 6; /* 1: data MSB @ lower address */ |
Spilly | 5:dd17c7b96e2b | 171 | reg_v = 0x01 << 4; /* +/- 4g */ |
Spilly | 5:dd17c7b96e2b | 172 | write_reg(addr_acc,CTRL_REG4_A,reg_v); |
Spilly | 5:dd17c7b96e2b | 173 | |
Spilly | 5:dd17c7b96e2b | 174 | /* -- mag --- */ |
Spilly | 5:dd17c7b96e2b | 175 | reg_v = 0; |
Spilly | 5:dd17c7b96e2b | 176 | //leave line below commented for 0.75Hz data output rate (less noise) |
Spilly | 5:dd17c7b96e2b | 177 | reg_v |= 0x04 << 2; /* Minimum data output rate = 15Hz */ |
Spilly | 5:dd17c7b96e2b | 178 | write_reg(addr_mag,CRA_REG_M,reg_v); |
Spilly | 5:dd17c7b96e2b | 179 | |
Spilly | 5:dd17c7b96e2b | 180 | reg_v = 0; |
Spilly | 5:dd17c7b96e2b | 181 | reg_v |= 0x01 << 5; /* +-1.3Gauss */ |
Spilly | 5:dd17c7b96e2b | 182 | //reg_v |= 0x07 << 5; /* +-8.1Gauss */ |
Spilly | 5:dd17c7b96e2b | 183 | write_reg(addr_mag,CRB_REG_M,reg_v); |
Spilly | 5:dd17c7b96e2b | 184 | |
Spilly | 5:dd17c7b96e2b | 185 | reg_v = 0; /* Continuous-conversion mode */ |
Spilly | 5:dd17c7b96e2b | 186 | write_reg(addr_mag,MR_REG_M,reg_v); |
Spilly | 5:dd17c7b96e2b | 187 | } |