MAG3110 Virgo robot adaptation
Fork of MAG3110 by
MAG3110.cpp@8:203af65371e8, 2016-08-01 (annotated)
- Committer:
- akashvibhute
- Date:
- Mon Aug 01 03:04:29 2016 +0000
- Revision:
- 8:203af65371e8
- Parent:
- 6:b2bb387caf3f
- Child:
- 9:f86b9453bb46
pointer bugfix :P
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
SomeRandomBloke | 0:63a8594a3866 | 1 | #include "MAG3110.h" |
SomeRandomBloke | 0:63a8594a3866 | 2 | |
SomeRandomBloke | 0:63a8594a3866 | 3 | /****************************************************************************** |
SomeRandomBloke | 0:63a8594a3866 | 4 | * Constructors |
SomeRandomBloke | 0:63a8594a3866 | 5 | ******************************************************************************/ |
akashvibhute | 6:b2bb387caf3f | 6 | MAG3110::MAG3110(PinName sda, PinName scl): _i2c(sda, scl) |
SomeRandomBloke | 1:5a0e7a58d980 | 7 | { |
SomeRandomBloke | 1:5a0e7a58d980 | 8 | begin(); |
SomeRandomBloke | 0:63a8594a3866 | 9 | } |
SomeRandomBloke | 0:63a8594a3866 | 10 | |
SomeRandomBloke | 1:5a0e7a58d980 | 11 | void MAG3110::begin() |
SomeRandomBloke | 0:63a8594a3866 | 12 | { |
SomeRandomBloke | 1:5a0e7a58d980 | 13 | char cmd[2]; |
SomeRandomBloke | 0:63a8594a3866 | 14 | |
SomeRandomBloke | 1:5a0e7a58d980 | 15 | cmd[0] = MAG_CTRL_REG2; |
SomeRandomBloke | 1:5a0e7a58d980 | 16 | cmd[1] = 0x80; |
akashvibhute | 6:b2bb387caf3f | 17 | _i2c.write(MAG_ADDR, cmd, 2); |
SomeRandomBloke | 1:5a0e7a58d980 | 18 | |
SomeRandomBloke | 1:5a0e7a58d980 | 19 | cmd[0] = MAG_CTRL_REG1; |
SomeRandomBloke | 1:5a0e7a58d980 | 20 | cmd[1] = MAG_3110_SAMPLE80+MAG_3110_OVERSAMPLE2+MAG_3110_ACTIVE; |
akashvibhute | 6:b2bb387caf3f | 21 | _i2c.write(MAG_ADDR, cmd, 2); |
SomeRandomBloke | 1:5a0e7a58d980 | 22 | |
SomeRandomBloke | 1:5a0e7a58d980 | 23 | // No adjustment initially |
SomeRandomBloke | 1:5a0e7a58d980 | 24 | _avgX = 0; |
SomeRandomBloke | 1:5a0e7a58d980 | 25 | _avgY = 0; |
SomeRandomBloke | 0:63a8594a3866 | 26 | } |
SomeRandomBloke | 0:63a8594a3866 | 27 | |
SomeRandomBloke | 0:63a8594a3866 | 28 | // Read a single byte form 8 bit register, return as int |
SomeRandomBloke | 0:63a8594a3866 | 29 | int MAG3110::readReg(char regAddr) |
SomeRandomBloke | 0:63a8594a3866 | 30 | { |
SomeRandomBloke | 0:63a8594a3866 | 31 | char cmd[1]; |
SomeRandomBloke | 0:63a8594a3866 | 32 | |
SomeRandomBloke | 0:63a8594a3866 | 33 | cmd[0] = regAddr; |
akashvibhute | 6:b2bb387caf3f | 34 | _i2c.write(MAG_ADDR, cmd, 1); |
SomeRandomBloke | 0:63a8594a3866 | 35 | |
SomeRandomBloke | 0:63a8594a3866 | 36 | cmd[0] = 0x00; |
akashvibhute | 6:b2bb387caf3f | 37 | _i2c.read(MAG_ADDR, cmd, 1); |
SomeRandomBloke | 0:63a8594a3866 | 38 | return (int)( cmd[0]); |
SomeRandomBloke | 0:63a8594a3866 | 39 | } |
SomeRandomBloke | 0:63a8594a3866 | 40 | |
SomeRandomBloke | 0:63a8594a3866 | 41 | |
SomeRandomBloke | 0:63a8594a3866 | 42 | // read a register per, pass first reg value, reading 2 bytes increments register |
SomeRandomBloke | 0:63a8594a3866 | 43 | // Reads MSB first then LSB |
SomeRandomBloke | 0:63a8594a3866 | 44 | int MAG3110::readVal(char regAddr) |
SomeRandomBloke | 0:63a8594a3866 | 45 | { |
SomeRandomBloke | 0:63a8594a3866 | 46 | char cmd[2]; |
SomeRandomBloke | 0:63a8594a3866 | 47 | |
SomeRandomBloke | 0:63a8594a3866 | 48 | cmd[0] = regAddr; |
akashvibhute | 6:b2bb387caf3f | 49 | _i2c.write(MAG_ADDR, cmd, 1); |
SomeRandomBloke | 0:63a8594a3866 | 50 | |
SomeRandomBloke | 0:63a8594a3866 | 51 | cmd[0] = 0x00; |
SomeRandomBloke | 0:63a8594a3866 | 52 | cmd[1] = 0x00; |
akashvibhute | 6:b2bb387caf3f | 53 | _i2c.read(MAG_ADDR, cmd, 2); |
SomeRandomBloke | 0:63a8594a3866 | 54 | return (int)( (cmd[1]|(cmd[0] << 8))); //concatenate the MSB and LSB |
SomeRandomBloke | 0:63a8594a3866 | 55 | } |
SomeRandomBloke | 0:63a8594a3866 | 56 | |
SomeRandomBloke | 0:63a8594a3866 | 57 | void MAG3110::getValues(int *xVal, int *yVal, int *zVal) |
SomeRandomBloke | 0:63a8594a3866 | 58 | { |
SomeRandomBloke | 0:63a8594a3866 | 59 | *xVal = readVal(MAG_OUT_X_MSB); |
SomeRandomBloke | 0:63a8594a3866 | 60 | *yVal = readVal(MAG_OUT_Y_MSB); |
SomeRandomBloke | 0:63a8594a3866 | 61 | *zVal = readVal(MAG_OUT_Z_MSB); |
SomeRandomBloke | 0:63a8594a3866 | 62 | } |
SomeRandomBloke | 0:63a8594a3866 | 63 | |
SomeRandomBloke | 0:63a8594a3866 | 64 | |
akashvibhute | 6:b2bb387caf3f | 65 | void MAG3110::setCalibration(float minX, float maxX, float minY, float maxY, float minZ, float maxZ) |
SomeRandomBloke | 0:63a8594a3866 | 66 | { |
SomeRandomBloke | 0:63a8594a3866 | 67 | _avgX=(maxX+minX)/2; |
SomeRandomBloke | 0:63a8594a3866 | 68 | _avgY=(maxY+minY)/2; |
akashvibhute | 6:b2bb387caf3f | 69 | _avgZ=(maxZ+minZ)/2; |
akashvibhute | 6:b2bb387caf3f | 70 | } |
akashvibhute | 6:b2bb387caf3f | 71 | |
akashvibhute | 8:203af65371e8 | 72 | void MAG3110::get_uT(float *uT[3]) |
akashvibhute | 6:b2bb387caf3f | 73 | { |
akashvibhute | 6:b2bb387caf3f | 74 | char data_bytes[7]; |
akashvibhute | 6:b2bb387caf3f | 75 | char d[1]; |
akashvibhute | 8:203af65371e8 | 76 | d[0]=MAG_DR_STATUS; |
akashvibhute | 6:b2bb387caf3f | 77 | |
akashvibhute | 6:b2bb387caf3f | 78 | _i2c.write(MAG_ADDR,d,1,true); |
akashvibhute | 6:b2bb387caf3f | 79 | _i2c.read(MAG_ADDR,data_bytes,7);// Read the 6 data bytes - LSB and MSB for X, Y and Z Axes. |
akashvibhute | 6:b2bb387caf3f | 80 | |
akashvibhute | 6:b2bb387caf3f | 81 | *uT[0] = (float)((int16_t)((data_bytes[1]*256) + (data_bytes[2]))) * 0.1; |
akashvibhute | 6:b2bb387caf3f | 82 | *uT[1] = (float)((int16_t)((data_bytes[3]*256) + (data_bytes[4]))) * 0.1; |
akashvibhute | 6:b2bb387caf3f | 83 | *uT[2] = (float)((int16_t)((data_bytes[5]*256) + (data_bytes[6]))) * 0.1; |
SomeRandomBloke | 0:63a8594a3866 | 84 | } |
SomeRandomBloke | 0:63a8594a3866 | 85 | |
SomeRandomBloke | 0:63a8594a3866 | 86 | |
SomeRandomBloke | 0:63a8594a3866 | 87 |