From 6662bbde927572c9ddf5fd48765eca050f59a258 Mon Sep 17 00:00:00 2001 From: Erki Date: Fri, 5 Mar 2021 20:59:28 +0200 Subject: [PATCH] Port IMU drivers to library --- Inc/peripherals_imu.hpp | 42 ++++++ Inc/peripherals_imu_icm.hpp | 80 +++++++++++ Inc/peripherals_spi.hpp | 43 ++++++ Inc/peripherals_utility.hpp | 26 ++++ Src/peripherals_imu_icm.cpp | 274 ++++++++++++++++++++++++++++++++++++ Src/peripherals_spi.cpp | 104 ++++++++++++++ 6 files changed, 569 insertions(+) create mode 100644 Inc/peripherals_imu.hpp create mode 100644 Inc/peripherals_imu_icm.hpp create mode 100644 Inc/peripherals_spi.hpp create mode 100644 Src/peripherals_imu_icm.cpp create mode 100644 Src/peripherals_spi.cpp diff --git a/Inc/peripherals_imu.hpp b/Inc/peripherals_imu.hpp new file mode 100644 index 0000000..059891c --- /dev/null +++ b/Inc/peripherals_imu.hpp @@ -0,0 +1,42 @@ +/* + * peripherals_imu.hpp + * + * Created on: Mar 5, 2021 + * Author: erki + */ + +#ifndef PERIPHERALS_IMU_HPP_ +#define PERIPHERALS_IMU_HPP_ + +#include + +namespace Peripherals +{ + +class IImu +{ +public: + enum class Axis : std::uint32_t + { + X = 0, + Y, + Z + }; + + virtual void Setup() = 0; + virtual void Calibrate(const std::uint32_t samples) = 0; + + virtual void ReadGyro(float* output) = 0; + virtual void ReadGyroRaw(std::int16_t* output) = 0; + + virtual void ReadAccelerometer(float* output) = 0; + virtual void ReadAccelerometerRaw(std::int16_t* output) = 0; + + virtual float ProcessGyroReading(const std::int16_t raw_reading) = 0; + virtual float ProcessAccelerometerReading(const std::int16_t raw_reading) = 0; +}; + +} + + +#endif /* PERIPHERALS_IMU_HPP_ */ diff --git a/Inc/peripherals_imu_icm.hpp b/Inc/peripherals_imu_icm.hpp new file mode 100644 index 0000000..693d36e --- /dev/null +++ b/Inc/peripherals_imu_icm.hpp @@ -0,0 +1,80 @@ +/* + * peripherals_imu_icm.hpp + * + * Created on: Mar 5, 2021 + * Author: erki + */ + +#ifndef PERIPHERALS_IMU_ICM_HPP_ +#define PERIPHERALS_IMU_ICM_HPP_ + +#include + +#include "spi.h" + +#include "peripherals_imu.hpp" +#include "peripherals_spi.hpp" + +namespace Peripherals +{ + +class ImuIcm : public IImu +{ +public: + 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 + }; + + ImuIcm() = delete; + ImuIcm(SPI_HandleTypeDef* spi, const IO& cs); + ImuIcm(const Spi& spi); + +#if USE_HAL_SPI_REGISTER_CALLBACKS == 1U + + void SetSpiRxCallback(pSPI_CallbackTypeDef cb); + void SetSpiTxCallback(pSPI_CallbackTypeDef cb); + +#endif + + void Setup() override; + void Calibrate(const std::uint32_t samples) override; + + void SetGyroscopeScale(const GyroScale scale); + void SetAccelerometerScale(const AccelerometerScale scale); + + void ReadGyro(float* output) override; + void ReadGyroRaw(std::int16_t* output) override; + + void ReadAccelerometer(float* output) override; + void ReadAccelerometerRaw(std::int16_t* output) override; + + std::int16_t AccelerometerReadingToRaw(const float& fs) const; + std::int16_t GyroReadingToRaw(const float& fs) const; + float AccelerometerRawToReading(const std::int16_t bit) const; + float GyroRawToReading(const std::int16_t bit) const; + +private: + Spi _spi; + + GyroScale _scale_gyro = GyroScale::DPS_2000; + AccelerometerScale _scale_accel = AccelerometerScale::G16; + + std::array _bias_gyro; + std::array _bias_accel; +}; + +} + +#endif /* PERIPHERALS_IMU_ICM_HPP_ */ diff --git a/Inc/peripherals_spi.hpp b/Inc/peripherals_spi.hpp new file mode 100644 index 0000000..bc63f50 --- /dev/null +++ b/Inc/peripherals_spi.hpp @@ -0,0 +1,43 @@ +/* + * peripherals_spi.hpp + * + * Created on: Mar 5, 2021 + * Author: erki + */ + +#ifndef INC_PERIPHERALS_SPI_HPP_ +#define INC_PERIPHERALS_SPI_HPP_ + +#include "spi.h" + +#include "peripherals_config.hpp" +#include "peripherals_io.hpp" + +namespace Peripherals +{ + +struct Spi +{ + SPI_HandleTypeDef* hspi; + IO cs; + + std::uint32_t timeout = 10; + +#ifdef PERIPHERALS_USE_DELAY_US + std::uint32_t cs_set_delay = 0; +#endif + + Spi() = delete; + Spi(SPI_HandleTypeDef* hspi, const IO& cs); + + void WriteRegister(std::uint8_t reg, uint8_t data); + void WriteRegisterMultibyte(std::uint8_t reg, std::uint8_t* data, const std::uint32_t len); + + std::uint8_t ReadRegister(std::uint8_t reg, const std::uint32_t read_delay = 0); + void ReadRegisterMultibyte(std::uint8_t reg, std::uint8_t* data, const std::uint32_t len, const std::uint32_t read_delay = 0); +}; + +} + + +#endif /* INC_PERIPHERALS_SPI_HPP_ */ diff --git a/Inc/peripherals_utility.hpp b/Inc/peripherals_utility.hpp index e100dc1..06a9e2e 100644 --- a/Inc/peripherals_utility.hpp +++ b/Inc/peripherals_utility.hpp @@ -32,6 +32,32 @@ constexpr const T& Clamp(const T& v, const T& lo, const T& hi) return v; } +template +T ByteToTypeBE(const std::uint8_t a[N]) +{ + T t(0); + + for (std::size_t i = 0; i < N; i++) + { + t |= a[i] << (i * 8); + } + + return t; +} + +template +T ByteToTypeLE(const std::uint8_t a[N]) +{ + T t(0); + + for (std::size_t i = N; i >= 0; i--) + { + t |= a[i] << ((i - 1) * 8); + } + + return t; +} + } diff --git a/Src/peripherals_imu_icm.cpp b/Src/peripherals_imu_icm.cpp new file mode 100644 index 0000000..aac7fb2 --- /dev/null +++ b/Src/peripherals_imu_icm.cpp @@ -0,0 +1,274 @@ +/* + * peripherals_imu_icm.cpp + * + * Created on: Mar 5, 2021 + * Author: erki + */ + +#include "peripherals_imu_icm.hpp" + +#include + +#include "peripherals_utility.hpp" + +namespace +{ + +namespace _REGS +{ + +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; + + +} + +static const float _accel_fs_to_bit_constants[] = { + (2.0f / 32768.0f), + (4.0f / 32768.0f), + (8.0f / 32768.0f), + (16.0f / 32768.0f) +}; + +static const float _gyro_fs_to_bit_constants[] = { + (250.0f / 32768.0f), + (500.0f / 32768.0f), + (1000.0f / 32768.0f), + (2000.0f / 32768.0f) +}; + +} + +namespace Peripherals +{ + +ImuIcm::ImuIcm(SPI_HandleTypeDef* spi, const IO& cs) + : _spi(spi, cs) +{ } + +ImuIcm::ImuIcm(const Spi& spi) + : _spi(spi) +{ } + +#if USE_HAL_SPI_REGISTER_CALLBACKS == 1U + +void ImuIcm::SetSpiRxCallback(pSPI_CallbackTypeDef cb) +{ + _spi.hspi->RxCpltCallback = cb; +} + +void ImuIcm::SetSpiTxCallback(pSPI_CallbackTypeDef cb) +{ + _spi.hspi->TxCpltCallback = cb; +} + +#endif + +void ImuIcm::Setup() +{ + _spi.cs.Set(true); + HAL_Delay(100); + + _spi.WriteRegister(_REGS::PWR_MGMT_1 & _REGS::WRITE_MASK, 0b10000000); + HAL_Delay(10); + + _spi.WriteRegister(_REGS::USER_CTRL & _REGS::WRITE_MASK, 0b00010000); + HAL_Delay(10); + + _spi.WriteRegister(_REGS::PWR_MGMT_1 & _REGS::WRITE_MASK, 0b00000000); + HAL_Delay(10); + + _spi.WriteRegister(_REGS::PWR_MGMT_1 & _REGS::WRITE_MASK, 0b00000001); + HAL_Delay(20); + + _spi.WriteRegister(_REGS::CONFIG & _REGS::WRITE_MASK, 0x03); // DLPF_CFG = 3, gyro filter = 41/59.0, gyro rate = 1KHz, temp filter = 42 + HAL_Delay(10); + + SetGyroscopeScale(_scale_gyro); + + SetAccelerometerScale(_scale_accel); + + // ACCEL_FCHOICE_B = 0, A_DLPF_CFG = 3 filter=44.8/61.5 rate=1KHz + _spi.WriteRegister(_REGS::ACCEL_CONFIG2 & _REGS::WRITE_MASK, 0x03); + HAL_Delay(10); + + // SAMPLE_RATE = INTERNAL_SAMPLE_RATE / (1 + SMPLRT_DIV) Where INTERNAL_SAMPLE_RATE = 1kHz + _spi.WriteRegister(_REGS::SMPLRT_DIV & _REGS::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 + _spi.WriteRegister(_REGS::INT_PIN_CFG & _REGS::WRITE_MASK, 0); + HAL_Delay(10); + + _spi.WriteRegister(_REGS::INT_ENABLE & _REGS::WRITE_MASK, 1); + HAL_Delay(10); +} + +void ImuIcm::Calibrate(const std::uint32_t samples) +{ + std::array avg_gyro; + std::array avg_accel; + + for (std::uint32_t i = 0; i < samples; i++) + { + std::array raw; + auto add_to_avg = [&raw](std::array& out) + { + for (std::uint32_t j = 0; j < 3; j++) + out[j] += raw[j]; + }; + + ReadGyroRaw(raw.data()); + add_to_avg(avg_gyro); + + ReadAccelerometerRaw(raw.data()); + add_to_avg(avg_accel); + } + + for (std::uint32_t i = 0; i < 3; i++) + { + const std::int32_t max = std::numeric_limits::max(); + const std::int32_t min = std::numeric_limits::min(); + + _bias_gyro[i] = Clamp(avg_gyro[i], max, min); + _bias_accel[i] = Clamp(avg_accel[i], max, min); + } + + _bias_accel[2] -= AccelerometerReadingToRaw(1); +} + +void ImuIcm::SetGyroscopeScale(const ImuIcm::GyroScale scale) +{ + const std::uint8_t current_config = _spi.ReadRegister(_REGS::GYRO_CONFIG | _REGS::READ_MASK); + const std::uint8_t new_config = (current_config & 0xE7) | (std::uint8_t(scale) << 3); + _spi.WriteRegister(_REGS::GYRO_CONFIG & _REGS::WRITE_MASK, new_config); + + _scale_gyro = scale; +} + +void ImuIcm::SetAccelerometerScale(const ImuIcm::AccelerometerScale scale) +{ + const std::uint8_t current_config = _spi.ReadRegister(_REGS::ACCEL_CONFIG | _REGS::READ_MASK); + const std::uint8_t new_config = (current_config & 0xE7) | (std::uint8_t(scale) << 3); + _spi.WriteRegister(_REGS::ACCEL_CONFIG & _REGS::WRITE_MASK, new_config); + + _scale_accel = scale; +} + +void ImuIcm::ReadGyro(float* output) +{ + uint8_t data[6] = { 0 }; + _spi.ReadRegisterMultibyte(_REGS::GYRO_XOUT_H | _REGS::READ_MASK, data, 6); + + for (std::uint32_t i = 0; i < 3; i++) + { + const std::int16_t bit = ByteToTypeBE(&data[i * 2]); + output[i] = GyroRawToReading(bit); + } +} + +void ImuIcm::ReadGyroRaw(std::int16_t* output) +{ + uint8_t data[6] = { 0 }; + _spi.ReadRegisterMultibyte(_REGS::GYRO_XOUT_H | _REGS::READ_MASK, data, 6); + + for (std::uint32_t i = 0; i < 3; i++) + { + output[i] = ByteToTypeBE(&data[i * 2]); + } +} + +void ImuIcm::ReadAccelerometer(float* output) +{ + uint8_t data[6] = { 0 }; + _spi.ReadRegisterMultibyte(_REGS::ACCEL_XOUT_H | _REGS::READ_MASK, data, 6); + + for (std::uint32_t i = 0; i < 3; i++) + { + const std::int16_t bit = ByteToTypeBE(&data[i * 2]); + output[i] = AccelerometerRawToReading(bit); + } +} + +void ImuIcm::ReadAccelerometerRaw(std::int16_t* output) +{ + uint8_t data[6] = { 0 }; + _spi.ReadRegisterMultibyte(_REGS::ACCEL_XOUT_H | _REGS::READ_MASK, data, 6); + + for (std::uint32_t i = 0; i < 3; i++) + { + output[i] = ByteToTypeBE(&data[i * 2]); + } +} + +std::int16_t ImuIcm::AccelerometerReadingToRaw(const float& fs) const +{ + return fs / _accel_fs_to_bit_constants[std::uint32_t(_scale_accel)]; +} + +std::int16_t ImuIcm::GyroReadingToRaw(const float& fs) const +{ + return fs / _gyro_fs_to_bit_constants[std::uint32_t(_scale_gyro)]; +} + +float ImuIcm::AccelerometerRawToReading(const std::int16_t bit) const +{ + return float(bit) * _accel_fs_to_bit_constants[std::uint32_t(_scale_accel)]; +} + +float ImuIcm::GyroRawToReading(const std::int16_t bit) const +{ + return float(bit) * _gyro_fs_to_bit_constants[std::uint32_t(_scale_gyro)]; +} + +} diff --git a/Src/peripherals_spi.cpp b/Src/peripherals_spi.cpp new file mode 100644 index 0000000..2d6c584 --- /dev/null +++ b/Src/peripherals_spi.cpp @@ -0,0 +1,104 @@ +/* + * peripherals_spi.cpp + * + * Created on: Mar 5, 2021 + * Author: erki + */ + +#include "peripherals_spi.hpp" + +#include "peripherals_utility.hpp" + +#ifdef PERIPHERALS_USE_DELAY_US +# define CS_SET() do { cs.SetDelayUs(false, cs_set_delay); } while (0) +#else +# define CS_SET() do { cs.Set(false); } while (0) +#endif + +#define CS_UNSET() do { cs.Set(true); } while (0) + +namespace +{ + +struct _CsGuard +{ + _CsGuard(Peripherals::Spi& spi) + : _spi(spi) + { +#ifdef PERIPHERALS_USE_DELAY_US + _spi.cs.SetDelayUs(false, _spi.cs_set_delay); +#else + _spi.cs.Set(false); +#endif + } + + ~_CsGuard() + { + _spi.cs.Set(true); + } +private: + Peripherals::Spi& _spi; +}; + +} + +namespace Peripherals +{ + +Spi::Spi(SPI_HandleTypeDef* hspi, const IO& cs) + : hspi(hspi) + , cs(cs) +{ } + +void Spi::WriteRegister(std::uint8_t reg, uint8_t data) +{ + _CsGuard guard(*this); + + HAL_SPI_Transmit(hspi, ®, 1, timeout); + HAL_SPI_Transmit(hspi, &data, 1, timeout); +} + +void Spi::WriteRegisterMultibyte(std::uint8_t reg, std::uint8_t* data, const std::uint32_t len) +{ + _CsGuard guard(*this); + + HAL_SPI_Transmit(hspi, ®, 1, timeout); + HAL_SPI_Transmit(hspi, data, len, timeout); +} + +std::uint8_t Spi::ReadRegister(std::uint8_t reg, const std::uint32_t read_delay) +{ + _CsGuard guard(*this); + + HAL_SPI_Transmit(hspi, ®, 1, timeout); + +#ifdef PERIPHERALS_USE_DELAY_US + if (read_delay) + DelayUs(read_delay); +#else + (void)read_delay; +#endif + + std::uint8_t output = 0; + HAL_SPI_Receive(hspi, &output, 1, timeout); + + return output; +} + +void Spi::ReadRegisterMultibyte(std::uint8_t reg, std::uint8_t* data, const std::uint32_t len, const std::uint32_t read_delay) +{ + _CsGuard guard(*this); + + HAL_SPI_Transmit(hspi, ®, 1, timeout); + +#ifdef PERIPHERALS_USE_DELAY_US + if (read_delay) + DelayUs(read_delay); +#else + (void)read_delay; +#endif + + HAL_SPI_Receive(hspi, data, len, timeout); +} + +}