LIS3MDL I2C Mbed class with compass heading function
LIS3MDL.cpp@1:fe199024ddfe, 2021-04-13 (annotated)
- Committer:
- gpmbed
- Date:
- Tue Apr 13 13:52:14 2021 +0000
- Revision:
- 1:fe199024ddfe
- Parent:
- 0:4aa6b3804281
comments removed
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
gpmbed | 0:4aa6b3804281 | 1 | #include "LIS3MDL.h" |
gpmbed | 0:4aa6b3804281 | 2 | |
gpmbed | 0:4aa6b3804281 | 3 | |
gpmbed | 0:4aa6b3804281 | 4 | LIS3MDL::LIS3MDL(PinName sda, PinName scl) : i2c(sda, scl) |
gpmbed | 0:4aa6b3804281 | 5 | { |
gpmbed | 0:4aa6b3804281 | 6 | _address = LIS3MDL_ADDRESS;//LIS3MDL_DEFAULT_ADDRESS; |
gpmbed | 0:4aa6b3804281 | 7 | } |
gpmbed | 0:4aa6b3804281 | 8 | |
gpmbed | 0:4aa6b3804281 | 9 | /* **** |
gpmbed | 0:4aa6b3804281 | 10 | addr data len |
gpmbed | 0:4aa6b3804281 | 11 | i2c.write(LSM6DS0_ADDRESS, data_write, 2); |
gpmbed | 0:4aa6b3804281 | 12 | */ |
gpmbed | 0:4aa6b3804281 | 13 | |
gpmbed | 0:4aa6b3804281 | 14 | int LIS3MDL::i2c_write(uint8_t DeviceAddr, uint8_t RegisterAddr,uint8_t value, uint16_t NumByteToWrite) |
gpmbed | 0:4aa6b3804281 | 15 | { |
gpmbed | 0:4aa6b3804281 | 16 | int ret; |
gpmbed | 0:4aa6b3804281 | 17 | int TEMP_BUF_SIZE = 5; |
gpmbed | 0:4aa6b3804281 | 18 | uint8_t tmp[TEMP_BUF_SIZE]; |
gpmbed | 0:4aa6b3804281 | 19 | |
gpmbed | 0:4aa6b3804281 | 20 | if(NumByteToWrite >= TEMP_BUF_SIZE) return -2; |
gpmbed | 0:4aa6b3804281 | 21 | |
gpmbed | 0:4aa6b3804281 | 22 | /* First, send device address. Then, send data and STOP condition */ |
gpmbed | 0:4aa6b3804281 | 23 | tmp[0] = RegisterAddr; |
gpmbed | 0:4aa6b3804281 | 24 | tmp[1] = value; |
gpmbed | 0:4aa6b3804281 | 25 | //memcpy(tmp+1, pBuffer, NumByteToWrite); |
gpmbed | 0:4aa6b3804281 | 26 | |
gpmbed | 0:4aa6b3804281 | 27 | ret = i2c.write(DeviceAddr, (const char*)tmp, NumByteToWrite+1, false); |
gpmbed | 0:4aa6b3804281 | 28 | |
gpmbed | 0:4aa6b3804281 | 29 | if(ret) return -1; |
gpmbed | 0:4aa6b3804281 | 30 | return 0; |
gpmbed | 0:4aa6b3804281 | 31 | } |
gpmbed | 0:4aa6b3804281 | 32 | |
gpmbed | 0:4aa6b3804281 | 33 | int LIS3MDL::i2c_read(uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToRead) { |
gpmbed | 0:4aa6b3804281 | 34 | int ret; |
gpmbed | 0:4aa6b3804281 | 35 | |
gpmbed | 0:4aa6b3804281 | 36 | /* Send device address, with no STOP condition */ |
gpmbed | 0:4aa6b3804281 | 37 | ret = i2c.write(DeviceAddr, (const char*)&RegisterAddr, 1, true); |
gpmbed | 0:4aa6b3804281 | 38 | if(!ret) { |
gpmbed | 0:4aa6b3804281 | 39 | /* Read data, with STOP condition */ |
gpmbed | 0:4aa6b3804281 | 40 | ret = i2c.read(DeviceAddr, (char*)pBuffer, NumByteToRead, false); |
gpmbed | 0:4aa6b3804281 | 41 | } |
gpmbed | 0:4aa6b3804281 | 42 | |
gpmbed | 0:4aa6b3804281 | 43 | if(ret) return -1; |
gpmbed | 0:4aa6b3804281 | 44 | return 0; |
gpmbed | 0:4aa6b3804281 | 45 | } |
gpmbed | 0:4aa6b3804281 | 46 | |
gpmbed | 0:4aa6b3804281 | 47 | bool LIS3MDL::init(void) { |
gpmbed | 0:4aa6b3804281 | 48 | |
gpmbed | 0:4aa6b3804281 | 49 | |
gpmbed | 0:4aa6b3804281 | 50 | // Choose device mode (bits 1:0 = 00 = continuous data read, 01 = single conversion, 10 & 11 = default power down) |
gpmbed | 0:4aa6b3804281 | 51 | //writeByte(LIS3MDL_ADDRESS, LIS3MDL_CTRL_REG3, 0x00); // Enable continuous data read mode (bits 1:0 = 00) |
gpmbed | 0:4aa6b3804281 | 52 | i2c_write(LIS3MDL_ADDRESS, LIS3MDL_CTRL_REG3,0x00,1); |
gpmbed | 0:4aa6b3804281 | 53 | i2c_write(LIS3MDL_ADDRESS, LIS3MDL_CTRL_REG5, 0x40,1); // output registers not updated until both data bytes have been read |
gpmbed | 0:4aa6b3804281 | 54 | |
gpmbed | 0:4aa6b3804281 | 55 | |
gpmbed | 0:4aa6b3804281 | 56 | return 1; |
gpmbed | 0:4aa6b3804281 | 57 | } |
gpmbed | 0:4aa6b3804281 | 58 | |
gpmbed | 0:4aa6b3804281 | 59 | bool LIS3MDL::writeRegister(const uint8_t register_addr, const uint8_t value) { |
gpmbed | 1:fe199024ddfe | 60 | |
gpmbed | 0:4aa6b3804281 | 61 | i2c_write(_address,register_addr,value,1); |
gpmbed | 0:4aa6b3804281 | 62 | |
gpmbed | 0:4aa6b3804281 | 63 | return 1; //returns whether the write succeeded or failed |
gpmbed | 0:4aa6b3804281 | 64 | } |
gpmbed | 0:4aa6b3804281 | 65 | |
gpmbed | 0:4aa6b3804281 | 66 | uint8_t LIS3MDL::readRegister(const uint8_t register_addr) |
gpmbed | 0:4aa6b3804281 | 67 | { |
gpmbed | 0:4aa6b3804281 | 68 | uint8_t reg_val = 0; |
gpmbed | 0:4aa6b3804281 | 69 | i2c_read(_address, register_addr, ®_val, 1); |
gpmbed | 0:4aa6b3804281 | 70 | |
gpmbed | 0:4aa6b3804281 | 71 | return reg_val; |
gpmbed | 0:4aa6b3804281 | 72 | } |
gpmbed | 0:4aa6b3804281 | 73 | |
gpmbed | 0:4aa6b3804281 | 74 | uint16_t LIS3MDL::readRegisters(const uint8_t msb_register, const uint8_t lsb_register) { |
gpmbed | 0:4aa6b3804281 | 75 | uint8_t msb = readRegister(msb_register); |
gpmbed | 0:4aa6b3804281 | 76 | uint8_t lsb = readRegister(lsb_register); |
gpmbed | 0:4aa6b3804281 | 77 | return (((int16_t)msb) << 8) | lsb; |
gpmbed | 0:4aa6b3804281 | 78 | } |
gpmbed | 0:4aa6b3804281 | 79 | |
gpmbed | 0:4aa6b3804281 | 80 | bool LIS3MDL::writeMaskedRegister(const uint8_t register_addr, const uint8_t mask, const bool value) { |
gpmbed | 0:4aa6b3804281 | 81 | uint8_t data = readRegister(register_addr); |
gpmbed | 0:4aa6b3804281 | 82 | uint8_t combo; |
gpmbed | 0:4aa6b3804281 | 83 | if(value) { |
gpmbed | 0:4aa6b3804281 | 84 | combo = (mask | data); |
gpmbed | 0:4aa6b3804281 | 85 | } else { |
gpmbed | 0:4aa6b3804281 | 86 | combo = ((~mask) & data); |
gpmbed | 0:4aa6b3804281 | 87 | } |
gpmbed | 0:4aa6b3804281 | 88 | return writeRegister(register_addr, combo); |
gpmbed | 0:4aa6b3804281 | 89 | } |
gpmbed | 0:4aa6b3804281 | 90 | |
gpmbed | 0:4aa6b3804281 | 91 | bool LIS3MDL::writeMaskedRegister(const int register_addr, const int mask, const int value) { |
gpmbed | 0:4aa6b3804281 | 92 | uint8_t data = readRegister(register_addr); |
gpmbed | 0:4aa6b3804281 | 93 | uint8_t masked_value = (data | (mask & value)); //Not sure if right... |
gpmbed | 0:4aa6b3804281 | 94 | return writeRegister(register_addr, masked_value); |
gpmbed | 0:4aa6b3804281 | 95 | } |
gpmbed | 0:4aa6b3804281 | 96 | |
gpmbed | 0:4aa6b3804281 | 97 | uint8_t LIS3MDL::readMaskedRegister(const uint8_t register_addr, const uint8_t mask) { |
gpmbed | 0:4aa6b3804281 | 98 | uint8_t data = readRegister(register_addr); |
gpmbed | 0:4aa6b3804281 | 99 | return (data & mask); |
gpmbed | 0:4aa6b3804281 | 100 | } |
gpmbed | 0:4aa6b3804281 | 101 | |
gpmbed | 0:4aa6b3804281 | 102 | |
gpmbed | 0:4aa6b3804281 | 103 | /** Read the X axis registers |
gpmbed | 0:4aa6b3804281 | 104 | * @see LIS3MDL_OUT_X_H |
gpmbed | 0:4aa6b3804281 | 105 | * @see LIS3MDL_OUT_X_L |
gpmbed | 0:4aa6b3804281 | 106 | */ |
gpmbed | 0:4aa6b3804281 | 107 | int16_t LIS3MDL::getAxisX(void) { |
gpmbed | 0:4aa6b3804281 | 108 | return readRegisters(LIS3MDL_OUT_X_H, LIS3MDL_OUT_X_L); |
gpmbed | 0:4aa6b3804281 | 109 | } |
gpmbed | 0:4aa6b3804281 | 110 | |
gpmbed | 0:4aa6b3804281 | 111 | float LIS3MDL::getAxisX_mag() |
gpmbed | 0:4aa6b3804281 | 112 | { |
gpmbed | 0:4aa6b3804281 | 113 | float res = 0; |
gpmbed | 0:4aa6b3804281 | 114 | float factor = 6842; |
gpmbed | 0:4aa6b3804281 | 115 | res = (int16_t)getAxisX(); |
gpmbed | 0:4aa6b3804281 | 116 | res = res / factor; |
gpmbed | 0:4aa6b3804281 | 117 | |
gpmbed | 0:4aa6b3804281 | 118 | return res; |
gpmbed | 0:4aa6b3804281 | 119 | } |
gpmbed | 0:4aa6b3804281 | 120 | |
gpmbed | 0:4aa6b3804281 | 121 | |
gpmbed | 0:4aa6b3804281 | 122 | /** Read the Y axis registers |
gpmbed | 0:4aa6b3804281 | 123 | * @see LIS3MDL_OUT_Y_H |
gpmbed | 0:4aa6b3804281 | 124 | * @see LIS3MDL_OUT_Y_L |
gpmbed | 0:4aa6b3804281 | 125 | */ |
gpmbed | 0:4aa6b3804281 | 126 | int16_t LIS3MDL::getAxisY(void) { |
gpmbed | 0:4aa6b3804281 | 127 | return readRegisters(LIS3MDL_OUT_Y_H, LIS3MDL_OUT_Y_L); |
gpmbed | 0:4aa6b3804281 | 128 | } |
gpmbed | 0:4aa6b3804281 | 129 | |
gpmbed | 0:4aa6b3804281 | 130 | float LIS3MDL::getAxisY_mag() |
gpmbed | 0:4aa6b3804281 | 131 | { |
gpmbed | 0:4aa6b3804281 | 132 | float res = 0; |
gpmbed | 0:4aa6b3804281 | 133 | float factor = 6842; |
gpmbed | 0:4aa6b3804281 | 134 | res = (int16_t)getAxisY(); |
gpmbed | 0:4aa6b3804281 | 135 | res = res / factor; |
gpmbed | 0:4aa6b3804281 | 136 | |
gpmbed | 0:4aa6b3804281 | 137 | return res; |
gpmbed | 0:4aa6b3804281 | 138 | } |
gpmbed | 0:4aa6b3804281 | 139 | |
gpmbed | 0:4aa6b3804281 | 140 | /** Read the Z axis registers |
gpmbed | 0:4aa6b3804281 | 141 | * @see LIS3MDL_OUT_Z_H |
gpmbed | 0:4aa6b3804281 | 142 | * @see LIS3MDL_OUT_Z_L |
gpmbed | 0:4aa6b3804281 | 143 | */ |
gpmbed | 0:4aa6b3804281 | 144 | int16_t LIS3MDL::getAxisZ(void) { |
gpmbed | 0:4aa6b3804281 | 145 | return readRegisters(LIS3MDL_OUT_Z_H, LIS3MDL_OUT_Z_L); |
gpmbed | 0:4aa6b3804281 | 146 | } |
gpmbed | 0:4aa6b3804281 | 147 | |
gpmbed | 0:4aa6b3804281 | 148 | int16_t LIS3MDL::getTemperature(void) { |
gpmbed | 0:4aa6b3804281 | 149 | return readRegisters(LIS3MDL_TEMP_OUT_H,LIS3MDL_TEMP_OUT_L); |
gpmbed | 0:4aa6b3804281 | 150 | } |
gpmbed | 0:4aa6b3804281 | 151 | |
gpmbed | 0:4aa6b3804281 | 152 | |
gpmbed | 0:4aa6b3804281 | 153 | float LIS3MDL::getHeading() |
gpmbed | 0:4aa6b3804281 | 154 | { |
gpmbed | 0:4aa6b3804281 | 155 | float xH = getAxisX_mag(); |
gpmbed | 0:4aa6b3804281 | 156 | float yH = getAxisY_mag(); |
gpmbed | 0:4aa6b3804281 | 157 | //float xH = (float)mag_axes[0]; //milliGauss |
gpmbed | 0:4aa6b3804281 | 158 | //float yH = (float)mag_axes[1]; //milliGauss |
gpmbed | 0:4aa6b3804281 | 159 | xH *=1000; //milliGauss |
gpmbed | 0:4aa6b3804281 | 160 | yH *=1000; //milliGauss |
gpmbed | 0:4aa6b3804281 | 161 | |
gpmbed | 0:4aa6b3804281 | 162 | xH = xH - 190; //taratura manuale |
gpmbed | 0:4aa6b3804281 | 163 | yH = yH + 550; |
gpmbed | 0:4aa6b3804281 | 164 | |
gpmbed | 0:4aa6b3804281 | 165 | float heading = atan(yH/xH)*LIS3MDL_Rad2Degree; |
gpmbed | 0:4aa6b3804281 | 166 | |
gpmbed | 0:4aa6b3804281 | 167 | |
gpmbed | 0:4aa6b3804281 | 168 | if(xH<0){heading=180-heading;} |
gpmbed | 0:4aa6b3804281 | 169 | else if(xH>0 && yH<0){heading=-heading;} |
gpmbed | 0:4aa6b3804281 | 170 | else if(xH>0 && yH>0){heading=360-heading; } |
gpmbed | 0:4aa6b3804281 | 171 | else if(xH==0 && yH<0){heading=90; } |
gpmbed | 0:4aa6b3804281 | 172 | else if(xH==0 && yH>0){heading=270; } |
gpmbed | 0:4aa6b3804281 | 173 | |
gpmbed | 0:4aa6b3804281 | 174 | heading = (360-heading)+180; |
gpmbed | 0:4aa6b3804281 | 175 | |
gpmbed | 0:4aa6b3804281 | 176 | if(heading<0){ heading +=360; } |
gpmbed | 0:4aa6b3804281 | 177 | else if(heading>360){ heading -=360; } |
gpmbed | 0:4aa6b3804281 | 178 | |
gpmbed | 0:4aa6b3804281 | 179 | |
gpmbed | 0:4aa6b3804281 | 180 | return heading; |
gpmbed | 0:4aa6b3804281 | 181 | } |
gpmbed | 0:4aa6b3804281 | 182 | |
gpmbed | 0:4aa6b3804281 | 183 | |
gpmbed | 1:fe199024ddfe | 184 | bool LIS3MDL::whoAmI(void) { |
gpmbed | 1:fe199024ddfe | 185 | return (LIS3MDL_I_AM_MASK == readRegister(LIS3MDL_WHO_AM_I)); |
gpmbed | 0:4aa6b3804281 | 186 | } |
gpmbed | 0:4aa6b3804281 | 187 | |
gpmbed | 0:4aa6b3804281 | 188 | |
gpmbed | 0:4aa6b3804281 | 189 | |
gpmbed | 0:4aa6b3804281 | 190 | |
gpmbed | 0:4aa6b3804281 | 191 | |
gpmbed | 0:4aa6b3804281 | 192 |