Gian Paolo Donnarumma / LIS3MDL

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?

UserRevisionLine numberNew 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, &reg_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