MAG3110 Virgo robot adaptation
Fork of MAG3110 by
MAG3110.cpp@14:73a4a09a49af, 2016-09-09 (annotated)
- Committer:
- akashvibhute
- Date:
- Fri Sep 09 10:48:07 2016 +0000
- Revision:
- 14:73a4a09a49af
- Parent:
- 12:2680e94139bb
- Child:
- 15:58c62b985e68
added sensor rest to overcome overexposure
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 | { |
akashvibhute | 11:31b140f32906 | 8 | |
SomeRandomBloke | 0:63a8594a3866 | 9 | } |
SomeRandomBloke | 0:63a8594a3866 | 10 | |
akashvibhute | 12:2680e94139bb | 11 | void MAG3110::begin(int16_t off_x, int16_t off_y, int16_t off_z) |
SomeRandomBloke | 0:63a8594a3866 | 12 | { |
SomeRandomBloke | 1:5a0e7a58d980 | 13 | char cmd[2]; |
akashvibhute | 11:31b140f32906 | 14 | |
akashvibhute | 11:31b140f32906 | 15 | cmd[0] = MAG_CTRL_REG1; |
akashvibhute | 11:31b140f32906 | 16 | cmd[1] = 0x00; //Puts device in Standby mode |
akashvibhute | 11:31b140f32906 | 17 | _i2c.write(MAG_ADDR, cmd,2); |
SomeRandomBloke | 0:63a8594a3866 | 18 | |
SomeRandomBloke | 1:5a0e7a58d980 | 19 | cmd[0] = MAG_CTRL_REG2; |
akashvibhute | 14:73a4a09a49af | 20 | cmd[1] = 0x90; //Set Automatic Magnetic Sensor Reset bit, and reset overexposure |
akashvibhute | 6:b2bb387caf3f | 21 | _i2c.write(MAG_ADDR, cmd, 2); |
SomeRandomBloke | 1:5a0e7a58d980 | 22 | |
SomeRandomBloke | 1:5a0e7a58d980 | 23 | cmd[0] = MAG_CTRL_REG1; |
akashvibhute | 12:2680e94139bb | 24 | cmd[1] = 0x01; //Set ODR=80Hz and OSR=16 and set device to active mode. |
akashvibhute | 6:b2bb387caf3f | 25 | _i2c.write(MAG_ADDR, cmd, 2); |
SomeRandomBloke | 1:5a0e7a58d980 | 26 | |
akashvibhute | 12:2680e94139bb | 27 | char data_bytes[7]; |
akashvibhute | 12:2680e94139bb | 28 | data_bytes[0]=MAG_OFF_X_MSB; //Write offset values to X,Y,Z registers |
akashvibhute | 12:2680e94139bb | 29 | |
akashvibhute | 12:2680e94139bb | 30 | data_bytes[1] = (off_x & 0xFF00) >> 8; |
akashvibhute | 12:2680e94139bb | 31 | data_bytes[2] = (off_x & 0xFF); |
akashvibhute | 12:2680e94139bb | 32 | |
akashvibhute | 12:2680e94139bb | 33 | data_bytes[3] = (off_y & 0xFF00) >> 8; |
akashvibhute | 12:2680e94139bb | 34 | data_bytes[4] = (off_y & 0xFF); |
akashvibhute | 12:2680e94139bb | 35 | |
akashvibhute | 12:2680e94139bb | 36 | data_bytes[5] = (off_z & 0xFF00) >> 8; |
akashvibhute | 12:2680e94139bb | 37 | data_bytes[6] = (off_z & 0xFF); |
akashvibhute | 12:2680e94139bb | 38 | |
akashvibhute | 12:2680e94139bb | 39 | _i2c.write(MAG_ADDR,data_bytes,7); |
SomeRandomBloke | 0:63a8594a3866 | 40 | } |
SomeRandomBloke | 0:63a8594a3866 | 41 | |
SomeRandomBloke | 0:63a8594a3866 | 42 | // Read a single byte form 8 bit register, return as int |
SomeRandomBloke | 0:63a8594a3866 | 43 | int MAG3110::readReg(char regAddr) |
SomeRandomBloke | 0:63a8594a3866 | 44 | { |
SomeRandomBloke | 0:63a8594a3866 | 45 | char cmd[1]; |
SomeRandomBloke | 0:63a8594a3866 | 46 | |
SomeRandomBloke | 0:63a8594a3866 | 47 | cmd[0] = regAddr; |
akashvibhute | 6:b2bb387caf3f | 48 | _i2c.write(MAG_ADDR, cmd, 1); |
SomeRandomBloke | 0:63a8594a3866 | 49 | |
SomeRandomBloke | 0:63a8594a3866 | 50 | cmd[0] = 0x00; |
akashvibhute | 6:b2bb387caf3f | 51 | _i2c.read(MAG_ADDR, cmd, 1); |
SomeRandomBloke | 0:63a8594a3866 | 52 | return (int)( cmd[0]); |
SomeRandomBloke | 0:63a8594a3866 | 53 | } |
SomeRandomBloke | 0:63a8594a3866 | 54 | |
SomeRandomBloke | 0:63a8594a3866 | 55 | |
SomeRandomBloke | 0:63a8594a3866 | 56 | // read a register per, pass first reg value, reading 2 bytes increments register |
SomeRandomBloke | 0:63a8594a3866 | 57 | // Reads MSB first then LSB |
SomeRandomBloke | 0:63a8594a3866 | 58 | int MAG3110::readVal(char regAddr) |
SomeRandomBloke | 0:63a8594a3866 | 59 | { |
SomeRandomBloke | 0:63a8594a3866 | 60 | char cmd[2]; |
SomeRandomBloke | 0:63a8594a3866 | 61 | |
SomeRandomBloke | 0:63a8594a3866 | 62 | cmd[0] = regAddr; |
akashvibhute | 6:b2bb387caf3f | 63 | _i2c.write(MAG_ADDR, cmd, 1); |
SomeRandomBloke | 0:63a8594a3866 | 64 | |
SomeRandomBloke | 0:63a8594a3866 | 65 | cmd[0] = 0x00; |
SomeRandomBloke | 0:63a8594a3866 | 66 | cmd[1] = 0x00; |
akashvibhute | 6:b2bb387caf3f | 67 | _i2c.read(MAG_ADDR, cmd, 2); |
SomeRandomBloke | 0:63a8594a3866 | 68 | return (int)( (cmd[1]|(cmd[0] << 8))); //concatenate the MSB and LSB |
SomeRandomBloke | 0:63a8594a3866 | 69 | } |
SomeRandomBloke | 0:63a8594a3866 | 70 | |
SomeRandomBloke | 0:63a8594a3866 | 71 | void MAG3110::getValues(int *xVal, int *yVal, int *zVal) |
SomeRandomBloke | 0:63a8594a3866 | 72 | { |
SomeRandomBloke | 0:63a8594a3866 | 73 | *xVal = readVal(MAG_OUT_X_MSB); |
SomeRandomBloke | 0:63a8594a3866 | 74 | *yVal = readVal(MAG_OUT_Y_MSB); |
SomeRandomBloke | 0:63a8594a3866 | 75 | *zVal = readVal(MAG_OUT_Z_MSB); |
SomeRandomBloke | 0:63a8594a3866 | 76 | } |
SomeRandomBloke | 0:63a8594a3866 | 77 | |
SomeRandomBloke | 0:63a8594a3866 | 78 | |
akashvibhute | 6:b2bb387caf3f | 79 | void MAG3110::setCalibration(float minX, float maxX, float minY, float maxY, float minZ, float maxZ) |
SomeRandomBloke | 0:63a8594a3866 | 80 | { |
SomeRandomBloke | 0:63a8594a3866 | 81 | _avgX=(maxX+minX)/2; |
SomeRandomBloke | 0:63a8594a3866 | 82 | _avgY=(maxY+minY)/2; |
akashvibhute | 6:b2bb387caf3f | 83 | _avgZ=(maxZ+minZ)/2; |
akashvibhute | 6:b2bb387caf3f | 84 | } |
akashvibhute | 6:b2bb387caf3f | 85 | |
akashvibhute | 11:31b140f32906 | 86 | void MAG3110::get_uT(float * uT) |
akashvibhute | 6:b2bb387caf3f | 87 | { |
akashvibhute | 12:2680e94139bb | 88 | |
akashvibhute | 6:b2bb387caf3f | 89 | char data_bytes[7]; |
akashvibhute | 6:b2bb387caf3f | 90 | char d[1]; |
akashvibhute | 8:203af65371e8 | 91 | d[0]=MAG_DR_STATUS; |
akashvibhute | 6:b2bb387caf3f | 92 | |
akashvibhute | 6:b2bb387caf3f | 93 | _i2c.write(MAG_ADDR,d,1,true); |
akashvibhute | 6:b2bb387caf3f | 94 | _i2c.read(MAG_ADDR,data_bytes,7);// Read the 6 data bytes - LSB and MSB for X, Y and Z Axes. |
akashvibhute | 12:2680e94139bb | 95 | |
akashvibhute | 11:31b140f32906 | 96 | uT[0] = (float)( (int16_t)data_bytes[1] << 8 | (int16_t)data_bytes[2] ) * 0.1; |
akashvibhute | 11:31b140f32906 | 97 | uT[1] = (float)( (int16_t)data_bytes[3] << 8 | (int16_t)data_bytes[4] ) * 0.1; |
akashvibhute | 11:31b140f32906 | 98 | uT[2] = (float)( (int16_t)data_bytes[5] << 8 | (int16_t)data_bytes[6] ) * 0.1; |
akashvibhute | 12:2680e94139bb | 99 | |
akashvibhute | 12:2680e94139bb | 100 | /* |
akashvibhute | 12:2680e94139bb | 101 | int xVal, yVal, zVal; |
akashvibhute | 12:2680e94139bb | 102 | |
akashvibhute | 12:2680e94139bb | 103 | getValues(&xVal,&yVal, &zVal); |
akashvibhute | 12:2680e94139bb | 104 | |
akashvibhute | 12:2680e94139bb | 105 | uT[0] = (float)( xVal ) * 0.1; |
akashvibhute | 12:2680e94139bb | 106 | uT[1] = (float)( yVal ) * 0.1; |
akashvibhute | 12:2680e94139bb | 107 | uT[2] = (float)( zVal ) * 0.1; |
akashvibhute | 12:2680e94139bb | 108 | */ |
akashvibhute | 12:2680e94139bb | 109 | |
SomeRandomBloke | 0:63a8594a3866 | 110 | } |
SomeRandomBloke | 0:63a8594a3866 | 111 | |
SomeRandomBloke | 0:63a8594a3866 | 112 | |
SomeRandomBloke | 0:63a8594a3866 | 113 |