Port IMU drivers to library

2021-03-05 20:59:28 +02:00 · 2021-03-05 20:59:28 +02:00 · 6662bbde92
commit 6662bbde92
parent 13dcb08f94
6 changed files with 569 additions and 0 deletions
--- a/Inc/peripherals_imu.hpp
+++ 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 <cstdint>
+
+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_ */
--- a/Inc/peripherals_imu_icm.hpp
+++ 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 <array>
+
+#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<std::int16_t, 3> _bias_gyro;
+	std::array<std::int16_t, 3> _bias_accel;
+};
+
+}
+
+#endif /* PERIPHERALS_IMU_ICM_HPP_ */
--- a/Inc/peripherals_spi.hpp
+++ 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_ */
--- 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<typename T, std::size_t N>
+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<typename T, std::size_t N>
+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;
+}
+
 }


--- a/Src/peripherals_imu_icm.cpp
+++ 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 <limits>
+
+#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<std::int32_t, 3> avg_gyro;
+	std::array<std::int32_t, 3> avg_accel;
+
+	for (std::uint32_t i = 0; i < samples; i++)
+	{
+		std::array<std::int16_t, 3> raw;
+		auto add_to_avg = [&raw](std::array<std::int32_t, 3>& 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<std::int16_t>::max();
+		const std::int32_t min = std::numeric_limits<std::int16_t>::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<std::int16_t, 2>(&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<std::int16_t, 2>(&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<std::int16_t, 2>(&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<std::int16_t, 2>(&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)];
+}
+
+}
--- a/Src/peripherals_spi.cpp
+++ 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, &reg, 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, &reg, 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, &reg, 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, &reg, 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);
+}
+
+}