155 lines
3.5 KiB
C++
155 lines
3.5 KiB
C++
#ifndef SENS_MPU6050
|
|
#define SENS_MPU6050
|
|
|
|
#include "../../io/SoftI2C.h"
|
|
#include "../../Debug.h"
|
|
|
|
/**
|
|
* accelereomter/gyroscope
|
|
* https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf
|
|
* https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf
|
|
* https://github.com/kriswiner/MPU6050/blob/master/MPU6050BasicExample.ino
|
|
*/
|
|
class MPU6050 {
|
|
|
|
private:
|
|
|
|
static constexpr const char* NAME = "MPU6050";
|
|
|
|
static constexpr uint8_t ADDR = 0b1101000 ;
|
|
|
|
static constexpr uint8_t REG_CFG_GYRO = 0x1B;
|
|
static constexpr uint8_t REG_CFG_ACC = 0x1C;
|
|
static constexpr uint8_t REG_ACCEL_XOUT_H = 0x3B;
|
|
|
|
static constexpr uint8_t REG_SMPLRT_DIV = 0x19;
|
|
static constexpr uint8_t REG_CONFIG = 0x1A;
|
|
|
|
static constexpr uint8_t REG_PWR_MGMT_1 = 0x6B;
|
|
|
|
bool inited = false;
|
|
|
|
|
|
private:
|
|
|
|
void init() {
|
|
|
|
debugMod(NAME, "init()");
|
|
|
|
// set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
|
|
writeRegister(REG_PWR_MGMT_1, 0b001); // table on page 40
|
|
|
|
// Configure Gyro and Accelerometer
|
|
// Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively;
|
|
// DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both
|
|
//writeRegister(REG_CONFIG, 0x03); // table on page 13
|
|
writeRegister(REG_CONFIG, 0x00); // table on page 13
|
|
|
|
// Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
|
|
writeRegister(REG_SMPLRT_DIV, 0x04); // Use a 200 Hz sample rate
|
|
|
|
// configure gyro as +/- 4G -> +32767 = +4G, -32768 = -4G
|
|
writeRegister(REG_CFG_ACC, 0b01 << 3); // table on page 15
|
|
#define oneGto1000(x) ((int)x)*1000/(32768/4)
|
|
}
|
|
|
|
bool writeRegister(const uint8_t addr, const uint8_t val) {
|
|
|
|
bool ok;
|
|
|
|
// address the slave in write mode and select the first register to read
|
|
ok = i2c::startWrite(ADDR);
|
|
if (!ok) {os_printf("failed start write\n"); return false;}
|
|
ok = i2c::writeByteAndCheck(addr);
|
|
if (!ok) {os_printf("failed to select register %d\n", addr); return false;}
|
|
ok = i2c::writeBytesAndCheck(&val, 1);
|
|
if (!ok) {os_printf("failed to write register contents \n"); return false;}
|
|
|
|
// done
|
|
i2c::stop();
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
public:
|
|
|
|
struct Acc {
|
|
int16_t x;
|
|
int16_t y;
|
|
int16_t z;
|
|
};
|
|
|
|
struct Gyro {
|
|
int16_t x;
|
|
int16_t y;
|
|
int16_t z;
|
|
};
|
|
|
|
struct Res {
|
|
Acc acc;
|
|
int16_t temp;
|
|
Gyro gyro;
|
|
};
|
|
|
|
void initOnce() {
|
|
if (inited) {return;}
|
|
init();
|
|
inited = true;
|
|
}
|
|
|
|
|
|
bool isPresent() {
|
|
return i2c::query(ADDR);
|
|
}
|
|
|
|
bool query(Res& res) {
|
|
|
|
bool ok;
|
|
|
|
// select register(s) to read
|
|
//i2c::start();
|
|
ok = i2c::startWrite(ADDR);
|
|
if (!ok) {os_printf("failed start write1\n"); return false;}
|
|
ok = i2c::writeByteAndCheck(REG_ACCEL_XOUT_H);
|
|
if (!ok) {os_printf("failed select register\n"); return false;}
|
|
i2c::stop();
|
|
|
|
// read registers
|
|
uint8_t buf[14];
|
|
//i2c::start();
|
|
ok = i2c::startRead(ADDR);
|
|
if (!ok) {os_printf("failed start write2\n"); return false;}
|
|
i2c::readBytes(buf, 14);
|
|
i2c::stop();
|
|
|
|
res.acc.x = (buf[0] << 8) | buf[1];
|
|
res.acc.y = (buf[2] << 8) | buf[3];
|
|
res.acc.z = (buf[4] << 8) | buf[5];
|
|
|
|
res.temp = (buf[6] << 8) | buf[7];
|
|
|
|
res.gyro.x = (buf[8] << 8) | buf[9];
|
|
res.gyro.y = (buf[10] << 8) | buf[11];
|
|
res.gyro.z = (buf[12] << 8) | buf[13];
|
|
|
|
res.acc.x = oneGto1000(res.acc.x);
|
|
res.acc.y = oneGto1000(res.acc.y);
|
|
res.acc.z = oneGto1000(res.acc.z);
|
|
|
|
|
|
//os_printf("Acc (%d, %d, %d)\n", res.acc.x, res.acc.y, res.acc.z);
|
|
//os_printf("Gyro (%d, %d, %d)\n", res.gyro.x, res.gyro.y, res.gyro.z);
|
|
//os_printf("Temp (%d)\n", res.temp);
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif
|