Simple library for MAG3110 magenetometer as built into Avnet Wi-Go module
Dependents: Wi-Go-MagnetometerTest EE202A_HW1_MH serialtoxively mbed_nanosec_timer ... more
MAG3110.cpp@9:1da3fe7b3510, 2014-05-16 (annotated)
- Committer:
- JimCarver
- Date:
- Fri May 16 18:19:17 2014 +0000
- Revision:
- 9:1da3fe7b3510
- Parent:
- 5:f3abe901c33a
Implements new virtual MotionSensor class
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
SomeRandomBloke | 0:63a8594a3866 | 1 | |
SomeRandomBloke | 0:63a8594a3866 | 2 | #include "MAG3110.h" |
SomeRandomBloke | 0:63a8594a3866 | 3 | #include "mbed.h" |
JimCarver | 9:1da3fe7b3510 | 4 | #include "MotionSensor.h" |
SomeRandomBloke | 0:63a8594a3866 | 5 | |
SomeRandomBloke | 0:63a8594a3866 | 6 | /****************************************************************************** |
SomeRandomBloke | 0:63a8594a3866 | 7 | * Constructors |
SomeRandomBloke | 0:63a8594a3866 | 8 | ******************************************************************************/ |
JimCarver | 9:1da3fe7b3510 | 9 | MAG3110::MAG3110(PinName sda, PinName scl): m_i2c(sda, scl), |
JimCarver | 9:1da3fe7b3510 | 10 | m_addr(0x1d) |
SomeRandomBloke | 0:63a8594a3866 | 11 | { |
SomeRandomBloke | 1:5a0e7a58d980 | 12 | char cmd[2]; |
SomeRandomBloke | 0:63a8594a3866 | 13 | |
SomeRandomBloke | 1:5a0e7a58d980 | 14 | cmd[0] = MAG_CTRL_REG2; |
SomeRandomBloke | 1:5a0e7a58d980 | 15 | cmd[1] = 0x80; |
JimCarver | 9:1da3fe7b3510 | 16 | m_i2c.write(m_addr, cmd, 2); |
JimCarver | 9:1da3fe7b3510 | 17 | |
JimCarver | 9:1da3fe7b3510 | 18 | } |
SomeRandomBloke | 1:5a0e7a58d980 | 19 | |
JimCarver | 9:1da3fe7b3510 | 20 | void MAG3110::enable(void) { |
JimCarver | 9:1da3fe7b3510 | 21 | uint8_t data[2]; |
JimCarver | 9:1da3fe7b3510 | 22 | readRegs( MAG_CTRL_REG1, &data[1], 1); |
JimCarver | 9:1da3fe7b3510 | 23 | data[1] |= 0x01; |
JimCarver | 9:1da3fe7b3510 | 24 | data[0] = MAG_CTRL_REG1; |
JimCarver | 9:1da3fe7b3510 | 25 | writeRegs(data, 2); |
SomeRandomBloke | 0:63a8594a3866 | 26 | } |
SomeRandomBloke | 0:63a8594a3866 | 27 | |
JimCarver | 9:1da3fe7b3510 | 28 | |
JimCarver | 9:1da3fe7b3510 | 29 | void MAG3110::disable(void) { |
JimCarver | 9:1da3fe7b3510 | 30 | uint8_t data[2]; |
JimCarver | 9:1da3fe7b3510 | 31 | readRegs( MAG_CTRL_REG1, &data[1], 1); |
JimCarver | 9:1da3fe7b3510 | 32 | data[1] &= 0xFE; |
JimCarver | 9:1da3fe7b3510 | 33 | data[0] = MAG_CTRL_REG1; |
JimCarver | 9:1da3fe7b3510 | 34 | writeRegs(data, 2); |
JimCarver | 9:1da3fe7b3510 | 35 | } |
JimCarver | 9:1da3fe7b3510 | 36 | |
JimCarver | 9:1da3fe7b3510 | 37 | |
JimCarver | 9:1da3fe7b3510 | 38 | void MAG3110::readRegs(int addr, uint8_t * data, int len) |
SomeRandomBloke | 0:63a8594a3866 | 39 | { |
SomeRandomBloke | 0:63a8594a3866 | 40 | char cmd[1]; |
SomeRandomBloke | 0:63a8594a3866 | 41 | |
JimCarver | 9:1da3fe7b3510 | 42 | cmd[0] = addr; |
JimCarver | 9:1da3fe7b3510 | 43 | m_i2c.write( m_addr, cmd, 1, true); |
JimCarver | 9:1da3fe7b3510 | 44 | m_i2c.read( m_addr, (char *) data, len); |
JimCarver | 9:1da3fe7b3510 | 45 | return; |
JimCarver | 9:1da3fe7b3510 | 46 | } |
JimCarver | 9:1da3fe7b3510 | 47 | |
JimCarver | 9:1da3fe7b3510 | 48 | |
JimCarver | 9:1da3fe7b3510 | 49 | void MAG3110::writeRegs(uint8_t * data, int len) { |
JimCarver | 9:1da3fe7b3510 | 50 | m_i2c.write(m_addr, (char *)data, len); |
JimCarver | 9:1da3fe7b3510 | 51 | } |
JimCarver | 9:1da3fe7b3510 | 52 | |
JimCarver | 9:1da3fe7b3510 | 53 | |
JimCarver | 9:1da3fe7b3510 | 54 | uint32_t MAG3110::whoAmI() { |
JimCarver | 9:1da3fe7b3510 | 55 | uint8_t who_am_i = 0; |
JimCarver | 9:1da3fe7b3510 | 56 | readRegs(MAG_WHOAMI, &who_am_i, 1); |
JimCarver | 9:1da3fe7b3510 | 57 | return (uint32_t) who_am_i; |
JimCarver | 9:1da3fe7b3510 | 58 | } |
JimCarver | 9:1da3fe7b3510 | 59 | |
JimCarver | 9:1da3fe7b3510 | 60 | uint32_t MAG3110::dataReady(void) { |
JimCarver | 9:1da3fe7b3510 | 61 | uint8_t stat = 0; |
JimCarver | 9:1da3fe7b3510 | 62 | readRegs(MAG_DR_STATUS, &stat, 1); |
JimCarver | 9:1da3fe7b3510 | 63 | return (uint32_t) stat; |
JimCarver | 9:1da3fe7b3510 | 64 | } |
JimCarver | 9:1da3fe7b3510 | 65 | |
JimCarver | 9:1da3fe7b3510 | 66 | uint32_t MAG3110::sampleRate(uint32_t f) { |
JimCarver | 9:1da3fe7b3510 | 67 | return(50); // for now sample rate is fixed at 50Hz |
JimCarver | 9:1da3fe7b3510 | 68 | } |
JimCarver | 9:1da3fe7b3510 | 69 | |
JimCarver | 9:1da3fe7b3510 | 70 | |
JimCarver | 9:1da3fe7b3510 | 71 | void MAG3110::getX(float * x) { |
JimCarver | 9:1da3fe7b3510 | 72 | *x = (float(getMagAxis(MAG_OUT_X_MSB)) * 0.1f); |
JimCarver | 9:1da3fe7b3510 | 73 | } |
JimCarver | 9:1da3fe7b3510 | 74 | |
JimCarver | 9:1da3fe7b3510 | 75 | void MAG3110::getY(float * y) { |
JimCarver | 9:1da3fe7b3510 | 76 | *y = (float(getMagAxis(MAG_OUT_Y_MSB)) * 0.1f); |
JimCarver | 9:1da3fe7b3510 | 77 | } |
JimCarver | 9:1da3fe7b3510 | 78 | |
JimCarver | 9:1da3fe7b3510 | 79 | void MAG3110::getZ(float * z) { |
JimCarver | 9:1da3fe7b3510 | 80 | *z = (float(getMagAxis(MAG_OUT_Z_MSB)) * 0.1f); |
JimCarver | 9:1da3fe7b3510 | 81 | } |
JimCarver | 9:1da3fe7b3510 | 82 | |
JimCarver | 9:1da3fe7b3510 | 83 | void MAG3110::getX(int16_t * d) { |
JimCarver | 9:1da3fe7b3510 | 84 | *d = getMagAxis(MAG_OUT_X_MSB); |
SomeRandomBloke | 0:63a8594a3866 | 85 | } |
SomeRandomBloke | 0:63a8594a3866 | 86 | |
JimCarver | 9:1da3fe7b3510 | 87 | void MAG3110::getY(int16_t * d) { |
JimCarver | 9:1da3fe7b3510 | 88 | *d = getMagAxis(MAG_OUT_Y_MSB); |
JimCarver | 9:1da3fe7b3510 | 89 | } |
JimCarver | 9:1da3fe7b3510 | 90 | |
JimCarver | 9:1da3fe7b3510 | 91 | void MAG3110::getZ(int16_t * d) { |
JimCarver | 9:1da3fe7b3510 | 92 | *d = getMagAxis(MAG_OUT_Z_MSB); |
JimCarver | 9:1da3fe7b3510 | 93 | } |
JimCarver | 9:1da3fe7b3510 | 94 | |
JimCarver | 9:1da3fe7b3510 | 95 | int16_t MAG3110::getMagAxis(uint8_t addr) { |
JimCarver | 9:1da3fe7b3510 | 96 | int16_t acc; |
JimCarver | 9:1da3fe7b3510 | 97 | uint8_t res[2]; |
JimCarver | 9:1da3fe7b3510 | 98 | readRegs(addr, res, 2); |
JimCarver | 9:1da3fe7b3510 | 99 | |
JimCarver | 9:1da3fe7b3510 | 100 | acc = (res[0] << 8) | res[1]; |
JimCarver | 9:1da3fe7b3510 | 101 | |
JimCarver | 9:1da3fe7b3510 | 102 | return acc; |
JimCarver | 9:1da3fe7b3510 | 103 | } |
JimCarver | 9:1da3fe7b3510 | 104 | |
JimCarver | 9:1da3fe7b3510 | 105 | |
JimCarver | 9:1da3fe7b3510 | 106 | void MAG3110::getAxis(MotionSensorDataUnits &data) { |
JimCarver | 9:1da3fe7b3510 | 107 | int16_t t[3]; |
JimCarver | 9:1da3fe7b3510 | 108 | uint8_t res[6]; |
JimCarver | 9:1da3fe7b3510 | 109 | |
JimCarver | 9:1da3fe7b3510 | 110 | readRegs(MAG_OUT_X_MSB, res, 6); |
JimCarver | 9:1da3fe7b3510 | 111 | t[0] = (res[0] << 8) | res[1]; |
JimCarver | 9:1da3fe7b3510 | 112 | t[1] = (res[2] << 8) | res[3]; |
JimCarver | 9:1da3fe7b3510 | 113 | t[2] = (res[4] << 8) | res[5]; |
JimCarver | 9:1da3fe7b3510 | 114 | data.x = ((float) t[0]) * 0.1f; |
JimCarver | 9:1da3fe7b3510 | 115 | data.y = ((float) t[1]) * 0.1f; |
JimCarver | 9:1da3fe7b3510 | 116 | data.z = ((float) t[2]) * 0.1f; |
JimCarver | 9:1da3fe7b3510 | 117 | } |
JimCarver | 9:1da3fe7b3510 | 118 | |
JimCarver | 9:1da3fe7b3510 | 119 | |
JimCarver | 9:1da3fe7b3510 | 120 | void MAG3110::getAxis(MotionSensorDataCounts &data) { |
JimCarver | 9:1da3fe7b3510 | 121 | |
JimCarver | 9:1da3fe7b3510 | 122 | uint8_t res[6]; |
JimCarver | 9:1da3fe7b3510 | 123 | readRegs(MAG_OUT_X_MSB, res, 6); |
JimCarver | 9:1da3fe7b3510 | 124 | |
JimCarver | 9:1da3fe7b3510 | 125 | data.x = (res[0] << 8) | res[1]; |
JimCarver | 9:1da3fe7b3510 | 126 | data.y = (res[2] << 8) | res[3]; |
JimCarver | 9:1da3fe7b3510 | 127 | data.z = (res[4] << 8) | res[5]; |
JimCarver | 9:1da3fe7b3510 | 128 | } |
JimCarver | 9:1da3fe7b3510 | 129 | /* |
JimCarver | 9:1da3fe7b3510 | 130 | |
SomeRandomBloke | 0:63a8594a3866 | 131 | // read a register per, pass first reg value, reading 2 bytes increments register |
SomeRandomBloke | 0:63a8594a3866 | 132 | // Reads MSB first then LSB |
SomeRandomBloke | 0:63a8594a3866 | 133 | int MAG3110::readVal(char regAddr) |
SomeRandomBloke | 0:63a8594a3866 | 134 | { |
SomeRandomBloke | 0:63a8594a3866 | 135 | char cmd[2]; |
JimCarver | 5:f3abe901c33a | 136 | int16_t t; |
SomeRandomBloke | 0:63a8594a3866 | 137 | cmd[0] = regAddr; |
JimCarver | 9:1da3fe7b3510 | 138 | if(_i2c.write(m_addr, cmd, 1)) { |
JimCarver | 5:f3abe901c33a | 139 | printf("MAG3110 write error\r\n"); |
JimCarver | 5:f3abe901c33a | 140 | _i2c.stop(); |
JimCarver | 5:f3abe901c33a | 141 | _i2c.start(); |
JimCarver | 5:f3abe901c33a | 142 | } |
SomeRandomBloke | 0:63a8594a3866 | 143 | |
SomeRandomBloke | 0:63a8594a3866 | 144 | cmd[0] = 0x00; |
SomeRandomBloke | 0:63a8594a3866 | 145 | cmd[1] = 0x00; |
JimCarver | 9:1da3fe7b3510 | 146 | _i2c.read(m_addr, cmd, 2); |
JimCarver | 5:f3abe901c33a | 147 | t = (cmd[0] * 256) + (unsigned short) cmd[1]; |
JimCarver | 5:f3abe901c33a | 148 | return ((int) t); //concatenate the MSB and LSB |
SomeRandomBloke | 0:63a8594a3866 | 149 | } |
SomeRandomBloke | 0:63a8594a3866 | 150 | |
SomeRandomBloke | 0:63a8594a3866 | 151 | |
SomeRandomBloke | 0:63a8594a3866 | 152 | float MAG3110::getHeading() |
SomeRandomBloke | 0:63a8594a3866 | 153 | { |
SomeRandomBloke | 0:63a8594a3866 | 154 | int xVal = readVal(MAG_OUT_X_MSB); |
SomeRandomBloke | 0:63a8594a3866 | 155 | int yVal = readVal(MAG_OUT_Y_MSB); |
SomeRandomBloke | 0:63a8594a3866 | 156 | return (atan2((double)(yVal - _avgY),(double)(xVal - _avgX)))*180/PI; |
SomeRandomBloke | 0:63a8594a3866 | 157 | } |
SomeRandomBloke | 0:63a8594a3866 | 158 | |
SomeRandomBloke | 0:63a8594a3866 | 159 | void MAG3110::getValues(int *xVal, int *yVal, int *zVal) |
SomeRandomBloke | 0:63a8594a3866 | 160 | { |
SomeRandomBloke | 0:63a8594a3866 | 161 | *xVal = readVal(MAG_OUT_X_MSB); |
SomeRandomBloke | 0:63a8594a3866 | 162 | *yVal = readVal(MAG_OUT_Y_MSB); |
SomeRandomBloke | 0:63a8594a3866 | 163 | *zVal = readVal(MAG_OUT_Z_MSB); |
SomeRandomBloke | 0:63a8594a3866 | 164 | } |
SomeRandomBloke | 0:63a8594a3866 | 165 | |
JimCarver | 5:f3abe901c33a | 166 | void MAG3110::ReadXYZ(float * mag) |
JimCarver | 5:f3abe901c33a | 167 | { |
JimCarver | 5:f3abe901c33a | 168 | int x, y, z; |
JimCarver | 5:f3abe901c33a | 169 | x = readVal(MAG_OUT_X_MSB); |
JimCarver | 5:f3abe901c33a | 170 | y = readVal(MAG_OUT_Y_MSB); |
JimCarver | 5:f3abe901c33a | 171 | z = readVal(MAG_OUT_Z_MSB); |
JimCarver | 5:f3abe901c33a | 172 | mag[0] = (float) x / 10.0; |
JimCarver | 5:f3abe901c33a | 173 | mag[1] = (float) y / 10.0; |
JimCarver | 5:f3abe901c33a | 174 | mag[2] = (float) z / 10.0; |
JimCarver | 5:f3abe901c33a | 175 | |
JimCarver | 5:f3abe901c33a | 176 | } |
JimCarver | 5:f3abe901c33a | 177 | |
JimCarver | 5:f3abe901c33a | 178 | void MAG3110::ReadXYZraw(int16_t * mag_raw) |
JimCarver | 5:f3abe901c33a | 179 | { |
JimCarver | 5:f3abe901c33a | 180 | mag_raw[0] = readVal(MAG_OUT_X_MSB); |
JimCarver | 5:f3abe901c33a | 181 | mag_raw[1] = readVal(MAG_OUT_Y_MSB); |
JimCarver | 5:f3abe901c33a | 182 | mag_raw[2] = readVal(MAG_OUT_Z_MSB); |
JimCarver | 5:f3abe901c33a | 183 | } |
SomeRandomBloke | 0:63a8594a3866 | 184 | |
SomeRandomBloke | 0:63a8594a3866 | 185 | void MAG3110::setCalibration(int minX, int maxX, int minY, int maxY ) |
SomeRandomBloke | 0:63a8594a3866 | 186 | { |
SomeRandomBloke | 0:63a8594a3866 | 187 | _avgX=(maxX+minX)/2; |
SomeRandomBloke | 0:63a8594a3866 | 188 | _avgY=(maxY+minY)/2; |
SomeRandomBloke | 0:63a8594a3866 | 189 | } |
JimCarver | 9:1da3fe7b3510 | 190 | */ |
SomeRandomBloke | 0:63a8594a3866 | 191 | |
SomeRandomBloke | 0:63a8594a3866 | 192 | |
SomeRandomBloke | 0:63a8594a3866 | 193 | |
SomeRandomBloke | 0:63a8594a3866 | 194 |