318 lines
9.8 KiB
C++
318 lines
9.8 KiB
C++
/*
|
|
* peripherals_imu_icm.hpp
|
|
*
|
|
* Created on: Mar 5, 2021
|
|
* Author: erki
|
|
*/
|
|
|
|
#ifndef SKULLC_PERIPHERALS_IMU_ICM_HPP_
|
|
#define SKULLC_PERIPHERALS_IMU_ICM_HPP_
|
|
|
|
#include <array>
|
|
#include <limits>
|
|
|
|
#include "peripherals_imu.hpp"
|
|
|
|
namespace Peripherals
|
|
{
|
|
|
|
template<typename T, typename HAL>
|
|
class ImuIcm : public IImu
|
|
{
|
|
public:
|
|
using registers_handle = T;
|
|
using hal = HAL;
|
|
|
|
enum class GyroScale : std::uint32_t
|
|
{
|
|
DPS_250 = 0,
|
|
DPS_500,
|
|
DPS_1000,
|
|
DPS_2000
|
|
};
|
|
|
|
enum class AccelerometerScale : std::uint32_t
|
|
{
|
|
G2 = 0,
|
|
G4,
|
|
G8,
|
|
G16
|
|
};
|
|
|
|
registers_handle registers;
|
|
|
|
ImuIcm() = delete;
|
|
ImuIcm(const registers_handle& registers) : registers(registers)
|
|
{
|
|
bias_accel_.fill(0);
|
|
bias_gyro_.fill(0);
|
|
}
|
|
|
|
void setup() override
|
|
{
|
|
registers.writeRegister(Registers_::PWR_MGMT_1 & Registers_::WRITE_MASK,
|
|
0b10000000);
|
|
hal::delay(10);
|
|
|
|
registers.writeRegister(Registers_::USER_CTRL & Registers_::WRITE_MASK,
|
|
0b00010000);
|
|
hal::delay(10);
|
|
|
|
registers.writeRegister(Registers_::PWR_MGMT_1 & Registers_::WRITE_MASK,
|
|
0b00000000);
|
|
hal::delay(10);
|
|
|
|
registers.writeRegister(Registers_::PWR_MGMT_1 & Registers_::WRITE_MASK,
|
|
0b00000001);
|
|
hal::delay(10);
|
|
|
|
registers.writeRegister(Registers_::CONFIG & Registers_::WRITE_MASK,
|
|
0x03);// DLPF_CFG = 3, gyro filter = 41/59.0, gyro
|
|
// rate = 1KHz, temp filter = 42
|
|
hal::delay(10);
|
|
|
|
const std::uint8_t new_gyro_conf = (std::uint32_t(scale_gyro_) << 3);
|
|
registers.writeRegister(Registers_::GYRO_CONFIG & Registers_::WRITE_MASK,
|
|
new_gyro_conf);
|
|
|
|
const std::uint8_t new_accel_config = (std::uint32_t(scale_accel_) << 3);
|
|
registers.writeRegister(Registers_::ACCEL_CONFIG & Registers_::WRITE_MASK,
|
|
new_accel_config);
|
|
|
|
// ACCEL_FCHOICE_B = 0, A_DLPF_CFG = 3 filter=44.8/61.5 rate=1KHz
|
|
registers.writeRegister(Registers_::ACCEL_CONFIG2 & Registers_::WRITE_MASK,
|
|
0x03);
|
|
hal::delay(10);
|
|
|
|
// SAMPLE_RATE = INTERNAL_SAMPLE_RATE / (1 + SMPLRT_DIV) Where
|
|
// INTERNAL_SAMPLE_RATE = 1kHz
|
|
registers.writeRegister(Registers_::SMPLRT_DIV & Registers_::WRITE_MASK, 0);
|
|
hal::delay(10);
|
|
|
|
// Enable interrupt
|
|
|
|
// The logic level for INT/DRDY pin is active high.
|
|
// INT/DRDY pin is configured as push-pull.
|
|
// INT/DRDY pin indicates interrupt pulse's width is 50us.
|
|
// Interrupt status is cleared only by reading INT_STATUS register
|
|
registers.writeRegister(Registers_::INT_PIN_CFG & Registers_::WRITE_MASK,
|
|
0);
|
|
hal::delay(10);
|
|
|
|
registers.writeRegister(Registers_::INT_ENABLE & Registers_::WRITE_MASK, 1);
|
|
hal::delay(10);
|
|
}
|
|
|
|
std::array<std::int16_t, 3>& gyroBias()
|
|
{
|
|
return bias_gyro_;
|
|
}
|
|
|
|
std::array<std::int16_t, 3>& accelBias()
|
|
{
|
|
return bias_accel_;
|
|
}
|
|
|
|
void calibrate(const std::uint32_t samples) override
|
|
{
|
|
std::array<std::int32_t, 3> avg_gyro;
|
|
std::array<std::int32_t, 3> avg_accel;
|
|
|
|
avg_gyro.fill(0);
|
|
avg_accel.fill(0);
|
|
|
|
for (std::uint32_t i = 0; i < samples; i++)
|
|
{
|
|
std::array<std::int16_t, 3> raw;
|
|
|
|
readGyroRaw(raw.data());
|
|
for (std::uint32_t j = 0; j < 3; j++)
|
|
avg_gyro[j] += raw[j];
|
|
|
|
readAccelerometerRaw(raw.data());
|
|
for (std::uint32_t j = 0; j < 3; j++)
|
|
avg_accel[j] += raw[j];
|
|
|
|
hal::delay(2);
|
|
}
|
|
|
|
for (std::uint32_t i = 0; i < 3; i++)
|
|
{
|
|
bias_gyro_[i] = avg_gyro[i] / std::int32_t(samples);
|
|
bias_accel_[i] = avg_accel[i] / std::int32_t(samples);
|
|
}
|
|
|
|
bias_accel_[2] -= accelerometerReadingToRaw(1);
|
|
}
|
|
|
|
void setGyroscopeScale(const GyroScale scale)
|
|
{
|
|
const std::uint8_t current_config =
|
|
registers.readRegister(Registers_::GYRO_CONFIG | Registers_::READ_MASK);
|
|
const std::uint8_t new_config =
|
|
(current_config & 0xE7) | (std::uint8_t(scale) << 3);
|
|
registers.writeRegister(Registers_::GYRO_CONFIG & Registers_::WRITE_MASK,
|
|
new_config);
|
|
|
|
scale_gyro_ = scale;
|
|
}
|
|
|
|
void setAccelerometerScale(const AccelerometerScale scale)
|
|
{
|
|
const std::uint8_t current_config = registers.readRegister(
|
|
Registers_::ACCEL_CONFIG | Registers_::READ_MASK);
|
|
const std::uint8_t new_config =
|
|
(current_config & 0xE7) | (std::uint8_t(scale) << 3);
|
|
registers.writeRegister(Registers_::ACCEL_CONFIG & Registers_::WRITE_MASK,
|
|
new_config);
|
|
|
|
scale_accel_ = scale;
|
|
}
|
|
|
|
void readGyro(float* output) override
|
|
{
|
|
uint8_t data[6] = {0};
|
|
registers.readRegisterMultibyte(
|
|
Registers_::GYRO_XOUT_H | Registers_::READ_MASK, data, 6);
|
|
|
|
output[0] = gyroRawToReading(std::int16_t((data[0] << 8) | data[1]), 0);
|
|
output[1] = gyroRawToReading(std::int16_t((data[2] << 8) | data[3]), 1);
|
|
output[2] = gyroRawToReading(std::int16_t((data[4] << 8) | data[5]), 2);
|
|
}
|
|
|
|
void readGyroRaw(std::int16_t* output) override
|
|
{
|
|
uint8_t data[6] = {0};
|
|
registers.readRegisterMultibyte(
|
|
Registers_::GYRO_XOUT_H | Registers_::READ_MASK, data, 6);
|
|
|
|
output[0] = std::int16_t((data[0] << 8) | data[1]);
|
|
output[1] = std::int16_t((data[2] << 8) | data[3]);
|
|
output[2] = std::int16_t((data[4] << 8) | data[5]);
|
|
}
|
|
|
|
void readAccelerometer(float* output) override
|
|
{
|
|
uint8_t data[6] = {0};
|
|
registers.readRegisterMultibyte(
|
|
Registers_::ACCEL_XOUT_H | Registers_::READ_MASK, data, 6);
|
|
|
|
output[0] = accelerometerRawToReading(std::int16_t((data[0] << 8) | data[1]), 0);
|
|
output[1] = accelerometerRawToReading(std::int16_t((data[2] << 8) | data[3]), 1);
|
|
output[2] = accelerometerRawToReading(std::int16_t((data[4] << 8) | data[5]), 2);
|
|
}
|
|
|
|
void readAccelerometerRaw(std::int16_t* output) override
|
|
{
|
|
uint8_t data[6] = {0};
|
|
registers.readRegisterMultibyte(
|
|
Registers_::ACCEL_XOUT_H | Registers_::READ_MASK, data, 6);
|
|
|
|
|
|
output[0] = std::int16_t((data[0] << 8) | data[1]);
|
|
output[1] = std::int16_t((data[2] << 8) | data[3]);
|
|
output[2] = std::int16_t((data[4] << 8) | data[5]);
|
|
}
|
|
|
|
std::int16_t accelerometerReadingToRaw(const float& fs) const
|
|
{
|
|
return fs / accel_fs_to_bit_constants_[std::uint32_t(scale_accel_)];
|
|
}
|
|
|
|
std::int16_t gyroReadingToRaw(const float& fs) const
|
|
{
|
|
return fs / gyro_fs_to_bit_constants_[std::uint32_t(scale_gyro_)];
|
|
}
|
|
|
|
float accelerometerRawToReading(const std::int16_t bit, const std::size_t axis) const
|
|
{
|
|
return (float(bit - bias_accel_[axis]) * accel_fs_to_bit_constants_[std::uint32_t(scale_accel_)]) * 9.8f;
|
|
}
|
|
|
|
float gyroRawToReading(const std::int16_t bit, const std::size_t axis) const
|
|
{
|
|
return float(bit - bias_gyro_[axis]) * gyro_fs_to_bit_constants_[std::uint32_t(scale_gyro_)];
|
|
}
|
|
|
|
std::uint8_t readProductId()
|
|
{
|
|
return registers.readRegister(Registers_::WHO_AM_I | Registers_::READ_MASK);
|
|
}
|
|
|
|
private:
|
|
GyroScale scale_gyro_ = GyroScale::DPS_2000;
|
|
AccelerometerScale scale_accel_ = AccelerometerScale::G16;
|
|
|
|
static constexpr float accel_fs_to_bit_constants_[4] = {
|
|
(2.0f / 32768.0f), (4.0f / 32768.0f), (8.0f / 32768.0f),
|
|
(16.0f / 32768.0f)};
|
|
|
|
static constexpr float gyro_fs_to_bit_constants_[4] = {
|
|
(250.0f / 32768.0f), (500.0f / 32768.0f), (1000.0f / 32768.0f),
|
|
(2000.0f / 32768.0f)};
|
|
|
|
std::array<std::int16_t, 3> bias_gyro_;
|
|
std::array<std::int16_t, 3> bias_accel_;
|
|
|
|
struct Registers_
|
|
{
|
|
static constexpr std::uint32_t ICM20689_ID = 0x98;
|
|
|
|
static constexpr std::uint8_t READ_MASK = 0x80;
|
|
static constexpr std::uint8_t WRITE_MASK = 0x7F;
|
|
|
|
static constexpr std::uint8_t SMPLRT_DIV = 0x19;
|
|
static constexpr std::uint8_t CONFIG = 0x1A;
|
|
static constexpr std::uint8_t GYRO_CONFIG = 0x1B;
|
|
static constexpr std::uint8_t ACCEL_CONFIG = 0x1C;
|
|
static constexpr std::uint8_t ACCEL_CONFIG2 = 0x1D;
|
|
|
|
static constexpr std::uint8_t INT_PIN_CFG = 0x37;
|
|
static constexpr std::uint8_t INT_ENABLE = 0x38;
|
|
static constexpr std::uint8_t INT_STATUS = 0x3A;
|
|
|
|
static constexpr std::uint8_t GYRO_XOUT_H = 0x43;
|
|
static constexpr std::uint8_t GYRO_XOUT_L = 0x44;
|
|
static constexpr std::uint8_t GYRO_YOUT_H = 0x45;
|
|
static constexpr std::uint8_t GYRO_YOUT_L = 0x46;
|
|
static constexpr std::uint8_t GYRO_ZOUT_H = 0x47;
|
|
static constexpr std::uint8_t GYRO_ZOUT_L = 0x48;
|
|
|
|
static constexpr std::uint8_t ACCEL_XOUT_H = 0x3B;
|
|
static constexpr std::uint8_t ACCEL_XOUT_L = 0x3C;
|
|
static constexpr std::uint8_t ACCEL_YOUT_H = 0x3D;
|
|
static constexpr std::uint8_t ACCEL_YOUT_L = 0x3E;
|
|
static constexpr std::uint8_t ACCEL_ZOUT_H = 0x3F;
|
|
static constexpr std::uint8_t ACCEL_ZOUT_L = 0x40;
|
|
|
|
static constexpr std::uint8_t USER_CTRL = 0x6A;
|
|
static constexpr std::uint8_t PWR_MGMT_1 = 0x6B;
|
|
static constexpr std::uint8_t PWR_MGMT_2 = 0x6C;
|
|
static constexpr std::uint8_t WHO_AM_I = 0x75;
|
|
|
|
static constexpr std::uint8_t XG_OFFSET_H = 0x13;
|
|
static constexpr std::uint8_t XG_OFFSET_L = 0x14;
|
|
static constexpr std::uint8_t YG_OFFSET_H = 0x15;
|
|
static constexpr std::uint8_t YG_OFFSET_L = 0x16;
|
|
static constexpr std::uint8_t ZG_OFFSET_H = 0x17;
|
|
static constexpr std::uint8_t ZG_OFFSET_L = 0x18;
|
|
|
|
static constexpr std::uint8_t XA_OFFSET_H = 0x77;
|
|
static constexpr std::uint8_t XA_OFFSET_L = 0x78;
|
|
static constexpr std::uint8_t YA_OFFSET_H = 0x7A;
|
|
static constexpr std::uint8_t YA_OFFSET_L = 0x7B;
|
|
static constexpr std::uint8_t ZA_OFFSET_H = 0x7D;
|
|
static constexpr std::uint8_t ZA_OFFSET_L = 0x7E;
|
|
};
|
|
};
|
|
|
|
template<typename T, typename HAL>
|
|
constexpr float ImuIcm<T, HAL>::accel_fs_to_bit_constants_[4];
|
|
|
|
template<typename T, typename HAL>
|
|
constexpr float ImuIcm<T, HAL>::gyro_fs_to_bit_constants_[4];
|
|
|
|
}// namespace Peripherals
|
|
|
|
#endif /* SKULLC_PERIPHERALS_IMU_ICM_HPP_ */
|