Basic untested LSM6DO32 6-axis IMU driver

Components/LSM6DO32Driver.cpp

@@ -0,0 +1,187 @@
+/*
+ * IMUDriver.cpp
+ *
+ *  Created on: Aug 31, 2024
+ *      Author: goada
+ */
+
+#include <LSM6DO32Driver.h>
+
+/* @brief Initialize the driver. Must be called before any other functions can be used.
+ * @param hspi_ Pointer to the SPI handle
+ * @param cs_gpio_ GPIO port for the chip select pin (GPIOA, GPIOB ...)
+ * @param cs_pin_ Pin number for the chip select
+ */
+void LSM6DO32_Driver::Init(SPI_HandleTypeDef* hspi_, GPIO_TypeDef* cs_gpio_, uint16_t cs_pin_) {
+	hspi = hspi_;
+	initialized = true;
+	SetCSPin(cs_gpio_, cs_pin_);
+	CSHigh();
+
+	uint8_t ID = GetRegister(LSM6DSO32_REG::WHO_AM_I);
+	if(ID != LSM6DSO32_ID) {
+		// couldn't get chip ID
+		initialized = false;
+		return;
+	}
+
+}
+
+/* @brief Sets a single 8-bit register.
+ * @param reg The register to set. Constants contained in ICM20948_REG namespace.
+ * @param val Value to set the register to.
+ * @return Success
+ */
+bool LSM6DO32_Driver::SetRegister(LSM6DSO32_REGISTER_t reg, uint8_t val) {
+	assert(initialized);
+
+	uint8_t data[2] = {(uint8_t)(0b00000000 | (reg&0x7F)),val};
+	CSLow();
+	HAL_StatusTypeDef result = HAL_SPI_Transmit(hspi, data, 2, 1000);
+	CSHigh();
+	return result == HAL_OK;
+
+}
+
+/* @brief Gets a single 8-bit register.
+ * @param reg The register to get. Constants contained in ICM20948_REG namespace.
+ * @return Value read from the register.
+ */
+uint8_t LSM6DO32_Driver::GetRegister(LSM6DSO32_REGISTER_t reg) {
+	assert(initialized);
+	uint8_t data[2] = {(uint8_t)(0b10000000 | (reg&0x7F)),SPI_DUMMY_BYTE};
+	uint8_t incoming[2] = {0,0}; 				// first byte is dummy byte
+	CSLow();
+	HAL_SPI_TransmitReceive(hspi, data, incoming, 2, 1000); // second incoming byte is response
+	CSHigh();
+	return incoming[1];
+}
+
+/* @brief Reads multiple successive registers in a row.
+ * @param startreg The register that the readings should start at.
+ * @param numBytes Number of bytes to read.
+ * @param out Address of buffer to receive data. Must be numBytes long.
+ */
+void LSM6DO32_Driver::GetMultipleRegisters(LSM6DSO32_REGISTER_t startreg, int numBytes,
+		uint8_t *out) {
+	assert(initialized);
+
+	uint8_t transmit[numBytes+1] = {SPI_DUMMY_BYTE};
+	transmit[0] = (uint8_t)(0b10000000 | startreg);
+
+	CSLow();
+	HAL_SPI_TransmitReceive(hspi, transmit, out, numBytes+1, 1000);
+	CSHigh();
+}
+
+/* @brief Repeatedly reads the top two bytes of each of the x, y, and z FIFOs in little-endian format.
+ * @param numReads Number of repetitive reads to perform.
+ * @param out Buffer to receive data in. Must be numReads*6 long.
+ */
+void LSM6DO32_Driver::SampleFIFOs(int numReads, uint8_t *out, size_t outBufferSize) {
+	assert(initialized);
+	assert(outBufferSize >= (size_t)numReads * 6);
+
+	for(int i = 0; i < numReads; i++) {
+		GetMultipleRegisters(LSM6DSO32_REG::FIFO_DATA_OUT_X_L, 6, out+i*6);
+	}
+}
+
+/* @brief Extract IMU data from a raw byte buffer (such as from ReadAllSensorRegs) into a struct.
+ * @param buf Input buffer containing raw data.
+ * @param accel Buffer includes accel data
+ * @param gyro Buffer includes gyroscope data
+ * @param temp Buffer includes temperature data
+ * @return Struct containing extracted data
+ */
+const LSM6DSO32_DATA_t LSM6DO32_Driver::ConvertRawMeasurementToStruct(const uint8_t *buf, bool accel, bool gyro, bool temp) {
+	LSM6DSO32_DATA_t out;
+	size_t i = 0;
+
+	// Accel gyro and temp are little-endian
+	if(temp) {
+		out.temp = (buf[i]) | (buf[i+1]<<8);
+		i += 2;
+	}
+
+	if(gyro) {
+
+		out.gyro.x = (buf[i  ]) | (buf[i+1] << 8);
+		out.gyro.y = (buf[i+2]) | (buf[i+3] << 8);
+		out.gyro.z = (buf[i+4]) | (buf[i+5] << 8);
+		i += 6;
+	}
+
+	if(accel) {
+		out.accel.x = (buf[i  ]) | ((buf[i+1]) << 8);
+		out.accel.y = (buf[i+2]) | ((buf[i+3]) << 8);
+		out.accel.z = (buf[i+4]) | ((buf[i+5]) << 8);
+		i += 6;
+	}
+
+	return out;
+
+}
+
+/* @brief Reads all 14 sensor registers in order. (temp, gyro, accel)
+ * @param out Raw bytes read from registers. Must be 14 bytes long
+ */
+void LSM6DO32_Driver::ReadSensors(uint8_t* out) {
+	GetMultipleRegisters(LSM6DSO32_REG::OUT_TEMP_L, 14, out);
+}
+
+
+LSM6DO32_Driver::LSM6DO32_Driver() {
+}
+
+LSM6DO32_Driver::~LSM6DO32_Driver() {
+}
+
+void LSM6DO32_Driver::CSLow() {
+	assert(initialized);
+	HAL_GPIO_WritePin(cs_gpio, cs_pin, GPIO_PIN_RESET);
+}
+
+void LSM6DO32_Driver::SetCSPin(GPIO_TypeDef* gpio, uint16_t pin) {
+	cs_gpio = gpio;
+	cs_pin = pin;
+}
+
+void LSM6DO32_Driver::CSHigh() {
+	assert(initialized);
+	HAL_GPIO_WritePin(cs_gpio, cs_pin, GPIO_PIN_SET);
+}
+
+/* @brief Set the output data rate of the accelerometer
+ * @param speed Output speed
+ */
+void LSM6DO32_Driver::SetAccelODR(LSM6D032_SAMPLE_SPEED speed) {
+	assert(initialized);
+	SetRegister(LSM6DSO32_REG::CTRL1_XL, (GetRegister(LSM6DSO32_REG::CTRL1_XL) & 0b00001111) | (speed<<4));
+}
+
+/* @brief Set the output data rate of the gyroscope
+ * @param speed Output speed
+ */
+void LSM6DO32_Driver::SetGyroODR(LSM6D032_SAMPLE_SPEED speed) {
+	assert(initialized);
+	SetRegister(LSM6DSO32_REG::CTRL2_G, (GetRegister(LSM6DSO32_REG::CTRL2_G) & 0b00001111) | (speed<<4));
+}
+
+/* @brief Set the full-scale range of the accelerometer
+ * @param scale Range, in plus/minus G
+ */
+void LSM6DO32_Driver::SetAccelFullScaleRange(LSM6D032_ACCEL_SCALE_SELECT scale) {
+	assert(initialized);
+	SetRegister(LSM6DSO32_REG::CTRL1_XL, (GetRegister(LSM6DSO32_REG::CTRL1_XL) & 0b11110011) | (scale<<2));
+}
+
+/* @brief Set the full-scale range of the gyroscope
+ * @param scale Range, in plus/minus dps
+ */
+void LSM6DO32_Driver::SetGyroFullScaleRange(LSM6D032_GYRO_SCALE_SELECT scale) {
+	assert(initialized);
+	SetRegister(LSM6DSO32_REG::CTRL2_G, (GetRegister(LSM6DSO32_REG::CTRL2_G) & 0b11110011) | (scale<<2));
+
+}
+

Components/LSM6DO32Driver.h

@@ -0,0 +1,162 @@
+/*
+ * IMUDriver.h
+ *
+ *  Created on: Aug 31, 2024
+ *      Author: goada
+ */
+
+#ifndef LSM6DO32DRIVER_H_
+#define LSM6DO32DRIVER_H_
+
+#include "stm32h7xx.h"
+
+constexpr uint8_t SPI_DUMMY_BYTE = 0x00;
+constexpr uint8_t LSM6DSO32_ID = 0x6C;
+
+typedef uint8_t LSM6DSO32_REGISTER_t;
+
+struct ACCEL_t {
+	int16_t x;
+	int16_t y;
+	int16_t z;
+};
+
+struct GYRO_t {
+	int16_t x;
+	int16_t y;
+	int16_t z;
+};
+
+struct LSM6DSO32_DATA_t {
+	ACCEL_t accel;
+	GYRO_t gyro;
+	int16_t temp;
+};
+
+/* @brief Singleton SPI driver for the 6-axis LSM6DSO32 IMU.
+ * Must call Init before using any other functions besides SetCSPin.
+ * SPI max clock speed 10MHz.
+ */
+class LSM6DO32_Driver {
+public:
+	LSM6DO32_Driver();
+	~LSM6DO32_Driver();
+
+	static LSM6DO32_Driver& Inst() {
+		static LSM6DO32_Driver inst;
+		return inst;
+	}
+	enum LSM6D032_SAMPLE_SPEED {
+		FREQ_12p5_HZ=0b0001,
+		FREQ_26_HZ=0b0010,
+		FREQ_52_HZ=0b0011,
+		FREQ_104_HZ=0b0100,
+		FREQ_208_HZ=0b0101,
+		FREQ_416_HZ=0b0110,
+		FREQ_833_HZ=0b0111,
+		FREQ_1660_HZ=0b1000,
+		FREQ_3330_HZ=0b1001,
+		FREQ_6660_HZ=0b1010
+	};
+
+	enum LSM6D032_ACCEL_SCALE_SELECT {
+		SCALE_4g = 0b00,
+		SCALE_32g = 0b01,
+		SCALE_8g = 0b10,
+		SCALE_16g = 0b11
+	};
+
+	enum LSM6D032_GYRO_SCALE_SELECT {
+		SCALE_250dps = 0b00,
+		SCALE_500dps = 0b01,
+		SCALE_1000dps = 0b10,
+		SCALE_2000dps = 0b11
+	};
+
+
+	void Init(SPI_HandleTypeDef* hspi, GPIO_TypeDef* cs_gpio_, uint16_t cs_pin_);
+
+	bool SetRegister(LSM6DSO32_REGISTER_t reg, uint8_t val);
+	uint8_t GetRegister(LSM6DSO32_REGISTER_t reg);
+	void GetMultipleRegisters(LSM6DSO32_REGISTER_t startreg, int numBytes, uint8_t* out);
+	void ReadSensors(uint8_t* out);
+	void ReadFIFOs(int numReads, uint8_t* out);
+
+	void SetAccelODR(LSM6D032_SAMPLE_SPEED speed);
+	void SetGyroODR(LSM6D032_SAMPLE_SPEED speed);
+	void SetAccelFullScaleRange(LSM6D032_ACCEL_SCALE_SELECT scale);
+	void SetGyroFullScaleRange(LSM6D032_GYRO_SCALE_SELECT scale);
+
+	LSM6DO32_Driver(const LSM6DO32_Driver&) = delete;
+	LSM6DO32_Driver& operator=(const LSM6DO32_Driver&) = delete;
+
+	const LSM6DSO32_DATA_t ConvertRawMeasurementToStruct(const uint8_t *buf, bool accel = true, bool gyro = true, bool temp = true);
+
+	void SetCSPin(GPIO_TypeDef* gpio, uint16_t pin);
+
+private:
+	bool initialized = false;
+	SPI_HandleTypeDef* hspi = nullptr;
+	GPIO_TypeDef* cs_gpio;
+	uint16_t cs_pin;
+
+	bool SwitchBank(uint8_t bank);
+
+	void CSLow();
+	void CSHigh();
+
+};
+
+
+
+// Constants for registers in the LSM6DSO32 IMU
+// First byte is the bank in which it is located
+// (0xff means it is located in the same place in every bank. Only used by the bank switch register)
+// Second byte is address within bank
+namespace LSM6DSO32_REG {
+
+	constexpr LSM6DSO32_REGISTER_t FIFO_CTRL2 = 0x08;
+	constexpr LSM6DSO32_REGISTER_t FIFO_CTRL3 = 0x09;
+	constexpr LSM6DSO32_REGISTER_t FIFO_CTRL4 = 0x0A;
+
+	constexpr LSM6DSO32_REGISTER_t WHO_AM_I  = 0x0F;
+
+	constexpr LSM6DSO32_REGISTER_t CTRL1_XL  = 0x10;
+	constexpr LSM6DSO32_REGISTER_t CTRL2_G   = 0x11;
+
+	constexpr LSM6DSO32_REGISTER_t STATUS_REG    = 0x1E;
+
+	constexpr LSM6DSO32_REGISTER_t OUT_TEMP_L    = 0x20;
+	constexpr LSM6DSO32_REGISTER_t OUT_TEMP_H    = 0x21;
+
+	constexpr LSM6DSO32_REGISTER_t OUTX_L_G    = 0x22;
+	constexpr LSM6DSO32_REGISTER_t OUTX_H_G    = 0x23;
+	constexpr LSM6DSO32_REGISTER_t OUTY_L_G    = 0x24;
+	constexpr LSM6DSO32_REGISTER_t OUTY_H_G    = 0x25;
+	constexpr LSM6DSO32_REGISTER_t OUTZ_L_G    = 0x26;
+	constexpr LSM6DSO32_REGISTER_t OUTZ_H_G    = 0x27;
+
+	constexpr LSM6DSO32_REGISTER_t OUTX_L_A    = 0x28;
+	constexpr LSM6DSO32_REGISTER_t OUTX_H_A    = 0x29;
+	constexpr LSM6DSO32_REGISTER_t OUTY_L_A    = 0x2A;
+	constexpr LSM6DSO32_REGISTER_t OUTY_H_A    = 0x2B;
+	constexpr LSM6DSO32_REGISTER_t OUTZ_L_A    = 0x2C;
+	constexpr LSM6DSO32_REGISTER_t OUTZ_H_A    = 0x2D;
+
+	constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_TAG = 0x78;
+
+	constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_X_L = 0x79;
+	constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_X_H = 0x7A;
+
+	constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Y_L = 0x7B;
+	constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Y_H = 0x7C;
+
+	constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Z_L = 0x7D;
+	constexpr LSM6DSO32_REGISTER_t FIFO_DATA_OUT_Z_H = 0x7E;
+
+
+}
+
+
+
+#endif /* LSM6DO32DRIVER_H_ */