182 lines
4.4 KiB
C++
182 lines
4.4 KiB
C++
#pragma once
|
|
|
|
// Datasheet: https://www.pololu.com/file/0J1087/LSM6DS33.pdf
|
|
|
|
#include <Arduino.h>
|
|
|
|
class LSM6
|
|
{
|
|
public:
|
|
template <typename T> struct vector
|
|
{
|
|
T x, y, z;
|
|
};
|
|
|
|
enum deviceType { device_DS33, device_auto };
|
|
enum sa0State { sa0_low, sa0_high, sa0_auto };
|
|
|
|
// full-scale factors
|
|
enum ACC_FS {ACC_FS2, ACC_FS4, ACC_FS8, ACC_FS16};
|
|
enum GYRO_FS {GYRO_FS245, GYRO_FS500, GYRO_FS1000, GYRO_FS2000};
|
|
|
|
// output data rate options
|
|
enum ODR
|
|
{
|
|
ODR13 = 0x1,
|
|
ODR26 = 0x2,
|
|
ODR52 = 0x3,
|
|
ODR104 = 0x4,
|
|
ODR208 = 0x5,
|
|
ODR416 = 0x6,
|
|
ODR833 = 0x7,
|
|
ODR166k = 0x8
|
|
};
|
|
|
|
// register addresses
|
|
enum regAddr
|
|
{
|
|
FUNC_CFG_ACCESS = 0x01,
|
|
|
|
FIFO_CTRL1 = 0x06,
|
|
FIFO_CTRL2 = 0x07,
|
|
FIFO_CTRL3 = 0x08,
|
|
FIFO_CTRL4 = 0x09,
|
|
FIFO_CTRL5 = 0x0A,
|
|
ORIENT_CFG_G = 0x0B,
|
|
|
|
INT1_CTRL = 0x0D,
|
|
INT2_CTRL = 0x0E,
|
|
WHO_AM_I = 0x0F,
|
|
CTRL1_XL = 0x10,
|
|
CTRL2_G = 0x11,
|
|
CTRL3_C = 0x12,
|
|
CTRL4_C = 0x13,
|
|
CTRL5_C = 0x14,
|
|
CTRL6_C = 0x15,
|
|
CTRL7_G = 0x16,
|
|
CTRL8_XL = 0x17,
|
|
CTRL9_XL = 0x18,
|
|
CTRL10_C = 0x19,
|
|
|
|
WAKE_UP_SRC = 0x1B,
|
|
TAP_SRC = 0x1C,
|
|
D6D_SRC = 0x1D,
|
|
STATUS_REG = 0x1E,
|
|
|
|
OUT_TEMP_L = 0x20,
|
|
OUT_TEMP_H = 0x21,
|
|
OUTX_L_G = 0x22,
|
|
OUTX_H_G = 0x23,
|
|
OUTY_L_G = 0x24,
|
|
OUTY_H_G = 0x25,
|
|
OUTZ_L_G = 0x26,
|
|
OUTZ_H_G = 0x27,
|
|
OUTX_L_XL = 0x28,
|
|
OUTX_H_XL = 0x29,
|
|
OUTY_L_XL = 0x2A,
|
|
OUTY_H_XL = 0x2B,
|
|
OUTZ_L_XL = 0x2C,
|
|
OUTZ_H_XL = 0x2D,
|
|
|
|
FIFO_STATUS1 = 0x3A,
|
|
FIFO_STATUS2 = 0x3B,
|
|
FIFO_STATUS3 = 0x3C,
|
|
FIFO_STATUS4 = 0x3D,
|
|
FIFO_DATA_OUT_L = 0x3E,
|
|
FIFO_DATA_OUT_H = 0x3F,
|
|
TIMESTAMP0_REG = 0x40,
|
|
TIMESTAMP1_REG = 0x41,
|
|
TIMESTAMP2_REG = 0x42,
|
|
|
|
STEP_TIMESTAMP_L = 0x49,
|
|
STEP_TIMESTAMP_H = 0x4A,
|
|
STEP_COUNTER_L = 0x4B,
|
|
STEP_COUNTER_H = 0x4C,
|
|
|
|
FUNC_SRC = 0x53,
|
|
|
|
TAP_CFG = 0x58,
|
|
TAP_THS_6D = 0x59,
|
|
INT_DUR2 = 0x5A,
|
|
WAKE_UP_THS = 0x5B,
|
|
WAKE_UP_DUR = 0x5C,
|
|
FREE_FALL = 0x5D,
|
|
MD1_CFG = 0x5E,
|
|
MD2_CFG = 0x5F,
|
|
};
|
|
|
|
vector<int16_t> a; // accelerometer readings
|
|
vector<int16_t> g; // gyro readings
|
|
|
|
const float SIGMA = 0.95; // for updating bias
|
|
|
|
public:
|
|
LSM6(void);
|
|
|
|
bool init(deviceType device = device_auto, sa0State sa0 = sa0_auto);
|
|
deviceType getDeviceType(void) { return _device; }
|
|
|
|
void enableDefault(void);
|
|
|
|
void writeReg(uint8_t reg, uint8_t value);
|
|
uint8_t readReg(uint8_t reg);
|
|
|
|
void readAcc(void);
|
|
void readGyro(void);
|
|
void read(void);
|
|
|
|
void setFullScaleGyro(GYRO_FS gfs);
|
|
void setFullScaleAcc(ACC_FS afs);
|
|
|
|
void setGyroDataOutputRate(ODR);
|
|
void setAccDataOutputRate(ODR);
|
|
|
|
void setTimeout(uint16_t timeout);
|
|
uint16_t getTimeout(void);
|
|
bool timeoutOccurred(void);
|
|
|
|
uint8_t getStatus(void) {return readReg(LSM6::STATUS_REG);}
|
|
|
|
bool checkForNewData(void);
|
|
|
|
vector<float> updateGyroBias(void)
|
|
{
|
|
if (initialMeasurement) {
|
|
gyroBias.z = g.z;
|
|
initialMeasurement = false;
|
|
}
|
|
float retain = (0.98);
|
|
gyroBias.z = g.z * (1- retain) + gyroBias.z * retain;
|
|
/**
|
|
* TODO: add a low-pass filter to update the bias
|
|
*/
|
|
|
|
return gyroBias;
|
|
}
|
|
|
|
protected:
|
|
deviceType _device; // chip type
|
|
uint8_t address;
|
|
|
|
uint16_t io_timeout;
|
|
bool did_timeout;
|
|
|
|
int16_t testReg(uint8_t address, regAddr reg);
|
|
|
|
//conversion factors are set when you change ODR or FS
|
|
float mdpsPerLSB = 0;
|
|
float mgPerLSB = 0;
|
|
float accODR = 0; // Hz
|
|
float gyroODR = 0; // Hz
|
|
|
|
uint8_t last_status; // status of last I2C transmission
|
|
|
|
// for the complementary filter
|
|
// float estimatedPitchAngle = 0;
|
|
// vector<float> eulerAngles;
|
|
vector<float> gyroBias;
|
|
bool initialMeasurement = true;
|
|
|
|
/* We make Robot a friend to avoid all the setters and getters. */
|
|
friend class Robot;
|
|
};
|