124 lines
2.3 KiB
C++
124 lines
2.3 KiB
C++
#pragma once
|
|
|
|
|
|
#include "../../Platforms.h"
|
|
#include "../../Debug.h"
|
|
|
|
/**
|
|
* LIS3MDL 3-axis magnetometer module
|
|
* https://www.st.com/resource/en/datasheet/lis3mdl.pdf
|
|
*/
|
|
template <typename I2C> class LIS3MDL {
|
|
|
|
I2C& i2c;
|
|
|
|
static constexpr const char* NAME = "LIS3MDL";
|
|
|
|
static constexpr uint8_t ADDR7 = 0b0011100;
|
|
|
|
static constexpr uint8_t CTRL_REG1 = 0x20;
|
|
static constexpr uint8_t CTRL_REG2 = 0x21;
|
|
static constexpr uint8_t CTRL_REG3 = 0x22;
|
|
static constexpr uint8_t CTRL_REG4 = 0x23;
|
|
static constexpr uint8_t CTRL_REG5 = 0x24;
|
|
|
|
static constexpr uint8_t REG_X = 0x28; //0x28(L= + 0x29(H)
|
|
static constexpr uint8_t REG_Y = 0x2A; //0x2A(L) + 0x2B(H)
|
|
static constexpr uint8_t REG_Z = 0x2C; //0x2C(L) + 0x2D(H)
|
|
|
|
|
|
|
|
public:
|
|
|
|
struct Magnetometer {
|
|
int16_t x;
|
|
int16_t y;
|
|
int16_t z;
|
|
};
|
|
|
|
struct Acceleromter {
|
|
int16_t x;
|
|
int16_t y;
|
|
int16_t z;
|
|
};
|
|
|
|
enum class Resolution : uint8_t {
|
|
GAUSS_4 = 0b00,
|
|
GAUSS_8 = 0b01,
|
|
GAUSS_12 = 0b10,
|
|
GAUSS_16 = 0b11,
|
|
};
|
|
|
|
enum class AxisMode : uint8_t {
|
|
LOW_POWER = 0b00,
|
|
MEDIUM_PERFORMANCE = 0b01,
|
|
HIGH_PERFORMANCE = 0b10,
|
|
ULTRA_HIGH_PERFORMANCE = 0b11,
|
|
};
|
|
|
|
enum class SamplingRate : uint8_t {
|
|
HZ_0_625,
|
|
HZ_1_25,
|
|
HZ_2_5,
|
|
HZ_5,
|
|
HZ_10,
|
|
HZ_20,
|
|
HZ_40,
|
|
HZ_80,
|
|
};
|
|
|
|
enum class OperationMode : uint8_t {
|
|
CONTINUOUS = 0b00,
|
|
SINGLE = 0b01,
|
|
OFF = 0b11,
|
|
};
|
|
|
|
public:
|
|
|
|
LIS3MDL(I2C& i2c, uint8_t addrOffset = 0) : i2c(i2c){
|
|
|
|
}
|
|
|
|
|
|
bool isPresent() {
|
|
return i2c.query(ADDR7);
|
|
}
|
|
|
|
|
|
|
|
void setResolution(Resolution res) {
|
|
getAndSet(CTRL_REG2, 0b01100000, (uint8_t)res << 5);
|
|
}
|
|
|
|
void setAxisMode(AxisMode mode) {
|
|
getAndSet(CTRL_REG1, 0b01100000, (uint8_t)mode << 5); // x and y
|
|
getAndSet(CTRL_REG4, 0b00001100, (uint8_t)mode << 2); // z
|
|
}
|
|
|
|
void setSamplingRate(SamplingRate rate) {
|
|
getAndSet(CTRL_REG1, 0b00011100, (uint8_t)rate << 2);
|
|
}
|
|
|
|
void setOperationMode(OperationMode mode) {
|
|
getAndSet(CTRL_REG3, 0b00000011, (uint8_t)mode << 0);
|
|
}
|
|
|
|
Magnetometer getMagnetometer() {
|
|
uint8_t res[6];
|
|
i2c.readReg(ADDR7, REG_X, 6, res);
|
|
Magnetometer mag;
|
|
mag.x = ((res[0] << 0) | (res[1] << 8));
|
|
mag.y = ((res[2] << 0) | (res[3] << 8));
|
|
mag.z = ((res[4] << 0) | (res[5] << 8));
|
|
return mag;
|
|
}
|
|
|
|
void getAndSet(uint8_t reg, uint8_t setMask, uint8_t setVal) {
|
|
uint8_t tmp = i2c.readReg8(ADDR7, reg);
|
|
tmp = (tmp & ~setMask) | setVal;
|
|
i2c.writeReg8(ADDR7, reg, tmp);
|
|
}
|
|
|
|
};
|
|
|