75 lines
1.4 KiB
C++
75 lines
1.4 KiB
C++
#pragma once
|
|
|
|
|
|
#include "../../Platforms.h"
|
|
#include "../../Debug.h"
|
|
|
|
/**
|
|
* CMPS10 3-axis magnetometer/accelerometer module
|
|
* https://www.robot-electronics.co.uk/htm/cmps10i2c.htm
|
|
*/
|
|
template <typename I2C> class CMPS10 {
|
|
|
|
I2C& i2c;
|
|
|
|
static constexpr const char* NAME = "CMPS10";
|
|
|
|
static constexpr uint8_t ADDR7 = 0xC0>>1;
|
|
|
|
static constexpr uint8_t REG_MAG_X = 10; //10+11;
|
|
static constexpr uint8_t REG_MAG_Y = 12; //12+13;
|
|
static constexpr uint8_t REG_MAG_Z = 14; //14+15;
|
|
|
|
static constexpr uint8_t REG_ACC_X = 16; //16+17;
|
|
static constexpr uint8_t REG_ACC_Y = 18; //18+19;
|
|
static constexpr uint8_t REG_ACC_Z = 20; //20+21;
|
|
|
|
|
|
public:
|
|
|
|
struct Magnetometer {
|
|
int16_t x;
|
|
int16_t y;
|
|
int16_t z;
|
|
};
|
|
|
|
struct Acceleromter {
|
|
int16_t x;
|
|
int16_t y;
|
|
int16_t z;
|
|
};
|
|
|
|
public:
|
|
|
|
CMPS10(I2C& i2c, uint8_t addrOffset = 0) : i2c(i2c){
|
|
|
|
}
|
|
|
|
|
|
bool isPresent() {
|
|
return i2c.query(ADDR7);
|
|
}
|
|
|
|
Magnetometer getMagnetometer() {
|
|
uint8_t res[6];
|
|
i2c.readReg(ADDR7, REG_MAG_X, 6, res);
|
|
Magnetometer mag;
|
|
mag.x = ((res[0] << 8) | (res[1] << 0));
|
|
mag.y = ((res[2] << 8) | (res[3] << 0));
|
|
mag.z = ((res[4] << 8) | (res[5] << 0));
|
|
return mag;
|
|
}
|
|
|
|
Acceleromter getAcceleromter() {
|
|
uint8_t res[6];
|
|
i2c.readReg(ADDR7, REG_ACC_X, 6, res);
|
|
Acceleromter acc;
|
|
acc.x = ((res[0] << 8) | (res[1] << 0));
|
|
acc.y = ((res[2] << 8) | (res[3] << 0));
|
|
acc.z = ((res[4] << 8) | (res[5] << 0));
|
|
return acc;
|
|
}
|
|
|
|
};
|
|
|