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