From 646e67967b02a7fd2424e386b8f29758c0e3a887 Mon Sep 17 00:00:00 2001 From: WilliamGomez1 <180569402+WilliamGomez1@users.noreply.github.com> Date: Mon, 6 Oct 2025 18:08:58 -0400 Subject: [PATCH 1/4] Add files via upload --- mag.c | 220 +++++++++++++++++++++++++++++++++++++++++++++++++ mag.h | 164 ++++++++++++++++++++++++++++++++++++ sensors_defs.h | 62 ++++++++++++++ 3 files changed, 446 insertions(+) create mode 100644 mag.c create mode 100644 mag.h create mode 100644 sensors_defs.h diff --git a/mag.c b/mag.c new file mode 100644 index 0000000..429e2d0 --- /dev/null +++ b/mag.c @@ -0,0 +1,220 @@ +/** + * @file mag.c + * @brief Implementation of magnetometer driver + * + * This file contains the implementation of the magnetometer driver + * using the LIS3MDL sensor. It includes functions for initializing, + * configuring, and reading data from the magnetometer. + * + * @authors Amelia Ellis + * @date 2025-04-11 + * @version 0.1 + * @bug No known bugs. + */ +#include "mag.h" + +//GPIO_TypeDef LIS3MDL_CS_PORT = GPIOG; +//uint16_t LIS3MDL_CS_PIN = GPIO_PIN_12; +/** + * @brief Write a single byte to a LIS3MDL register. + * @param reg: Register address. + * @param value: Value to write. + */ +void LIS3MDL_WriteRegister(uint8_t reg, uint8_t value) { + HAL_StatusTypeDef status; + uint8_t txData[2] = {reg & 0x7F, value}; // MSB 0 for write + LIS3MDL_CS_LOW(); + status = HAL_SPI_Transmit(&hspi1, txData, sizeof(txData), SENSORS_SPI_TIMEOUT); + if (status != HAL_OK) { + printf("Error writing to LIS3MDL register 0x%02X: %d\n", reg, status); + if (status == HAL_TIMEOUT) { + printf("LIS3MDL SPI write timeout\n"); + } else if (status == HAL_ERROR) { + printf("LIS3MDL SPI write error\n"); + } else if (status == HAL_BUSY) { + printf("LIS3MDL SPI is busy\n"); + } + } + LIS3MDL_CS_HIGH(); +} + +/** + * @brief Read a single byte from a LIS3MDL register. + * @param reg: Register address. + * @return The value read. + */ +uint8_t LIS3MDL_ReadRegister(uint8_t reg) { + HAL_StatusTypeDef status; + uint8_t txData = reg | 0x80; // MSB 1 for read + uint8_t rxData; + LIS3MDL_CS_LOW(); + status = HAL_SPI_Transmit(&hspi1, &txData, 1, SENSORS_SPI_TIMEOUT); + if (status != HAL_OK) { + printf("Error writing to LIS3MDL register 0x%02X: %d\n", reg, status); + if (status == HAL_TIMEOUT) { + printf("LIS3MDL SPI write timeout\n"); + } else if (status == HAL_ERROR) { + printf("LIS3MDL SPI write error\n"); + } else if (status == HAL_BUSY) { + printf("LIS3MDL SPI is busy\n"); + } + } + status = HAL_SPI_Receive(&hspi1, &rxData, 1, SENSORS_SPI_TIMEOUT); + if (status != HAL_OK) { + printf("Error reading from LIS3MDL register 0x%02X: %d\n", reg, status); + if (status == HAL_TIMEOUT) { + printf("LIS3MDL SPI read timeout\n"); + } else if (status == HAL_ERROR) { + printf("LIS3MDL SPI read error\n"); + } else if (status == HAL_BUSY) { + printf("LIS3MDL SPI is busy\n"); + } + } + LIS3MDL_CS_HIGH(); + return rxData; +} + +/** + * @brief Read multiple bytes starting from a LIS3MDL register. + * @param reg: Starting register address. + * @param buffer: Buffer to store the data. + * @param len: Number of bytes to read. + */ +void LIS3MDL_ReadRegisters(uint8_t reg, uint8_t *buffer, uint8_t len) { + HAL_StatusTypeDef status; + uint8_t txData = reg | 0x80; // MSB 1 for read + LIS3MDL_CS_LOW(); + status = HAL_SPI_Transmit(&hspi1, &txData, 1, SENSORS_SPI_TIMEOUT); + if (status != HAL_OK) { + printf("Error writing to LIS3MDL register 0x%02X: %d\n", reg, status); + if (status == HAL_TIMEOUT) { + printf("LIS3MDL SPI write timeout\n"); + } else if (status == HAL_ERROR) { + printf("LIS3MDL SPI write error\n"); + } else if (status == HAL_BUSY) { + printf("LIS3MDL SPI is busy\n"); + } + } + status = HAL_SPI_Receive(&hspi1, buffer, len, SENSORS_SPI_TIMEOUT); + if (status != HAL_OK) { + printf("Error reading from LIS3MDL register 0x%02X: %d\n", reg, status); + if (status == HAL_TIMEOUT) { + printf("LIS3MDL SPI read timeout\n"); + } else if (status == HAL_ERROR) { + printf("LIS3MDL SPI read error\n"); + } else if (status == HAL_BUSY) { + printf("LIS3MDL SPI is busy\n"); + } + } + LIS3MDL_CS_HIGH(); +} + +uint8_t Mag_Init() { + printf("Initializing Magnetometer...\n"); + uint8_t whoAmI = LIS3MDL_ReadRegister(LIS3MDL_WHO_AM_I_ADDR); + if (whoAmI != LIS3MDL_WHO_AM_I_VALUE) { + printf("Initialization error: LIS3MDL not found\n"); + printf("Initialization Failed: Magnetometer not responding\n"); + return 1; // Device not found + } + + LIS3MDL_SETTINGS[0] = LIS3MDL_RANGE_12_GAUSS; + LIS3MDL_SETTINGS[1] = LIS3MDL_DATARATE_10_HZ; + LIS3MDL_SETTINGS[2] = LIS3MDL_PERFORMANCEMODE_HIGH; + LIS3MDL_SETTINGS[3] = LIS3MDL_OPERATIONMODE_CONTINUOUS; + + // Configure CTRL_REG1: Enable temperature sensor, set high-performance mode, ODR=10Hz + LIS3MDL_WriteRegister(LIS3MDL_CTRL_REG1_ADDR, 0x70); + + // Configure CTRL_REG2: Set full scale to ±12 gauss + LIS3MDL_WriteRegister(LIS3MDL_CTRL_REG2_ADDR, 0x20); + + // Configure CTRL_REG3: Set continuous-conversion mode + LIS3MDL_WriteRegister(LIS3MDL_CTRL_REG3_ADDR, 0x00); + + Mag_GetConfig(); + + return 0; // Initialization successful +} + +uint8_t Mag_Read(vector_t *pData) { + uint8_t buffer[6]; + LIS3MDL_ReadRegisters(LIS3MDL_OUT_X_L_ADDR, buffer, 6); + // Convert raw data to gauss + int16_t x_raw = (int16_t)(buffer[1] << 8 | buffer[0]); + int16_t y_raw = (int16_t)(buffer[3] << 8 | buffer[2]); + int16_t z_raw = (int16_t)(buffer[5] << 8 | buffer[4]); + + float scale = 1.0f; // Scale factor based on range + switch (LIS3MDL_SETTINGS[0]) { + case LIS3MDL_RANGE_4_GAUSS: scale = 6842.0f; break; + case LIS3MDL_RANGE_8_GAUSS: scale = 3421.0f; break; + case LIS3MDL_RANGE_12_GAUSS: scale = 2281.0f; break; + case LIS3MDL_RANGE_16_GAUSS: scale = 1711.0f; break; + } + + pData->x = x_raw / scale; + pData->y = y_raw / scale; + pData->z = z_raw / scale; + + return 0; // Success +} + +uint8_t Mag_SetConfig(uint8_t *settings) { + uint8_t ctrl_reg1 = (settings[1] << 1); // Data rate bits + uint8_t ctrl_reg2 = (settings[0] << 5); // Range bits + // uint8_t ctrl_reg3 = (settings[3] << 0); // Operation mode bits + uint8_t ctrl_reg4 = (settings[2] << 2); // Performance mode bits + + // Write configuration to registers + LIS3MDL_WriteRegister(LIS3MDL_CTRL_REG1_ADDR, ctrl_reg1); + LIS3MDL_WriteRegister(LIS3MDL_CTRL_REG2_ADDR, ctrl_reg2); + // LIS3MDL_WriteRegister(self, LIS3MDL_CTRL_REG3_ADDR, ctrl_reg3); + LIS3MDL_WriteRegister(LIS3MDL_CTRL_REG4_ADDR, ctrl_reg4); + // Read back the configuration to verify + if (Mag_GetConfig(LIS3MDL_SETTINGS) != 0 || + LIS3MDL_SETTINGS[0] != settings[0] || + LIS3MDL_SETTINGS[1] != settings[1] || + LIS3MDL_SETTINGS[2] != settings[2]) { + return 1; // Configuration verification failed + } + + return 0; // Success +} + +uint8_t Mag_GetConfig() { + uint8_t ctrl_reg1, ctrl_reg2, ctrl_reg4; + // Read configuration registers + ctrl_reg1 = LIS3MDL_ReadRegister(LIS3MDL_CTRL_REG1_ADDR); + ctrl_reg2 = LIS3MDL_ReadRegister(LIS3MDL_CTRL_REG2_ADDR); + ctrl_reg4 = LIS3MDL_ReadRegister(LIS3MDL_CTRL_REG4_ADDR); + // Check if the registers are valid + if (ctrl_reg1 == 0xFF || ctrl_reg2 == 0xFF || ctrl_reg4 == 0xFF) { + return 1; // Error reading registers + } + + // Extract configuration values + LIS3MDL_SETTINGS[0] = (ctrl_reg2 >> 5) & 0x03; // Range bits + LIS3MDL_SETTINGS[1] = (ctrl_reg1 >> 1) & 0x0F; // Data rate bits + LIS3MDL_SETTINGS[2] = (ctrl_reg4 >> 5) & 0x03; // Performance Mode bits + return 0; // Success +} + +uint8_t Mag_Test() { + printf("Starting LIS3MDL Driver Test\n"); + // Initialize LIS3MDL + if (Mag_Init() != 0) { + // Handle error: LIS3MDL not found + printf("Initialization error: LIS3MDL not found\n"); + return 1; + } + printf("LIS3MDL Initialized Successfully \n"); + DelayMs(1000); + vector_t magData; + if (Mag_Read(&magData) != 0) { + printf("Read Error: LIS3MDL Read Failed "); + return 1; + } + printf("X: %4.2f Y: %4.2f Z: %4.2f \n", magData.x, magData.y, magData.z); + return 0; +} diff --git a/mag.h b/mag.h new file mode 100644 index 0000000..5e93c85 --- /dev/null +++ b/mag.h @@ -0,0 +1,164 @@ +/** + * @file mag.h + * @brief Header file for magnetometer driver + * + * This file contains the definitions and function prototypes for + * initializing and reading data from the magnetometer. It includes + * the necessary structures, function prototypes, and constants + * required for the magnetometer driver. + * + * @authors Amelia Ellis + * @date 2025-04-11 + * @version 0.1 + * @bug No known bugs. + */ +#ifndef MAG_H +#define MAG_H + +#include +#include +#include +#include "stm32h7xx_hal.h" +#include "sensors_defs.h" + + + +// Register Addresses +#define LIS3MDL_WHO_AM_I_ADDR 0x0F +#define LIS3MDL_CTRL_REG1_ADDR 0x20 +#define LIS3MDL_CTRL_REG2_ADDR 0x21 +#define LIS3MDL_CTRL_REG3_ADDR 0x22 +#define LIS3MDL_CTRL_REG4_ADDR 0x23 +#define LIS3MDL_OUT_X_L_ADDR 0x28 +#define LIS3MDL_OUT_X_H_ADDR 0x29 +#define LIS3MDL_OUT_Y_L_ADDR 0x2A +#define LIS3MDL_OUT_Y_H_ADDR 0x2B +#define LIS3MDL_OUT_Z_L_ADDR 0x2C +#define LIS3MDL_OUT_Z_H_ADDR 0x2D + +// WHO_AM_I Expected Value +#define LIS3MDL_WHO_AM_I_VALUE 0x3D + + + +// Pin Definitions +#define LIS3MDL_CS_PORT GPIOG +#define LIS3MDL_CS_PIN GPIO_PIN_12 +//extern GPIO_TypeDef LIS3MDL_CS_PORT; +//extern uint16_t LIS3MDL_CS_PIN; + + +extern uint8_t LIS3MDL_SETTINGS[8]; + +// SPI Communication Macros +#define LIS3MDL_CS_LOW() HAL_GPIO_WritePin(LIS3MDL_CS_PORT, LIS3MDL_CS_PIN, GPIO_PIN_RESET) +#define LIS3MDL_CS_HIGH() HAL_GPIO_WritePin(LIS3MDL_CS_PORT, LIS3MDL_CS_PIN, GPIO_PIN_SET) + +/** The magnetometer ranges */ +typedef enum { + LIS3MDL_RANGE_4_GAUSS = 0b00, ///< +/- 4g (default value) + LIS3MDL_RANGE_8_GAUSS = 0b01, ///< +/- 8g + LIS3MDL_RANGE_12_GAUSS = 0b10, ///< +/- 12g + LIS3MDL_RANGE_16_GAUSS = 0b11, ///< +/- 16g + } lis3mdl_range_t; + + /** The magnetometer data rate, includes FAST_ODR bit */ + typedef enum { + LIS3MDL_DATARATE_0_625_HZ = 0b0000, ///< 0.625 Hz + LIS3MDL_DATARATE_1_25_HZ = 0b0010, ///< 1.25 Hz + LIS3MDL_DATARATE_2_5_HZ = 0b0100, ///< 2.5 Hz + LIS3MDL_DATARATE_5_HZ = 0b0110, ///< 5 Hz + LIS3MDL_DATARATE_10_HZ = 0b1000, ///< 10 Hz + LIS3MDL_DATARATE_20_HZ = 0b1010, ///< 20 Hz + LIS3MDL_DATARATE_40_HZ = 0b1100, ///< 40 Hz + LIS3MDL_DATARATE_80_HZ = 0b1110, ///< 80 Hz + LIS3MDL_DATARATE_155_HZ = 0b0001, ///< 155 Hz (FAST_ODR + UHP) + LIS3MDL_DATARATE_300_HZ = 0b0011, ///< 300 Hz (FAST_ODR + HP) + LIS3MDL_DATARATE_560_HZ = 0b0101, ///< 560 Hz (FAST_ODR + MP) + LIS3MDL_DATARATE_1000_HZ = 0b0111, ///< 1000 Hz (FAST_ODR + LP) + } lis3mdl_dataRate_t; + + /** The magnetometer performance mode */ + typedef enum { + LIS3MDL_PERFORMANCEMODE_LOWPOWER = 0b00, ///< Low power mode + LIS3MDL_PERFORMANCEMODE_MEDIUM = 0b01, ///< Medium performance mode + LIS3MDL_PERFORMANCEMODE_HIGH = 0b10, ///< High performance mode + LIS3MDL_PERFORMANCEMODE_ULTRAHIGH = 0b11, ///< Ultra-high performance mode + } lis3mdl_performancemode_t; + + /** The magnetometer operation mode */ + typedef enum { + LIS3MDL_OPERATIONMODE_CONTINUOUS = 0b00, ///< Continuous conversion mode + LIS3MDL_OPERATIONMODE_SINGLE = 0b01, ///< Single-shot conversion + LIS3MDL_OPERATIONMODE_POWERDOWN = 0b11, ///< Powered-down + } lis3mdl_operationmode_t; + + + +/** + * @brief Write a single byte to a LIS3MDL register. + * @param reg: Register address. + * @param value: Value to write. + */ +void LIS3MDL_WriteRegister(uint8_t reg, uint8_t value); +/** + * @brief Read a single byte from a LIS3MDL register. + * @param reg: Register address. + * @return The value read. + */ +uint8_t LIS3MDL_ReadRegister(uint8_t reg); + +/** + * @brief Read multiple bytes starting from a LIS3MDL register. + * @param reg: Starting register address. + * @param buffer: Buffer to store the data. + * @param length: Number of bytes to read. + */ +void LIS3MDL_ReadRegisters(uint8_t reg, uint8_t *buffer, uint8_t len); + +/** + * @brief Initializes the magnetometer. + * + * This function configures the SPI communication and sets up the GPIO pins + * for the magnetometer. It also performs a self-test to ensure the sensor is + * functioning correctly. + * + * @return uint8_t Status of the initialization (0 for success, 1 for failure). + */ +uint8_t Mag_Init(); + +/** + * @brief This function configures the mode, data rate, and range of the magnetometer. + * + * @param mode The mode to set (0 for continuous, 1 for single). + * @param dataRate The data rate to set (in Hz). + * @param range The range to set (in Gauss). + * @return uint8_t Status of the configuration (0 for success, 1 for failure). + */ +uint8_t Mag_SetConfig(uint8_t *settings); + +/** + * @brief Gets the current configuration of the magnetometer. + * + * This function retrieves the current mode, data rate, and range of the magnetometer. + * + * @param mode Pointer to store the current mode. + * @param dataRate Pointer to store the current data rate. + * @param range Pointer to store the current range. + * @return uint8_t Status of the configuration retrieval (0 for success, 1 for failure). + */ +uint8_t Mag_GetConfig(); + +/** + * @brief Reads data from the magnetometer. + * + * This function retrieves the magnetic field data from the magnetometer. + * + * @param pData Pointer to store the magnetic field data (x, y, z components). + * @return uint8_t Status of the read operation (0 for success, 1 for failure). + */ +uint8_t Mag_Read(vector_t *pData); + +uint8_t Mag_Test(); + +#endif // MAG_H diff --git a/sensors_defs.h b/sensors_defs.h new file mode 100644 index 0000000..390aa25 --- /dev/null +++ b/sensors_defs.h @@ -0,0 +1,62 @@ +/** + * @file sensors.h + * @brief Header file for sensor initialization and data reading functions. + * + * This file contains the definitions for initializing and reading data from + * various sensors including IMU, Barometer, Magnetometer, Accelerometer, + * and GPS. It also includes function prototypes for temperature sensors. + * + * @authors Amelia Ellis + * @date 2025-04-10 + * @version 0.1 + * @bug No known bugs. + * + * @todo Remove fake_hal.h and replace with actual HAL header. + * @todo Add error handling for sensor initialization and data reading. + */ +#ifndef SENSORS_H +#define SENSORS_H + +#include +#include +#include +#include +#include "stm32h7xx_hal.h" + + +#define SENSORS_MAGFIELD_EARTH_MAX \ + (60.0F) /**< Maximum magnetic field on Earth's surface */ +#define SENSORS_MAGFIELD_EARTH_MIN \ + (30.0F) /**< Minimum magnetic field on Earth's surface */ +#define SENSORS_PRESSURE_SEALEVELHPA \ + (1013.25F) /**< Average sea level pressure is 1013.25 hPa */ +#define SENSORS_DPS_TO_RADS \ + (0.017453293F) /**< Degrees/s to rad/s multiplier */ +#define SENSORS_RADS_TO_DPS \ + (57.29577793F) /**< Rad/s to degrees/s multiplier */ +#define SENSORS_GAUSS_TO_MICROTESLA \ + (100) /**< Gauss to micro-Tesla multiplier */ +#define SENSORS_MICROTESLA_TO_GAUSS \ + (0.01F) /**< Micro-Tesla to Gauss multiplier */ +#define SENSORS_MG_TO_MS2 \ + (0.00980665F) /**< mg to m/s^2 multiplier */ +#define SENSORS_MS2_TO_MG \ + (101.971621F) /**< m/s^2 to mg multiplier */ + +#define SENSORS_GRAVITY_STANDARD (9.80665F) /**< Standard gravity in m/s^2 */ + + +#define SENSORS_SPI_TIMEOUT 2000 /**< SPI timeout in milliseconds */ + +// SPI Handle +extern SPI_HandleTypeDef hspi1; + +typedef union vector3_u { + float v[3]; + struct { + float x, y, z; + }; +} vector_t; + + +#endif // SENSORS_H From 92414f21f0d9a570c6efec978d55a4ddb6ea80ce Mon Sep 17 00:00:00 2001 From: ericslchiang Date: Sat, 11 Oct 2025 18:37:47 -0400 Subject: [PATCH 2/4] first rev of LIS2MDL magnetometer driver --- mag.c | 207 ++++++++++++++++++++----------------------------- mag.h | 180 +++++++++++++++++++++--------------------- sensors_defs.h | 62 --------------- 3 files changed, 173 insertions(+), 276 deletions(-) delete mode 100644 sensors_defs.h diff --git a/mag.c b/mag.c index 429e2d0..f25cbfb 100644 --- a/mag.c +++ b/mag.c @@ -3,218 +3,177 @@ * @brief Implementation of magnetometer driver * * This file contains the implementation of the magnetometer driver - * using the LIS3MDL sensor. It includes functions for initializing, + * using the LIS2MDL sensor. It includes functions for initializing, * configuring, and reading data from the magnetometer. * - * @authors Amelia Ellis + * @authors Amelia Ellis, Eric Chiang * @date 2025-04-11 * @version 0.1 * @bug No known bugs. */ -#include "mag.h" +#include "mag2.h" -//GPIO_TypeDef LIS3MDL_CS_PORT = GPIOG; -//uint16_t LIS3MDL_CS_PIN = GPIO_PIN_12; /** - * @brief Write a single byte to a LIS3MDL register. + * @brief Write a single byte to a LIS2MDL register. * @param reg: Register address. * @param value: Value to write. */ -void LIS3MDL_WriteRegister(uint8_t reg, uint8_t value) { +void LIS2MDL_WriteRegister(uint8_t addr, uint8_t value) { HAL_StatusTypeDef status; - uint8_t txData[2] = {reg & 0x7F, value}; // MSB 0 for write - LIS3MDL_CS_LOW(); + uint8_t txData[2] = {addr & 0x7F, value}; // MSB 0 for write + LIS2MDL_CS_LOW(); status = HAL_SPI_Transmit(&hspi1, txData, sizeof(txData), SENSORS_SPI_TIMEOUT); if (status != HAL_OK) { - printf("Error writing to LIS3MDL register 0x%02X: %d\n", reg, status); + printf("Error writing to LIS2MDL register 0x%02X: %d\n", addr, status); if (status == HAL_TIMEOUT) { - printf("LIS3MDL SPI write timeout\n"); + printf("LIS2MDL SPI write timeout\n"); } else if (status == HAL_ERROR) { - printf("LIS3MDL SPI write error\n"); + printf("LIS2MDL SPI write error\n"); } else if (status == HAL_BUSY) { - printf("LIS3MDL SPI is busy\n"); + printf("LIS2MDL SPI is busy\n"); } } - LIS3MDL_CS_HIGH(); + LIS2MDL_CS_HIGH(); } /** - * @brief Read a single byte from a LIS3MDL register. + * @brief Read a single byte from a LIS2MDL register. * @param reg: Register address. * @return The value read. */ -uint8_t LIS3MDL_ReadRegister(uint8_t reg) { +uint8_t LIS2MDL_ReadRegister(uint8_t addr) { HAL_StatusTypeDef status; uint8_t txData = reg | 0x80; // MSB 1 for read uint8_t rxData; - LIS3MDL_CS_LOW(); + LIS2MDL_CS_LOW(); status = HAL_SPI_Transmit(&hspi1, &txData, 1, SENSORS_SPI_TIMEOUT); if (status != HAL_OK) { - printf("Error writing to LIS3MDL register 0x%02X: %d\n", reg, status); + printf("Error writing to LIS2MDL register 0x%02X: %d\n", addr, status); if (status == HAL_TIMEOUT) { - printf("LIS3MDL SPI write timeout\n"); + printf("LIS2MDL SPI write timeout\n"); } else if (status == HAL_ERROR) { - printf("LIS3MDL SPI write error\n"); + printf("LIS2MDL SPI write error\n"); } else if (status == HAL_BUSY) { - printf("LIS3MDL SPI is busy\n"); + printf("LIS2MDL SPI is busy\n"); } } status = HAL_SPI_Receive(&hspi1, &rxData, 1, SENSORS_SPI_TIMEOUT); if (status != HAL_OK) { - printf("Error reading from LIS3MDL register 0x%02X: %d\n", reg, status); + printf("Error reading from LIS2MDL register 0x%02X: %d\n", addr, status); if (status == HAL_TIMEOUT) { - printf("LIS3MDL SPI read timeout\n"); + printf("LIS2MDL SPI read timeout\n"); } else if (status == HAL_ERROR) { - printf("LIS3MDL SPI read error\n"); + printf("LIS2MDL SPI read error\n"); } else if (status == HAL_BUSY) { - printf("LIS3MDL SPI is busy\n"); + printf("LIS2MDL SPI is busy\n"); } } - LIS3MDL_CS_HIGH(); + LIS2MDL_CS_HIGH(); return rxData; } /** - * @brief Read multiple bytes starting from a LIS3MDL register. + * @brief Read multiple bytes starting from a LIS2MDL register. * @param reg: Starting register address. * @param buffer: Buffer to store the data. * @param len: Number of bytes to read. */ -void LIS3MDL_ReadRegisters(uint8_t reg, uint8_t *buffer, uint8_t len) { +void LIS2MDL_ReadRegisters(uint8_t addr, uint8_t *buffer, uint8_t len) { HAL_StatusTypeDef status; - uint8_t txData = reg | 0x80; // MSB 1 for read - LIS3MDL_CS_LOW(); + uint8_t txData = addr | 0x80; // MSB 1 for read + LIS2MDL_CS_LOW(); status = HAL_SPI_Transmit(&hspi1, &txData, 1, SENSORS_SPI_TIMEOUT); if (status != HAL_OK) { - printf("Error writing to LIS3MDL register 0x%02X: %d\n", reg, status); + printf("Error writing to LIS2MDL register 0x%02X: %d\n", addr, status); if (status == HAL_TIMEOUT) { - printf("LIS3MDL SPI write timeout\n"); + printf("LIS2MDL SPI write timeout\n"); } else if (status == HAL_ERROR) { - printf("LIS3MDL SPI write error\n"); + printf("LIS2MDL SPI write error\n"); } else if (status == HAL_BUSY) { - printf("LIS3MDL SPI is busy\n"); + printf("LIS2MDL SPI is busy\n"); } } status = HAL_SPI_Receive(&hspi1, buffer, len, SENSORS_SPI_TIMEOUT); if (status != HAL_OK) { - printf("Error reading from LIS3MDL register 0x%02X: %d\n", reg, status); + printf("Error reading from LIS2MDL register 0x%02X: %d\n", addr, status); if (status == HAL_TIMEOUT) { - printf("LIS3MDL SPI read timeout\n"); + printf("LIS2MDL SPI read timeout\n"); } else if (status == HAL_ERROR) { - printf("LIS3MDL SPI read error\n"); + printf("LIS2MDL SPI read error\n"); } else if (status == HAL_BUSY) { - printf("LIS3MDL SPI is busy\n"); + printf("LIS2MDL SPI is busy\n"); } } - LIS3MDL_CS_HIGH(); + LIS2MDL_CS_HIGH(); } -uint8_t Mag_Init() { +// It seems like we only require ODR, performance mode and operation mode. We'll set only these three setting for now +// and if user will need to change other settings, they can write directly to config registers +uint8_t LIS2MDL_Init(LIS2MDL_MAG* mag) { printf("Initializing Magnetometer...\n"); - uint8_t whoAmI = LIS3MDL_ReadRegister(LIS3MDL_WHO_AM_I_ADDR); - if (whoAmI != LIS3MDL_WHO_AM_I_VALUE) { - printf("Initialization error: LIS3MDL not found\n"); + uint8_t whoAmI = LIS2MDL_ReadRegister(LIS2MDL_WHO_AM_I_ADDR); + if (whoAmI != LIS2MDL_WHO_AM_I_VALUE) { + printf("Initialization error: LIS2MDL not found\n"); printf("Initialization Failed: Magnetometer not responding\n"); return 1; // Device not found } + uint8_t val = 0; - LIS3MDL_SETTINGS[0] = LIS3MDL_RANGE_12_GAUSS; - LIS3MDL_SETTINGS[1] = LIS3MDL_DATARATE_10_HZ; - LIS3MDL_SETTINGS[2] = LIS3MDL_PERFORMANCEMODE_HIGH; - LIS3MDL_SETTINGS[3] = LIS3MDL_OPERATIONMODE_CONTINUOUS; - - // Configure CTRL_REG1: Enable temperature sensor, set high-performance mode, ODR=10Hz - LIS3MDL_WriteRegister(LIS3MDL_CTRL_REG1_ADDR, 0x70); - - // Configure CTRL_REG2: Set full scale to ±12 gauss - LIS3MDL_WriteRegister(LIS3MDL_CTRL_REG2_ADDR, 0x20); - - // Configure CTRL_REG3: Set continuous-conversion mode - LIS3MDL_WriteRegister(LIS3MDL_CTRL_REG3_ADDR, 0x00); + // Configure CFG_REG_B: Enable temperature compensation, set high-performance mode, ODR=100Hz, set data mode + val = (1 << 7) | (mag->performance_mode << 4) | (mag->odr << 2) | (mag->data_mode); + LIS2MDL_WriteRegister(LIS2MDL_CFG_REG_A_ADDR, val); - Mag_GetConfig(); + // Configure CFG_REG_C: Disable I2C + val = 1 << 6; + LIS2MDL_WriteRegister(LIS2MDL_CFG_REG_C_ADDR, val); return 0; // Initialization successful } -uint8_t Mag_Read(vector_t *pData) { - uint8_t buffer[6]; - LIS3MDL_ReadRegisters(LIS3MDL_OUT_X_L_ADDR, buffer, 6); +uint8_t LIS2MDL_Read_Mag(LIS2MDL_MAG* mag) { + uint8_t buffer[8]; + HAL_StatusTypeDef status = LIS2MDL_ReadRegisters(LIS2MDL_OUT_X_L_ADDR, buffer, 8); + // Should we return a HAL Status here // Convert raw data to gauss - int16_t x_raw = (int16_t)(buffer[1] << 8 | buffer[0]); - int16_t y_raw = (int16_t)(buffer[3] << 8 | buffer[2]); - int16_t z_raw = (int16_t)(buffer[5] << 8 | buffer[4]); - - float scale = 1.0f; // Scale factor based on range - switch (LIS3MDL_SETTINGS[0]) { - case LIS3MDL_RANGE_4_GAUSS: scale = 6842.0f; break; - case LIS3MDL_RANGE_8_GAUSS: scale = 3421.0f; break; - case LIS3MDL_RANGE_12_GAUSS: scale = 2281.0f; break; - case LIS3MDL_RANGE_16_GAUSS: scale = 1711.0f; break; - } - - pData->x = x_raw / scale; - pData->y = y_raw / scale; - pData->z = z_raw / scale; + mag->x = (float)((buffer[1] << 8 | buffer[0]) * MAG_BIT_TO_MILLIGAUSS); + mag->y = (float)((buffer[3] << 8 | buffer[2]) * MAG_BIT_TO_MILLIGAUSS); + mag->z = (float)((buffer[5] << 8 | buffer[4]) * MAG_BIT_TO_MILLIGAUSS); + mag->temp = (float)((buffer[7] << 8 | buffer[6]) / 8.0f + 25.0f); // Taken from ST Drivers. IDK doesn't really line up with datasheet instructions return 0; // Success } -uint8_t Mag_SetConfig(uint8_t *settings) { - uint8_t ctrl_reg1 = (settings[1] << 1); // Data rate bits - uint8_t ctrl_reg2 = (settings[0] << 5); // Range bits - // uint8_t ctrl_reg3 = (settings[3] << 0); // Operation mode bits - uint8_t ctrl_reg4 = (settings[2] << 2); // Performance mode bits - - // Write configuration to registers - LIS3MDL_WriteRegister(LIS3MDL_CTRL_REG1_ADDR, ctrl_reg1); - LIS3MDL_WriteRegister(LIS3MDL_CTRL_REG2_ADDR, ctrl_reg2); - // LIS3MDL_WriteRegister(self, LIS3MDL_CTRL_REG3_ADDR, ctrl_reg3); - LIS3MDL_WriteRegister(LIS3MDL_CTRL_REG4_ADDR, ctrl_reg4); - // Read back the configuration to verify - if (Mag_GetConfig(LIS3MDL_SETTINGS) != 0 || - LIS3MDL_SETTINGS[0] != settings[0] || - LIS3MDL_SETTINGS[1] != settings[1] || - LIS3MDL_SETTINGS[2] != settings[2]) { - return 1; // Configuration verification failed - } - - return 0; // Success -} +uint8_t LIS2MDL_GetConfig(LIS2MDL_MAG* mag) { + uint8_t cfg_A = LIS2MDL_ReadRegister(LIS2MDL_CFG_REG_A_ADDR); + mag->data_mode = cfg_A & 0b00000011; + mag->odr = cfg_A & 0b00001100; + mag->performance_mode = cfg_A & 0b00010000; -uint8_t Mag_GetConfig() { - uint8_t ctrl_reg1, ctrl_reg2, ctrl_reg4; - // Read configuration registers - ctrl_reg1 = LIS3MDL_ReadRegister(LIS3MDL_CTRL_REG1_ADDR); - ctrl_reg2 = LIS3MDL_ReadRegister(LIS3MDL_CTRL_REG2_ADDR); - ctrl_reg4 = LIS3MDL_ReadRegister(LIS3MDL_CTRL_REG4_ADDR); - // Check if the registers are valid - if (ctrl_reg1 == 0xFF || ctrl_reg2 == 0xFF || ctrl_reg4 == 0xFF) { - return 1; // Error reading registers - } - - // Extract configuration values - LIS3MDL_SETTINGS[0] = (ctrl_reg2 >> 5) & 0x03; // Range bits - LIS3MDL_SETTINGS[1] = (ctrl_reg1 >> 1) & 0x0F; // Data rate bits - LIS3MDL_SETTINGS[2] = (ctrl_reg4 >> 5) & 0x03; // Performance Mode bits - return 0; // Success + return 0; } -uint8_t Mag_Test() { - printf("Starting LIS3MDL Driver Test\n"); - // Initialize LIS3MDL - if (Mag_Init() != 0) { - // Handle error: LIS3MDL not found - printf("Initialization error: LIS3MDL not found\n"); +uint8_t LIS2MDL_Test_Mag(LIS2MDL_MAG* mag) { + printf("Starting LIS2MDL Driver Test\n"); + // Initialize LIS2MDL + if (LIS2MDL_Init(mag) != 0) { + // Handle error: LIS2MDL not found + printf("Initialization error: LIS2MDL not found\n"); return 1; } - printf("LIS3MDL Initialized Successfully \n"); - DelayMs(1000); - vector_t magData; - if (Mag_Read(&magData) != 0) { - printf("Read Error: LIS3MDL Read Failed "); + printf("LIS2MDL Initialized Successfully \n"); + HAL_Delay(1000); // Delay 1 Second + if (LIS2MDL_Read_Mag(mag)) != 0) { + printf("Read Error: LIS2MDL Read Failed "); return 1; } - printf("X: %4.2f Y: %4.2f Z: %4.2f \n", magData.x, magData.y, magData.z); + printf("X: %4.2f Y: %4.2f Z: %4.2f \n", mag->x, mag->y, mag->z); return 0; } + +void LIS2MDL_Reboot_Mag(LIS2MDL_MAG* mag) { + LIS2MDL_WriteRegister(LIS2MDL_CFG_REG_A_ADDR, 1 << 6); + mag->temp = 0; + mag->x = 0; + mag->y = 0; + mag->z = 0; +} diff --git a/mag.h b/mag.h index 5e93c85..a8c07e8 100644 --- a/mag.h +++ b/mag.h @@ -7,7 +7,7 @@ * the necessary structures, function prototypes, and constants * required for the magnetometer driver. * - * @authors Amelia Ellis + * @authors Amelia Ellis, Eric Chiang * @date 2025-04-11 * @version 0.1 * @bug No known bugs. @@ -19,102 +19,104 @@ #include #include #include "stm32h7xx_hal.h" -#include "sensors_defs.h" - - // Register Addresses -#define LIS3MDL_WHO_AM_I_ADDR 0x0F -#define LIS3MDL_CTRL_REG1_ADDR 0x20 -#define LIS3MDL_CTRL_REG2_ADDR 0x21 -#define LIS3MDL_CTRL_REG3_ADDR 0x22 -#define LIS3MDL_CTRL_REG4_ADDR 0x23 -#define LIS3MDL_OUT_X_L_ADDR 0x28 -#define LIS3MDL_OUT_X_H_ADDR 0x29 -#define LIS3MDL_OUT_Y_L_ADDR 0x2A -#define LIS3MDL_OUT_Y_H_ADDR 0x2B -#define LIS3MDL_OUT_Z_L_ADDR 0x2C -#define LIS3MDL_OUT_Z_H_ADDR 0x2D - -// WHO_AM_I Expected Value -#define LIS3MDL_WHO_AM_I_VALUE 0x3D - - - -// Pin Definitions -#define LIS3MDL_CS_PORT GPIOG -#define LIS3MDL_CS_PIN GPIO_PIN_12 -//extern GPIO_TypeDef LIS3MDL_CS_PORT; -//extern uint16_t LIS3MDL_CS_PIN; - - -extern uint8_t LIS3MDL_SETTINGS[8]; +#define LIS2MDL_WHO_AM_I_ADDR 0x4F +#define LIS2MDL_CFG_REG_A_ADDR 0x60 +#define LIS2MDL_CFG_REG_B_ADDR 0x61 +#define LIS2MDL_CFG_REG_C_ADDR 0x62 +#define LIS2MDL_INT_CTRL_ADDR 0x63 +#define LIS2MDL_INT_SRC_ADDR 0x64 +#define LIS2MDL_THIS_L_ADDR 0x65 +#define LIS2MDL_THIS_H_ADDR 0x66 +#define LIS2MDL_STATUS_REG_ADDR 0x67 +#define LIS2MDL_OUT_X_L_ADDR 0x68 +#define LIS2MDL_OUT_X_H_ADDR 0x69 +#define LIS2MDL_OUT_Y_L_ADDR 0x6A +#define LIS2MDL_OUT_Y_H_ADDR 0x6B +#define LIS2MDL_OUT_Z_L_ADDR 0x6C +#define LIS2MDL_OUT_Z_H_ADDR 0x6D +#define LIS2MDL_OUT_TEMP_L_ADDR 0x6E +#define LIS2MDL_OUT_TEMP_H_ADDR 0X6F + +// WHO_AM_I expected value +#define LIS2MDL_WHO_AM_I_VALUE 0x40 + +// SPI Chip Select Pin Configurations +#define LIS2MDL_CS_PORT GPIOG +#define LIS2MDL_CS_PIN GPIO_PIN_12 + +// Constants to convert to gauss +#define MAG_BIT_TO_MILLIGAUSS 1.5 // 1.5 milligauss per bit +#define MAG_BIT_TO_CELSIUS 0.125 // 0.125 C per bit + +// TODO: find out where these setting are defined +/* +0 - Gauss Range +1 - ODR +2 - Performance Mode +3 - Continous/Single +Not sure why there is 8 settings but whatevs +*/ +extern SPI_HandleTypeDef hspi1; // SPI Communication Macros -#define LIS3MDL_CS_LOW() HAL_GPIO_WritePin(LIS3MDL_CS_PORT, LIS3MDL_CS_PIN, GPIO_PIN_RESET) -#define LIS3MDL_CS_HIGH() HAL_GPIO_WritePin(LIS3MDL_CS_PORT, LIS3MDL_CS_PIN, GPIO_PIN_SET) - -/** The magnetometer ranges */ +#define LIS2MDL_CS_LOW() HAL_GPIO_WritePin(LIS2MDL_CS_PORT, LIS2MDL_CS_PIN, GPIO_PIN_RESET) +#define LIS2MDL_CS_HIGH() HAL_GPIO_WritePin(LIS2MDL_CS_PORT, LIS2MDL_CS_PIN, GPIO_PIN_SET) + +// Magnetometer Struct + +typedef struct { + float x; + float y; + float z; + float temp; + lis2mdl_datamode_t data_mode; + lis2mdl_dataRate_t odr; + lis2mdl_performancemode_t performance_mode; +} LIS2MDL_MAG; + +/** The magnetometer data rate for single mode measurement*/ typedef enum { - LIS3MDL_RANGE_4_GAUSS = 0b00, ///< +/- 4g (default value) - LIS3MDL_RANGE_8_GAUSS = 0b01, ///< +/- 8g - LIS3MDL_RANGE_12_GAUSS = 0b10, ///< +/- 12g - LIS3MDL_RANGE_16_GAUSS = 0b11, ///< +/- 16g - } lis3mdl_range_t; - - /** The magnetometer data rate, includes FAST_ODR bit */ - typedef enum { - LIS3MDL_DATARATE_0_625_HZ = 0b0000, ///< 0.625 Hz - LIS3MDL_DATARATE_1_25_HZ = 0b0010, ///< 1.25 Hz - LIS3MDL_DATARATE_2_5_HZ = 0b0100, ///< 2.5 Hz - LIS3MDL_DATARATE_5_HZ = 0b0110, ///< 5 Hz - LIS3MDL_DATARATE_10_HZ = 0b1000, ///< 10 Hz - LIS3MDL_DATARATE_20_HZ = 0b1010, ///< 20 Hz - LIS3MDL_DATARATE_40_HZ = 0b1100, ///< 40 Hz - LIS3MDL_DATARATE_80_HZ = 0b1110, ///< 80 Hz - LIS3MDL_DATARATE_155_HZ = 0b0001, ///< 155 Hz (FAST_ODR + UHP) - LIS3MDL_DATARATE_300_HZ = 0b0011, ///< 300 Hz (FAST_ODR + HP) - LIS3MDL_DATARATE_560_HZ = 0b0101, ///< 560 Hz (FAST_ODR + MP) - LIS3MDL_DATARATE_1000_HZ = 0b0111, ///< 1000 Hz (FAST_ODR + LP) - } lis3mdl_dataRate_t; - - /** The magnetometer performance mode */ - typedef enum { - LIS3MDL_PERFORMANCEMODE_LOWPOWER = 0b00, ///< Low power mode - LIS3MDL_PERFORMANCEMODE_MEDIUM = 0b01, ///< Medium performance mode - LIS3MDL_PERFORMANCEMODE_HIGH = 0b10, ///< High performance mode - LIS3MDL_PERFORMANCEMODE_ULTRAHIGH = 0b11, ///< Ultra-high performance mode - } lis3mdl_performancemode_t; - - /** The magnetometer operation mode */ - typedef enum { - LIS3MDL_OPERATIONMODE_CONTINUOUS = 0b00, ///< Continuous conversion mode - LIS3MDL_OPERATIONMODE_SINGLE = 0b01, ///< Single-shot conversion - LIS3MDL_OPERATIONMODE_POWERDOWN = 0b11, ///< Powered-down - } lis3mdl_operationmode_t; + LIS2MDL_DATARATE_10_HZ = 0b00, ///< 10 Hz + LIS2MDL_DATARATE_20_HZ = 0b01, ///< 20 Hz + LIS2MDL_DATARATE_50_HZ = 0b10, ///< 50 Hz + LIS2MDL_DATARATE_100_HZ = 0b11, ///< 100 Hz +} lis2mdl_dataRate_t; +/** The magnetometer performance mode */ +typedef enum { + LIS2MDL_PERFORMANCEMODE_LOWPOWER = 0b0, ///< Low Power Mode + LIS2MDL_PERFORMANCEMODE_HIGH_RES = 0b1, ///< High Resolution Mode +} lis2mdl_performancemode_t; + /** The magnetometer data mode */ + typedef enum { + LIS2MDL_OPERATIONMODE_CONTINUOUS = 0b00, ///< Continuous conversion mode + LIS2MDL_OPERATIONMODE_SINGLE = 0b01, ///< Single-shot conversion + LIS2MDL_OPERATIONMODE_POWERDOWN = 0b11, ///< Idle mode + } lis2mdl_datamode_t; /** - * @brief Write a single byte to a LIS3MDL register. + * @brief Write a single byte to a LIS2MDL register. * @param reg: Register address. * @param value: Value to write. */ -void LIS3MDL_WriteRegister(uint8_t reg, uint8_t value); +void LIS2MDL_WriteRegister(uint8_t addr, uint8_t value); /** - * @brief Read a single byte from a LIS3MDL register. + * @brief Read a single byte from a LIS2MDL register. * @param reg: Register address. * @return The value read. */ -uint8_t LIS3MDL_ReadRegister(uint8_t reg); +uint8_t LIS2MDL_ReadRegister(uint8_t addr); /** - * @brief Read multiple bytes starting from a LIS3MDL register. + * @brief Read multiple bytes starting from a LIS2MDL register. * @param reg: Starting register address. * @param buffer: Buffer to store the data. * @param length: Number of bytes to read. */ -void LIS3MDL_ReadRegisters(uint8_t reg, uint8_t *buffer, uint8_t len); +void LIS2MDL_ReadRegisters(uint8_t addr, uint8_t *buffer, uint8_t len); /** * @brief Initializes the magnetometer. @@ -125,7 +127,7 @@ void LIS3MDL_ReadRegisters(uint8_t reg, uint8_t *buffer, uint8_t len); * * @return uint8_t Status of the initialization (0 for success, 1 for failure). */ -uint8_t Mag_Init(); +uint8_t LIS2MDL_Init(LIS2MDL_MAG* mag); /** * @brief This function configures the mode, data rate, and range of the magnetometer. @@ -135,19 +137,8 @@ uint8_t Mag_Init(); * @param range The range to set (in Gauss). * @return uint8_t Status of the configuration (0 for success, 1 for failure). */ -uint8_t Mag_SetConfig(uint8_t *settings); -/** - * @brief Gets the current configuration of the magnetometer. - * - * This function retrieves the current mode, data rate, and range of the magnetometer. - * - * @param mode Pointer to store the current mode. - * @param dataRate Pointer to store the current data rate. - * @param range Pointer to store the current range. - * @return uint8_t Status of the configuration retrieval (0 for success, 1 for failure). - */ -uint8_t Mag_GetConfig(); +uint8_t LIS2MDL_GetConfig(LIS2MDL_MAG* mag); /** * @brief Reads data from the magnetometer. @@ -157,8 +148,17 @@ uint8_t Mag_GetConfig(); * @param pData Pointer to store the magnetic field data (x, y, z components). * @return uint8_t Status of the read operation (0 for success, 1 for failure). */ -uint8_t Mag_Read(vector_t *pData); +uint8_t LIS2MDL_Read_Mag(LIS2MDL_MAG* mag); + +/** + * @brief Reboots the magnetometer. + * + * Magnetometer memory is reset. The user config registers are retained. + * + * @return None + */ +void LIS2MDL_Reboot_Mag(LIS2MDL_MAG* mag); uint8_t Mag_Test(); -#endif // MAG_H +#endif // MAG_H \ No newline at end of file diff --git a/sensors_defs.h b/sensors_defs.h deleted file mode 100644 index 390aa25..0000000 --- a/sensors_defs.h +++ /dev/null @@ -1,62 +0,0 @@ -/** - * @file sensors.h - * @brief Header file for sensor initialization and data reading functions. - * - * This file contains the definitions for initializing and reading data from - * various sensors including IMU, Barometer, Magnetometer, Accelerometer, - * and GPS. It also includes function prototypes for temperature sensors. - * - * @authors Amelia Ellis - * @date 2025-04-10 - * @version 0.1 - * @bug No known bugs. - * - * @todo Remove fake_hal.h and replace with actual HAL header. - * @todo Add error handling for sensor initialization and data reading. - */ -#ifndef SENSORS_H -#define SENSORS_H - -#include -#include -#include -#include -#include "stm32h7xx_hal.h" - - -#define SENSORS_MAGFIELD_EARTH_MAX \ - (60.0F) /**< Maximum magnetic field on Earth's surface */ -#define SENSORS_MAGFIELD_EARTH_MIN \ - (30.0F) /**< Minimum magnetic field on Earth's surface */ -#define SENSORS_PRESSURE_SEALEVELHPA \ - (1013.25F) /**< Average sea level pressure is 1013.25 hPa */ -#define SENSORS_DPS_TO_RADS \ - (0.017453293F) /**< Degrees/s to rad/s multiplier */ -#define SENSORS_RADS_TO_DPS \ - (57.29577793F) /**< Rad/s to degrees/s multiplier */ -#define SENSORS_GAUSS_TO_MICROTESLA \ - (100) /**< Gauss to micro-Tesla multiplier */ -#define SENSORS_MICROTESLA_TO_GAUSS \ - (0.01F) /**< Micro-Tesla to Gauss multiplier */ -#define SENSORS_MG_TO_MS2 \ - (0.00980665F) /**< mg to m/s^2 multiplier */ -#define SENSORS_MS2_TO_MG \ - (101.971621F) /**< m/s^2 to mg multiplier */ - -#define SENSORS_GRAVITY_STANDARD (9.80665F) /**< Standard gravity in m/s^2 */ - - -#define SENSORS_SPI_TIMEOUT 2000 /**< SPI timeout in milliseconds */ - -// SPI Handle -extern SPI_HandleTypeDef hspi1; - -typedef union vector3_u { - float v[3]; - struct { - float x, y, z; - }; -} vector_t; - - -#endif // SENSORS_H From d7edc8a5bc5104e84f2730110741c69982cc9455 Mon Sep 17 00:00:00 2001 From: eric Date: Fri, 31 Oct 2025 17:41:55 -0400 Subject: [PATCH 3/4] Working. Have to clarify which STM32 chip HAL to use as well as output format --- MMC5983MA.c | 205 +++++++++++++++++++++++++++++++++++++++++++++++++ MMC5983MA.h | 157 +++++++++++++++++++++++++++++++++++++ MMC5983MC.ioc | 191 +++++++++++++++++++++++++++++++++++++++++++++ README.md | 2 - mag.c | 179 ------------------------------------------ mag.h | 164 --------------------------------------- sensors_defs.h | 58 ++++++++++++++ 7 files changed, 611 insertions(+), 345 deletions(-) create mode 100644 MMC5983MA.c create mode 100644 MMC5983MA.h create mode 100644 MMC5983MC.ioc delete mode 100644 README.md delete mode 100644 mag.c delete mode 100644 mag.h create mode 100644 sensors_defs.h diff --git a/MMC5983MA.c b/MMC5983MA.c new file mode 100644 index 0000000..218c8bc --- /dev/null +++ b/MMC5983MA.c @@ -0,0 +1,205 @@ +/** + * @file mag.c + * @brief Implementation of magnetometer driver + * + * This file contains the implementation of the magnetometer driver + * using the MMC5983MA sensor. It includes functions for initializing, + * configuring, and reading data from the magnetometer. + * + * @authors Eric Chiang + * @date 2025-10-18 + * @version 0.1 + * @bug No known bugs. + */ +#include "MMC5983MC.h" +#include "sensors_defs.h" + +/* + * [0] = Bandwidth + * [1] = Continous Mode Measurement Freq + * [2] = Set Count + */ +uint8_t MMC5983MA_SETTINGS[3]; + +uint8_t MMC5983MA_Init(void) { + HAL_StatusTypeDef status = HAL_OK; + uint8_t whoami = MMC5983MA_ReadRegister(MMC5983MA_WHO_AM_I); + if (whoami != MMC5983MA_WHO_AM_I_VALUE) { + printf("Initialization failed, incorrect whoami value. Expected: %x, Received: %x", MMC5983MA_WHO_AM_I_VALUE, whoami); + return 1; + } + + // Enable Auto_SR_en + uint8_t reg_val = 1 << 1; + MMC5983MA_WriteRegister(MMC5983MA_CTRL0, reg_val); + + // Set bandwidth of sensor to be 100hz (default) (measurement time = 8ms) +// reg_val = 0b00000000; +// MMC5983MA_WriteRegister(MMC5983MA_CTRL1, reg_val); + MMC5983MA_SETTINGS[0] = MMC5983MA_BANDWIDTH_100HZ; + + // Set measurement mode to be continuous @ 100Hz. + // Set auto SET operation of magnetometer/1000 measurement (realigns the mag??) + reg_val = 0b11101101; + MMC5983MA_WriteRegister(MMC5983MA_CTRL2, reg_val); + MMC5983MA_SETTINGS[1] = MMC5983MA_MEASUREMENT_100HZ; + MMC5983MA_SETTINGS[2] = MMC5983MA_SET_1000; + + // Nothing to modify on CTRL3 register + + return 0; +} + +uint8_t MMC5983MA_ReadRegister(uint8_t addr) { + HAL_StatusTypeDef status = HAL_OK; + + // MSB = 1 for read, 5 bit register addresses starts at Bit 2 + uint8_t txData = addr | 0x80; + uint8_t rxData; + + // Set CS Pin Low + status = HAL_SPI_GetState(&hspi1); + HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_12); + HAL_GPIO_WritePin(MMC5983MA_CS_PORT, MMC5983MA_CS_PIN, GPIO_PIN_RESET); + status = HAL_SPI_Transmit(&hspi1, &txData, 1, SENSORS_SPI_TIMEOUT); + if (status != HAL_OK) { + printf("Error writing to MMC5983MA register 0x%02X: %d\n", addr, status); + if (status == HAL_TIMEOUT) { + printf("MMC5983MA SPI write timeout\n"); + } else if (status == HAL_ERROR) { + printf("MMC5983MA SPI write error\n"); + } else if (status == HAL_BUSY) { + printf("MMC5983MA SPI is busy\n"); + } + } + status = HAL_SPI_Receive(&hspi1, &rxData, 1, SENSORS_SPI_TIMEOUT); + if (status != HAL_OK) { + printf("Error reading from MMC5983MA register 0x%02X: %d\n", addr, status); + if (status == HAL_TIMEOUT) { + printf("MMC5983MA SPI read timeout\n"); + } else if (status == HAL_ERROR) { + printf("MMC5983MA SPI read error\n"); + } else if (status == HAL_BUSY) { + printf("MMC5983MA SPI is busy\n"); + } + } + + // Set CS Pin High + HAL_GPIO_WritePin(MMC5983MA_CS_PORT, MMC5983MA_CS_PIN, GPIO_PIN_SET); + + return rxData; +} + +void MMC5983MA_ReadRegisters(uint8_t addr, uint8_t *buffer, uint8_t length) { + HAL_StatusTypeDef status = HAL_OK; + + // MSB = 1 for read, 5 bit register addresses starts at Bit 2 + uint8_t txData = addr | 0x80; + + // Set CS to Low + HAL_GPIO_WritePin(MMC5983MA_CS_PORT, MMC5983MA_CS_PIN, GPIO_PIN_RESET); + status = HAL_SPI_Transmit(&hspi1, &txData, 1, SENSORS_SPI_TIMEOUT); + if (status != HAL_OK) { + printf("Error writing to MMC5983MA register 0x%02X: %d\n", addr, status); + if (status == HAL_TIMEOUT) { + printf("MMC5983MA SPI write timeout\n"); + } else if (status == HAL_ERROR) { + printf("MMC5983MA SPI write error\n"); + } else if (status == HAL_BUSY) { + printf("MMC5983MA SPI is busy\n"); + } + } + + status = HAL_SPI_Receive(&hspi1, buffer, length, SENSORS_SPI_TIMEOUT); + if (status != HAL_OK) { + printf("Error reading from MMC5983MA register 0x%02X: %d\n", addr, status); + if (status == HAL_TIMEOUT) { + printf("MMC5983MA SPI read timeout\n"); + } else if (status == HAL_ERROR) { + printf("MMC5983MA SPI read error\n"); + } else if (status == HAL_BUSY) { + printf("MMC5983MA SPI is busy\n"); + } + } + + // Set CS Pin High + HAL_GPIO_WritePin(MMC5983MA_CS_PORT, MMC5983MA_CS_PIN, GPIO_PIN_SET); +} + +void MMC5983MA_WriteRegister(uint8_t addr, uint8_t value) { + HAL_StatusTypeDef status = HAL_OK; + uint8_t buffer[2] = { addr, value }; + + // Set CS to Low + HAL_GPIO_WritePin(MMC5983MA_CS_PORT, MMC5983MA_CS_PIN, GPIO_PIN_RESET); + status = HAL_SPI_Transmit(&hspi1, buffer, 2, SENSORS_SPI_TIMEOUT); + if (status != HAL_OK) { + printf("Error writing to MMC5983MA register 0x%02X: %d\n", addr, status); + if (status == HAL_TIMEOUT) { + printf("MMC5983MA SPI write timeout\n"); + } else if (status == HAL_ERROR) { + printf("MMC5983MA SPI write error\n"); + } else if (status == HAL_BUSY) { + printf("MMC5983MA SPI is busy\n"); + } + } + + // Set CS to High + HAL_GPIO_WritePin(MMC5983MA_CS_PORT, MMC5983MA_CS_PIN, GPIO_PIN_SET); +} + +void MMC5983MA_ReadMagneticField16(vector_t* mag_data) { + uint8_t buffer[6]; + MMC5983MA_ReadRegisters(MMC5983MA_XOUT_H, buffer, 6); + + uint16_t x_raw = (uint16_t)(buffer[0] << 8 | buffer[1]); + uint16_t y_raw = (uint16_t)(buffer[2] << 8 | buffer[3]); + uint16_t z_raw = (uint16_t)(buffer[4] << 8 | buffer[5]); + + mag_data->v[0] = MMC5983MA_16Bits_to_mGauss(x_raw); + mag_data->v[1] = MMC5983MA_16Bits_to_mGauss(y_raw); + mag_data->v[2] = MMC5983MA_16Bits_to_mGauss(z_raw); +} + +void MMC5983MA_ReadMagneticField18(vector_t* mag_data) { + uint8_t buffer[7]; + MMC5983MA_ReadRegisters(MMC5983MA_XOUT_H, buffer, 7); + + // buffer[7] contains two additional bits of x, y, z mag readings + uint32_t x_raw = (uint32_t)(buffer[0] << 10 | buffer[1] << 2 | buffer[6] >> 6); + uint32_t y_raw = (uint32_t)(buffer[2] << 10 | buffer[3] << 2 | (buffer[6] & 0x30) >> 4); + uint32_t z_raw = (uint32_t)(buffer[4] << 10 | buffer[5] << 2 | (buffer[6] & 0x0C) >> 2); + + mag_data->v[0] = MMC5983MA_18Bits_to_mGauss(x_raw); + mag_data->v[1] = MMC5983MA_18Bits_to_mGauss(y_raw); + mag_data->v[2] = MMC5983MA_18Bits_to_mGauss(z_raw); +} + +float MMC5983MA_Get_Temp(void) { + HAL_StatusTypeDef status = HAL_OK; + uint8_t raw_temp = MMC5983MA_ReadRegister(MMC5983MA_TOUT); + return (float)raw_temp * 0.8 - 75.0; +} + +void MMC5983MA_SW_Reset(void) { + MMC5983MA_WriteRegister(MMC5983MA_CTRL1, 0x80); // SW Reset is the MSB + // TODO: Maybe use a timer instead so we don't block + HAL_Delay(10); // 10ms power-up time +} + +float MMC5983MA_16Bits_to_mGauss(uint16_t mag_val) { + float mag_val_mgauss = (float)mag_val - 65536.0; + mag_val_mgauss /= 65536.0; + mag_val_mgauss *= 8; + return mag_val_mgauss; +} + +float MMC5983MA_18Bits_to_mGauss(uint32_t mag_val) { + float mag_val_mgauss = (float)mag_val - 131072.0; + mag_val_mgauss /= 131072.0; + mag_val_mgauss *= 8; + return mag_val_mgauss; +} + + + diff --git a/MMC5983MA.h b/MMC5983MA.h new file mode 100644 index 0000000..e66fd95 --- /dev/null +++ b/MMC5983MA.h @@ -0,0 +1,157 @@ +/** + * @file mag.h + * @brief Header file for magnetometer driver + * + * This file contains the definitions and function prototypes for + * initializing and reading data from the magnetometer. It includes + * the necessary structures, function prototypes, and constants + * required for the magnetometer driver. + * + * @authors Eric Chiang + * @date 2025-10-18 + * @version 0.1 + * @bug No known bugs. + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef MMC5983MA_DRIVER_H_ +#define MMC5983MA_DRIVER_H_ + +// Includes +#include "stm32g0xx_hal.h" +#include "sensors_defs.h" +#include + +// SPI Handle +extern SPI_HandleTypeDef hspi1; + +// Pin Definitions +#define MMC5983MA_CS_PORT GPIOA +#define MMC5983MA_CS_PIN GPIO_PIN_0 + +// Register Addresses +#define MMC5983MA_WHO_AM_I 0x2F +#define MMC5983MA_XOUT_H 0x00 +#define MMC5983MA_XOUT_L 0x01 +#define MMC5983MA_YOUT_H 0x02 +#define MMC5983MA_YOUT_L 0x03 +#define MMC5983MA_ZOUT_H 0x04 +#define MMC5983MA_ZOUT_L 0x05 +#define MMC5983MA_XYZOUT 0x06 // If in 18 bit mode, this register contains the last two bits of each orientation +#define MMC5983MA_TOUT 0x07 +#define MMC5983MA_STATUS 0x08 +#define MMC5983MA_CTRL0 0x09 +#define MMC5983MA_CTRL1 0x0A +#define MMC5983MA_CTRL2 0x0B +#define MMC5983MA_CTRL3 0x0C + +// WHO_AM_I Expected Value +#define MMC5983MA_WHO_AM_I_VALUE 0x30 + +// Magnetometer Settings +typedef enum { + MMC5983MA_BANDWIDTH_100HZ = 0b00, + MMC5983MA_BANDWIDTH_200HZ = 0b01, + MMC5983MA_BANDWIDTH_400HZ = 0b10, + MMC5983MA_BANDWIDTH_800HZ = 0b11 +} mmc5983ma_bandwith_t; + +typedef enum { + MMC5983MA_MEASUREMENT_OFF = 0b000, + MMC5983MA_MEASUREMENT_1HZ = 0b001, + MMC5983MA_MEASUREMENT_10HZ = 0b010, + MMC5983MA_MEASUREMENT_20HZ = 0b011, + MMC5983MA_MEASUREMENT_50HZ = 0b100, + MMC5983MA_MEASUREMENT_100HZ = 0b101, + MMC5983MA_MEASUREMENT_200HZ = 0b110, + MMC5983MA_MEASUREMENT_1000HZ = 0b111 +} mmc5983ma_measurement_freq_t; + +typedef enum { + MMC5983MA_SET_1 = 0b000, + MMC5983MA_SET_25 = 0b001, + MMC5983MA_SET_75 = 0b010, + MMC5983MA_SET_100 = 0b011, + MMC5983MA_SET_250 = 0b100, + MMC5983MA_SET_500 = 0b101, + MMC5983MA_SET_1000 = 0b110, + MMC5983MA_SET_2000 = 0b111, +} mmc5983ma_set_count_t; + +// Function Prototypes +/** + * @brief Write a single byte to a MMC5983MA register. + * @param addr: Register address to write. + * @param value: Value to write. + */ +void MMC5983MA_WriteRegister(uint8_t addr, uint8_t value); + +/** + * @brief Read a single byte from a MMC5983MA register. + * @param addr: Register addres to read. + * @return The byte value of the register + */ +uint8_t MMC5983MA_ReadRegister(uint8_t addr); + +/** + * @brief Read a single byte from a MMC5983MA register. + * @param addr: Register addres to read. + * @param buffer: The buffer to write returned values to. + * @param length: The length of the input buffer. + */ +void MMC5983MA_ReadRegisters(uint8_t addr, uint8_t *buffer, uint8_t length); + +/** + * @brief Initializes the magnetometer. + * + * This function configures the SPI communication and sets up the GPIO pins + * for the magnetometer. It also performs a self-test to ensure the sensor is + * functioning correctly. + * + * @return Status of the initialization (0 for success, 1 for failure). + */ +uint8_t MMC5983MA_Init(void); + +/** + * @brief Read the output of the magnetometer and stores it into the output data container. + * Resolution of the output is 16 bits/0.25 mG + * @param mag_data: The container for the data + */ +void MMC5983MA_ReadMagneticField16(vector_t* mag_data); + +/** + * @brief Read the output of the magnetometer and stores it into the output data container + * Resolution of the output is 18 bits/0.0625 mG + * @param mag_data: The container for the data + */ +void MMC5983MA_ReadMagneticField18(vector_t* mag_data); + +/** + * @brief Change the functionality of the magnetometer + * @param ctrl_reg: One of four CTRL register addreses for the MMC5983MA + * @param val: The byte to be writtern to the CTRL registers + */ + +float MMC5983MA_16Bits_to_mGauss(uint16_t mag_val); + +/** + * @brief Convert the 18 bit output of the magnetometer to mGauss + * @param ctrl_reg: One of the X, Y, Z magnetometer outputs + * @return The value of the magnetometer output in mGauss + */ +float MMC5983MA_18Bits_to_mGauss(uint32_t mag_val); + + +/** + * @brief Get the die temperature of the MMC5983MA + * @return The temperature in celsius + */ +float MMC5983MA_Get_Temp(void); + +/** + * @brief Software Reset of the magnetometer. Clears all registers and self-tests on reset. + */ +void MMC5983MA_SW_Reset(void); + + +#endif // MMC5983MA_DRIVER_H_ diff --git a/MMC5983MC.ioc b/MMC5983MC.ioc new file mode 100644 index 0000000..d9a6500 --- /dev/null +++ b/MMC5983MC.ioc @@ -0,0 +1,191 @@ +#MicroXplorer Configuration settings - do not modify +CAD.formats= +CAD.pinconfig= +CAD.provider= +File.Version=6 +GPIO.groupedBy=Group By Peripherals +KeepUserPlacement=false +Mcu.CPN=STM32G0B1RET6 +Mcu.Family=STM32G0 +Mcu.IP0=NVIC +Mcu.IP1=RCC +Mcu.IP2=SPI1 +Mcu.IP3=SYS +Mcu.IP4=USART2 +Mcu.IPNb=5 +Mcu.Name=STM32G0B1R(B-C-E)Tx +Mcu.Package=LQFP64 +Mcu.Pin0=PC13 +Mcu.Pin1=PC14-OSC32_IN (PC14) +Mcu.Pin10=PA7 +Mcu.Pin11=PB12 +Mcu.Pin12=PA13 +Mcu.Pin13=PA14-BOOT0 +Mcu.Pin14=VP_SYS_VS_Systick +Mcu.Pin15=VP_SYS_VS_DBSignals +Mcu.Pin2=PC15-OSC32_OUT (PC15) +Mcu.Pin3=PF0-OSC_IN (PF0) +Mcu.Pin4=PA0 +Mcu.Pin5=PA1 +Mcu.Pin6=PA2 +Mcu.Pin7=PA3 +Mcu.Pin8=PA5 +Mcu.Pin9=PA6 +Mcu.PinsNb=16 +Mcu.ThirdPartyNb=0 +Mcu.UserConstants= +Mcu.UserName=STM32G0B1RETx +MxCube.Version=6.15.0 +MxDb.Version=DB.6.0.150 +NVIC.ForceEnableDMAVector=true +NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.SVC_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true +NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false +PA0.GPIOParameters=PinState,GPIO_PuPd,GPIO_Label +PA0.GPIO_Label=SPI1-CS +PA0.GPIO_PuPd=GPIO_NOPULL +PA0.Locked=true +PA0.PinState=GPIO_PIN_SET +PA0.Signal=GPIO_Output +PA1.Mode=Full_Duplex_Master +PA1.Signal=SPI1_SCK +PA13.GPIOParameters=GPIO_Label +PA13.GPIO_Label=TMS +PA13.Locked=true +PA13.Mode=Serial_Wire +PA13.Signal=SYS_SWDIO +PA14-BOOT0.GPIOParameters=GPIO_Label +PA14-BOOT0.GPIO_Label=TCK +PA14-BOOT0.Locked=true +PA14-BOOT0.Mode=Serial_Wire +PA14-BOOT0.Signal=SYS_SWCLK +PA2.GPIOParameters=GPIO_Label +PA2.GPIO_Label=USART2_TX [STLK_TX] +PA2.Locked=true +PA2.Mode=Asynchronous +PA2.Signal=USART2_TX +PA3.GPIOParameters=GPIO_Label +PA3.GPIO_Label=USART2_RX [STLK_RX] +PA3.Locked=true +PA3.Mode=Asynchronous +PA3.Signal=USART2_RX +PA5.GPIOParameters=GPIO_Speed,GPIO_Label +PA5.GPIO_Label=LED_GREEN +PA5.GPIO_Speed=GPIO_SPEED_FREQ_HIGH +PA5.Locked=true +PA5.Signal=GPIO_Output +PA6.Mode=Full_Duplex_Master +PA6.Signal=SPI1_MISO +PA7.Mode=Full_Duplex_Master +PA7.Signal=SPI1_MOSI +PB12.Locked=true +PB12.Signal=GPIO_Output +PC13.GPIOParameters=GPIO_Label +PC13.GPIO_Label=B1 [blue push button] +PC13.Locked=true +PC13.Signal=GPXTI13 +PC14-OSC32_IN\ (PC14).Locked=true +PC14-OSC32_IN\ (PC14).Mode=LSE-External-Oscillator +PC14-OSC32_IN\ (PC14).Signal=RCC_OSC32_IN +PC15-OSC32_OUT\ (PC15).Locked=true +PC15-OSC32_OUT\ (PC15).Mode=LSE-External-Oscillator +PC15-OSC32_OUT\ (PC15).Signal=RCC_OSC32_OUT +PF0-OSC_IN\ (PF0).GPIOParameters=GPIO_Label +PF0-OSC_IN\ (PF0).GPIO_Label=MCO +PF0-OSC_IN\ (PF0).Locked=true +PF0-OSC_IN\ (PF0).Mode=HSE-External-Clock-Source +PF0-OSC_IN\ (PF0).Signal=RCC_OSC_IN +PinOutPanel.RotationAngle=0 +ProjectManager.AskForMigrate=true +ProjectManager.BackupPrevious=false +ProjectManager.CompilerLinker=GCC +ProjectManager.CompilerOptimize=6 +ProjectManager.ComputerToolchain=false +ProjectManager.CoupleFile=false +ProjectManager.CustomerFirmwarePackage= +ProjectManager.DefaultFWLocation=true +ProjectManager.DeletePrevious=true +ProjectManager.DeviceId=STM32G0B1RETx +ProjectManager.FirmwarePackage=STM32Cube FW_G0 V1.6.2 +ProjectManager.FreePins=false +ProjectManager.HalAssertFull=false +ProjectManager.HeapSize=0x200 +ProjectManager.KeepUserCode=true +ProjectManager.LastFirmware=true +ProjectManager.LibraryCopy=1 +ProjectManager.MainLocation=Core/Src +ProjectManager.NoMain=false +ProjectManager.PreviousToolchain= +ProjectManager.ProjectBuild=false +ProjectManager.ProjectFileName=MMC5983MC.ioc +ProjectManager.ProjectName=MMC5983MC +ProjectManager.ProjectStructure= +ProjectManager.RegisterCallBack= +ProjectManager.StackSize=0x400 +ProjectManager.TargetToolchain=STM32CubeIDE +ProjectManager.ToolChainLocation= +ProjectManager.UAScriptAfterPath= +ProjectManager.UAScriptBeforePath= +ProjectManager.UnderRoot=true +ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_USART2_UART_Init-USART2-false-HAL-true,4-MX_SPI1_Init-SPI1-false-HAL-true +RCC.AHBFreq_Value=16000000 +RCC.APBFreq_Value=16000000 +RCC.APBTimFreq_Value=16000000 +RCC.CECFreq_Value=32786.88524590164 +RCC.CortexFreq_Value=16000000 +RCC.EXTERNAL_CLOCK_VALUE=12288000 +RCC.FCLKCortexFreq_Value=16000000 +RCC.FDCANFreq_Value=16000000 +RCC.FamilyName=M +RCC.HCLKFreq_Value=16000000 +RCC.HSE_VALUE=8000000 +RCC.HSI48_VALUE=48000000 +RCC.HSI_VALUE=16000000 +RCC.I2C1Freq_Value=16000000 +RCC.I2C2Freq_Value=16000000 +RCC.I2S1Freq_Value=16000000 +RCC.I2S2Freq_Value=16000000 +RCC.IPParameters=AHBFreq_Value,APBFreq_Value,APBTimFreq_Value,CECFreq_Value,CortexFreq_Value,EXTERNAL_CLOCK_VALUE,FCLKCortexFreq_Value,FDCANFreq_Value,FamilyName,HCLKFreq_Value,HSE_VALUE,HSI48_VALUE,HSI_VALUE,I2C1Freq_Value,I2C2Freq_Value,I2S1Freq_Value,I2S2Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPUART1Freq_Value,LPUART2Freq_Value,LSCOPinFreq_Value,LSI_VALUE,MCO1PinFreq_Value,MCO2PinFreq_Value,PLLPoutputFreq_Value,PLLQoutputFreq_Value,PLLRCLKFreq_Value,PWRFreq_Value,SYSCLKFreq_VALUE,TIM15Freq_Value,TIM1Freq_Value,USART1Freq_Value,USART2Freq_Value,USART3Freq_Value,USBFreq_Value,VCOInputFreq_Value,VCOOutputFreq_Value +RCC.LPTIM1Freq_Value=16000000 +RCC.LPTIM2Freq_Value=16000000 +RCC.LPUART1Freq_Value=16000000 +RCC.LPUART2Freq_Value=16000000 +RCC.LSCOPinFreq_Value=32000 +RCC.LSI_VALUE=32000 +RCC.MCO1PinFreq_Value=16000000 +RCC.MCO2PinFreq_Value=16000000 +RCC.PLLPoutputFreq_Value=64000000 +RCC.PLLQoutputFreq_Value=64000000 +RCC.PLLRCLKFreq_Value=64000000 +RCC.PWRFreq_Value=16000000 +RCC.SYSCLKFreq_VALUE=16000000 +RCC.TIM15Freq_Value=16000000 +RCC.TIM1Freq_Value=16000000 +RCC.USART1Freq_Value=16000000 +RCC.USART2Freq_Value=16000000 +RCC.USART3Freq_Value=16000000 +RCC.USBFreq_Value=48000000 +RCC.VCOInputFreq_Value=16000000 +RCC.VCOOutputFreq_Value=128000000 +SH.GPXTI13.0=GPIO_EXTI13 +SH.GPXTI13.ConfNb=1 +SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_64 +SPI1.CLKPhase=SPI_PHASE_1EDGE +SPI1.CLKPolarity=SPI_POLARITY_LOW +SPI1.CalculateBaudRate=250.0 KBits/s +SPI1.DataSize=SPI_DATASIZE_8BIT +SPI1.Direction=SPI_DIRECTION_2LINES +SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,DataSize,CLKPhase,CLKPolarity,BaudRatePrescaler +SPI1.Mode=SPI_MODE_MASTER +SPI1.VirtualType=VM_MASTER +USART2.IPParameters=VirtualMode-Asynchronous +USART2.VirtualMode-Asynchronous=VM_ASYNC +VP_SYS_VS_DBSignals.Mode=DisableDeadBatterySignals +VP_SYS_VS_DBSignals.Signal=SYS_VS_DBSignals +VP_SYS_VS_Systick.Mode=SysTick +VP_SYS_VS_Systick.Signal=SYS_VS_Systick +board=NUCLEO-G0B1RE +boardIOC=true +isbadioc=false diff --git a/README.md b/README.md deleted file mode 100644 index 1eccce7..0000000 --- a/README.md +++ /dev/null @@ -1,2 +0,0 @@ -# Discovery-Dev -Development repo for Discovery avionics system. diff --git a/mag.c b/mag.c deleted file mode 100644 index f25cbfb..0000000 --- a/mag.c +++ /dev/null @@ -1,179 +0,0 @@ -/** - * @file mag.c - * @brief Implementation of magnetometer driver - * - * This file contains the implementation of the magnetometer driver - * using the LIS2MDL sensor. It includes functions for initializing, - * configuring, and reading data from the magnetometer. - * - * @authors Amelia Ellis, Eric Chiang - * @date 2025-04-11 - * @version 0.1 - * @bug No known bugs. - */ -#include "mag2.h" - -/** - * @brief Write a single byte to a LIS2MDL register. - * @param reg: Register address. - * @param value: Value to write. - */ -void LIS2MDL_WriteRegister(uint8_t addr, uint8_t value) { - HAL_StatusTypeDef status; - uint8_t txData[2] = {addr & 0x7F, value}; // MSB 0 for write - LIS2MDL_CS_LOW(); - status = HAL_SPI_Transmit(&hspi1, txData, sizeof(txData), SENSORS_SPI_TIMEOUT); - if (status != HAL_OK) { - printf("Error writing to LIS2MDL register 0x%02X: %d\n", addr, status); - if (status == HAL_TIMEOUT) { - printf("LIS2MDL SPI write timeout\n"); - } else if (status == HAL_ERROR) { - printf("LIS2MDL SPI write error\n"); - } else if (status == HAL_BUSY) { - printf("LIS2MDL SPI is busy\n"); - } - } - LIS2MDL_CS_HIGH(); -} - -/** - * @brief Read a single byte from a LIS2MDL register. - * @param reg: Register address. - * @return The value read. - */ -uint8_t LIS2MDL_ReadRegister(uint8_t addr) { - HAL_StatusTypeDef status; - uint8_t txData = reg | 0x80; // MSB 1 for read - uint8_t rxData; - LIS2MDL_CS_LOW(); - status = HAL_SPI_Transmit(&hspi1, &txData, 1, SENSORS_SPI_TIMEOUT); - if (status != HAL_OK) { - printf("Error writing to LIS2MDL register 0x%02X: %d\n", addr, status); - if (status == HAL_TIMEOUT) { - printf("LIS2MDL SPI write timeout\n"); - } else if (status == HAL_ERROR) { - printf("LIS2MDL SPI write error\n"); - } else if (status == HAL_BUSY) { - printf("LIS2MDL SPI is busy\n"); - } - } - status = HAL_SPI_Receive(&hspi1, &rxData, 1, SENSORS_SPI_TIMEOUT); - if (status != HAL_OK) { - printf("Error reading from LIS2MDL register 0x%02X: %d\n", addr, status); - if (status == HAL_TIMEOUT) { - printf("LIS2MDL SPI read timeout\n"); - } else if (status == HAL_ERROR) { - printf("LIS2MDL SPI read error\n"); - } else if (status == HAL_BUSY) { - printf("LIS2MDL SPI is busy\n"); - } - } - LIS2MDL_CS_HIGH(); - return rxData; -} - -/** - * @brief Read multiple bytes starting from a LIS2MDL register. - * @param reg: Starting register address. - * @param buffer: Buffer to store the data. - * @param len: Number of bytes to read. - */ -void LIS2MDL_ReadRegisters(uint8_t addr, uint8_t *buffer, uint8_t len) { - HAL_StatusTypeDef status; - uint8_t txData = addr | 0x80; // MSB 1 for read - LIS2MDL_CS_LOW(); - status = HAL_SPI_Transmit(&hspi1, &txData, 1, SENSORS_SPI_TIMEOUT); - if (status != HAL_OK) { - printf("Error writing to LIS2MDL register 0x%02X: %d\n", addr, status); - if (status == HAL_TIMEOUT) { - printf("LIS2MDL SPI write timeout\n"); - } else if (status == HAL_ERROR) { - printf("LIS2MDL SPI write error\n"); - } else if (status == HAL_BUSY) { - printf("LIS2MDL SPI is busy\n"); - } - } - status = HAL_SPI_Receive(&hspi1, buffer, len, SENSORS_SPI_TIMEOUT); - if (status != HAL_OK) { - printf("Error reading from LIS2MDL register 0x%02X: %d\n", addr, status); - if (status == HAL_TIMEOUT) { - printf("LIS2MDL SPI read timeout\n"); - } else if (status == HAL_ERROR) { - printf("LIS2MDL SPI read error\n"); - } else if (status == HAL_BUSY) { - printf("LIS2MDL SPI is busy\n"); - } - } - LIS2MDL_CS_HIGH(); -} - -// It seems like we only require ODR, performance mode and operation mode. We'll set only these three setting for now -// and if user will need to change other settings, they can write directly to config registers -uint8_t LIS2MDL_Init(LIS2MDL_MAG* mag) { - printf("Initializing Magnetometer...\n"); - uint8_t whoAmI = LIS2MDL_ReadRegister(LIS2MDL_WHO_AM_I_ADDR); - if (whoAmI != LIS2MDL_WHO_AM_I_VALUE) { - printf("Initialization error: LIS2MDL not found\n"); - printf("Initialization Failed: Magnetometer not responding\n"); - return 1; // Device not found - } - uint8_t val = 0; - - // Configure CFG_REG_B: Enable temperature compensation, set high-performance mode, ODR=100Hz, set data mode - val = (1 << 7) | (mag->performance_mode << 4) | (mag->odr << 2) | (mag->data_mode); - LIS2MDL_WriteRegister(LIS2MDL_CFG_REG_A_ADDR, val); - - // Configure CFG_REG_C: Disable I2C - val = 1 << 6; - LIS2MDL_WriteRegister(LIS2MDL_CFG_REG_C_ADDR, val); - - return 0; // Initialization successful -} - -uint8_t LIS2MDL_Read_Mag(LIS2MDL_MAG* mag) { - uint8_t buffer[8]; - HAL_StatusTypeDef status = LIS2MDL_ReadRegisters(LIS2MDL_OUT_X_L_ADDR, buffer, 8); - // Should we return a HAL Status here - // Convert raw data to gauss - mag->x = (float)((buffer[1] << 8 | buffer[0]) * MAG_BIT_TO_MILLIGAUSS); - mag->y = (float)((buffer[3] << 8 | buffer[2]) * MAG_BIT_TO_MILLIGAUSS); - mag->z = (float)((buffer[5] << 8 | buffer[4]) * MAG_BIT_TO_MILLIGAUSS); - mag->temp = (float)((buffer[7] << 8 | buffer[6]) / 8.0f + 25.0f); // Taken from ST Drivers. IDK doesn't really line up with datasheet instructions - - return 0; // Success -} - -uint8_t LIS2MDL_GetConfig(LIS2MDL_MAG* mag) { - uint8_t cfg_A = LIS2MDL_ReadRegister(LIS2MDL_CFG_REG_A_ADDR); - mag->data_mode = cfg_A & 0b00000011; - mag->odr = cfg_A & 0b00001100; - mag->performance_mode = cfg_A & 0b00010000; - - return 0; -} - -uint8_t LIS2MDL_Test_Mag(LIS2MDL_MAG* mag) { - printf("Starting LIS2MDL Driver Test\n"); - // Initialize LIS2MDL - if (LIS2MDL_Init(mag) != 0) { - // Handle error: LIS2MDL not found - printf("Initialization error: LIS2MDL not found\n"); - return 1; - } - printf("LIS2MDL Initialized Successfully \n"); - HAL_Delay(1000); // Delay 1 Second - if (LIS2MDL_Read_Mag(mag)) != 0) { - printf("Read Error: LIS2MDL Read Failed "); - return 1; - } - printf("X: %4.2f Y: %4.2f Z: %4.2f \n", mag->x, mag->y, mag->z); - return 0; -} - -void LIS2MDL_Reboot_Mag(LIS2MDL_MAG* mag) { - LIS2MDL_WriteRegister(LIS2MDL_CFG_REG_A_ADDR, 1 << 6); - mag->temp = 0; - mag->x = 0; - mag->y = 0; - mag->z = 0; -} diff --git a/mag.h b/mag.h deleted file mode 100644 index a8c07e8..0000000 --- a/mag.h +++ /dev/null @@ -1,164 +0,0 @@ -/** - * @file mag.h - * @brief Header file for magnetometer driver - * - * This file contains the definitions and function prototypes for - * initializing and reading data from the magnetometer. It includes - * the necessary structures, function prototypes, and constants - * required for the magnetometer driver. - * - * @authors Amelia Ellis, Eric Chiang - * @date 2025-04-11 - * @version 0.1 - * @bug No known bugs. - */ -#ifndef MAG_H -#define MAG_H - -#include -#include -#include -#include "stm32h7xx_hal.h" - -// Register Addresses -#define LIS2MDL_WHO_AM_I_ADDR 0x4F -#define LIS2MDL_CFG_REG_A_ADDR 0x60 -#define LIS2MDL_CFG_REG_B_ADDR 0x61 -#define LIS2MDL_CFG_REG_C_ADDR 0x62 -#define LIS2MDL_INT_CTRL_ADDR 0x63 -#define LIS2MDL_INT_SRC_ADDR 0x64 -#define LIS2MDL_THIS_L_ADDR 0x65 -#define LIS2MDL_THIS_H_ADDR 0x66 -#define LIS2MDL_STATUS_REG_ADDR 0x67 -#define LIS2MDL_OUT_X_L_ADDR 0x68 -#define LIS2MDL_OUT_X_H_ADDR 0x69 -#define LIS2MDL_OUT_Y_L_ADDR 0x6A -#define LIS2MDL_OUT_Y_H_ADDR 0x6B -#define LIS2MDL_OUT_Z_L_ADDR 0x6C -#define LIS2MDL_OUT_Z_H_ADDR 0x6D -#define LIS2MDL_OUT_TEMP_L_ADDR 0x6E -#define LIS2MDL_OUT_TEMP_H_ADDR 0X6F - -// WHO_AM_I expected value -#define LIS2MDL_WHO_AM_I_VALUE 0x40 - -// SPI Chip Select Pin Configurations -#define LIS2MDL_CS_PORT GPIOG -#define LIS2MDL_CS_PIN GPIO_PIN_12 - -// Constants to convert to gauss -#define MAG_BIT_TO_MILLIGAUSS 1.5 // 1.5 milligauss per bit -#define MAG_BIT_TO_CELSIUS 0.125 // 0.125 C per bit - -// TODO: find out where these setting are defined -/* -0 - Gauss Range -1 - ODR -2 - Performance Mode -3 - Continous/Single -Not sure why there is 8 settings but whatevs -*/ -extern SPI_HandleTypeDef hspi1; - -// SPI Communication Macros -#define LIS2MDL_CS_LOW() HAL_GPIO_WritePin(LIS2MDL_CS_PORT, LIS2MDL_CS_PIN, GPIO_PIN_RESET) -#define LIS2MDL_CS_HIGH() HAL_GPIO_WritePin(LIS2MDL_CS_PORT, LIS2MDL_CS_PIN, GPIO_PIN_SET) - -// Magnetometer Struct - -typedef struct { - float x; - float y; - float z; - float temp; - lis2mdl_datamode_t data_mode; - lis2mdl_dataRate_t odr; - lis2mdl_performancemode_t performance_mode; -} LIS2MDL_MAG; - -/** The magnetometer data rate for single mode measurement*/ -typedef enum { - LIS2MDL_DATARATE_10_HZ = 0b00, ///< 10 Hz - LIS2MDL_DATARATE_20_HZ = 0b01, ///< 20 Hz - LIS2MDL_DATARATE_50_HZ = 0b10, ///< 50 Hz - LIS2MDL_DATARATE_100_HZ = 0b11, ///< 100 Hz -} lis2mdl_dataRate_t; - -/** The magnetometer performance mode */ -typedef enum { - LIS2MDL_PERFORMANCEMODE_LOWPOWER = 0b0, ///< Low Power Mode - LIS2MDL_PERFORMANCEMODE_HIGH_RES = 0b1, ///< High Resolution Mode -} lis2mdl_performancemode_t; - - /** The magnetometer data mode */ - typedef enum { - LIS2MDL_OPERATIONMODE_CONTINUOUS = 0b00, ///< Continuous conversion mode - LIS2MDL_OPERATIONMODE_SINGLE = 0b01, ///< Single-shot conversion - LIS2MDL_OPERATIONMODE_POWERDOWN = 0b11, ///< Idle mode - } lis2mdl_datamode_t; - -/** - * @brief Write a single byte to a LIS2MDL register. - * @param reg: Register address. - * @param value: Value to write. - */ -void LIS2MDL_WriteRegister(uint8_t addr, uint8_t value); -/** - * @brief Read a single byte from a LIS2MDL register. - * @param reg: Register address. - * @return The value read. - */ -uint8_t LIS2MDL_ReadRegister(uint8_t addr); - -/** - * @brief Read multiple bytes starting from a LIS2MDL register. - * @param reg: Starting register address. - * @param buffer: Buffer to store the data. - * @param length: Number of bytes to read. - */ -void LIS2MDL_ReadRegisters(uint8_t addr, uint8_t *buffer, uint8_t len); - -/** - * @brief Initializes the magnetometer. - * - * This function configures the SPI communication and sets up the GPIO pins - * for the magnetometer. It also performs a self-test to ensure the sensor is - * functioning correctly. - * - * @return uint8_t Status of the initialization (0 for success, 1 for failure). - */ -uint8_t LIS2MDL_Init(LIS2MDL_MAG* mag); - -/** - * @brief This function configures the mode, data rate, and range of the magnetometer. - * - * @param mode The mode to set (0 for continuous, 1 for single). - * @param dataRate The data rate to set (in Hz). - * @param range The range to set (in Gauss). - * @return uint8_t Status of the configuration (0 for success, 1 for failure). - */ - -uint8_t LIS2MDL_GetConfig(LIS2MDL_MAG* mag); - -/** - * @brief Reads data from the magnetometer. - * - * This function retrieves the magnetic field data from the magnetometer. - * - * @param pData Pointer to store the magnetic field data (x, y, z components). - * @return uint8_t Status of the read operation (0 for success, 1 for failure). - */ -uint8_t LIS2MDL_Read_Mag(LIS2MDL_MAG* mag); - -/** - * @brief Reboots the magnetometer. - * - * Magnetometer memory is reset. The user config registers are retained. - * - * @return None - */ -void LIS2MDL_Reboot_Mag(LIS2MDL_MAG* mag); - -uint8_t Mag_Test(); - -#endif // MAG_H \ No newline at end of file diff --git a/sensors_defs.h b/sensors_defs.h new file mode 100644 index 0000000..57e7633 --- /dev/null +++ b/sensors_defs.h @@ -0,0 +1,58 @@ +/** + * @file sensors.h + * @brief Header file for sensor initialization and data reading functions. + * + * This file contains the definitions for initializing and reading data from + * various sensors including IMU, Barometer, Magnetometer, Accelerometer, + * and GPS. It also includes function prototypes for temperature sensors. + * + * @authors Amelia Ellis + * @date 2025-04-10 + * @version 0.1 + * @bug No known bugs. + * + * @todo Remove fake_hal.h and replace with actual HAL header. + * @todo Add error handling for sensor initialization and data reading. + */ +#ifndef SENSORS_H +#define SENSORS_H + +#include +#include +#include +#include +#include "stm32g0xx_hal.h" + + +#define SENSORS_MAGFIELD_EARTH_MAX \ + (60.0F) /**< Maximum magnetic field on Earth's surface */ +#define SENSORS_MAGFIELD_EARTH_MIN \ + (30.0F) /**< Minimum magnetic field on Earth's surface */ +#define SENSORS_PRESSURE_SEALEVELHPA \ + (1013.25F) /**< Average sea level pressure is 1013.25 hPa */ +#define SENSORS_DPS_TO_RADS \ + (0.017453293F) /**< Degrees/s to rad/s multiplier */ +#define SENSORS_RADS_TO_DPS \ + (57.29577793F) /**< Rad/s to degrees/s multiplier */ +#define SENSORS_GAUSS_TO_MICROTESLA \ + (100) /**< Gauss to micro-Tesla multiplier */ +#define SENSORS_MICROTESLA_TO_GAUSS \ + (0.01F) /**< Micro-Tesla to Gauss multiplier */ +#define SENSORS_MG_TO_MS2 \ + (0.00980665F) /**< mg to m/s^2 multiplier */ +#define SENSORS_MS2_TO_MG \ + (101.971621F) /**< m/s^2 to mg multiplier */ + +#define SENSORS_GRAVITY_STANDARD (9.80665F) /**< Standard gravity in m/s^2 */ + + +#define SENSORS_SPI_TIMEOUT 2000 /**< SPI timeout in milliseconds */ + +// SPI Handle +extern SPI_HandleTypeDef hspi1; + +typedef struct { + float v[3]; // 0 - X, 1 - Y, 2 - Z +} vector_t; + +#endif // SENSORS_H From 44948c832e1c9823bcfdffa46f9903829bad845e Mon Sep 17 00:00:00 2001 From: eric Date: Tue, 11 Nov 2025 19:56:12 -0500 Subject: [PATCH 4/4] Added calibration functionality and cleaned up code. Clarified comments --- MMC5983MA.c | 69 ++++++++++- MMC5983MA.h | 14 +++ main.c | 338 ++++++++++++++++++++++++++++++++++++++++++++++++++++ main.h | 85 +++++++++++++ 4 files changed, 500 insertions(+), 6 deletions(-) create mode 100644 main.c create mode 100644 main.h diff --git a/MMC5983MA.c b/MMC5983MA.c index 218c8bc..135e5a2 100644 --- a/MMC5983MA.c +++ b/MMC5983MA.c @@ -11,7 +11,7 @@ * @version 0.1 * @bug No known bugs. */ -#include "MMC5983MC.h" +#include "MMC5983MA.h" #include "sensors_defs.h" /* @@ -21,6 +21,14 @@ */ uint8_t MMC5983MA_SETTINGS[3]; +// Calibration Values +float hard_iron[3] = {0}; +float soft_iron[3][3] = { + {1,0,0}, + {0,1,0}, + {0,0,1} +}; + uint8_t MMC5983MA_Init(void) { HAL_StatusTypeDef status = HAL_OK; uint8_t whoami = MMC5983MA_ReadRegister(MMC5983MA_WHO_AM_I); @@ -165,7 +173,7 @@ void MMC5983MA_ReadMagneticField18(vector_t* mag_data) { uint8_t buffer[7]; MMC5983MA_ReadRegisters(MMC5983MA_XOUT_H, buffer, 7); - // buffer[7] contains two additional bits of x, y, z mag readings + // buffer[6] contains two additional bits of x, y, z mag readings uint32_t x_raw = (uint32_t)(buffer[0] << 10 | buffer[1] << 2 | buffer[6] >> 6); uint32_t y_raw = (uint32_t)(buffer[2] << 10 | buffer[3] << 2 | (buffer[6] & 0x30) >> 4); uint32_t z_raw = (uint32_t)(buffer[4] << 10 | buffer[5] << 2 | (buffer[6] & 0x0C) >> 2); @@ -190,16 +198,65 @@ void MMC5983MA_SW_Reset(void) { float MMC5983MA_16Bits_to_mGauss(uint16_t mag_val) { float mag_val_mgauss = (float)mag_val - 65536.0; mag_val_mgauss /= 65536.0; - mag_val_mgauss *= 8; - return mag_val_mgauss; + mag_val_mgauss *= 8; // Get full range of +-8 Gauss + return mag_val_mgauss * 1000; // Get milligauss } float MMC5983MA_18Bits_to_mGauss(uint32_t mag_val) { float mag_val_mgauss = (float)mag_val - 131072.0; mag_val_mgauss /= 131072.0; - mag_val_mgauss *= 8; - return mag_val_mgauss; + mag_val_mgauss *= 8; // Get full range of +-8 Gauss + return mag_val_mgauss * 1000; // Get milligauss +} + +/* + * More in depth explanation of this function: + * We are using MotionCal (https://github.com/PaulStoffregen/MotionCal) to calibrate the magnetometer. + * MotionCal reads in sensor data over UART in a very specific format, and then automatically performs + * calibration and outputs calibration values for the magnetometer. + * Required output format for MotionCal: "Raw:{accX},{accY},{accZ},{gyrX},{gyrY},{gyrZ},{magX},{magY},{magZ}\r\n" + * + * Either way, these calibrations need to happen after the magnetometer has been installed onto the rocket system for accurate + * bias values. + */ +void MMC5983MA_Calibrate_MotionCal(vector_t* mag_data) { + char raw_data[] = "Raw:0,0,0,0,0,0,"; // Currently only require calibration for mag values + char new_line[] = "\r\n"; + char comma = ','; + + // Inputted mag values must be in units of milligauss + char x_buffer[8] = ""; + char y_buffer[8] = ""; + char z_buffer[8] = ""; + sprintf(x_buffer, "%d", (int16_t)mag_data->v[0]); + sprintf(y_buffer, "%d", (int16_t)mag_data->v[1]); + sprintf(z_buffer, "%d", (int16_t)mag_data->v[2]); + + HAL_UART_Transmit(&huart2, (uint8_t*)raw_data, sizeof(raw_data)-1, HAL_UART_TIMEOUT_VALUE); + HAL_UART_Transmit(&huart2, (uint8_t*)x_buffer, strlen(x_buffer), HAL_UART_TIMEOUT_VALUE); + HAL_UART_Transmit(&huart2, (uint8_t*)&comma, 1, HAL_UART_TIMEOUT_VALUE); + HAL_UART_Transmit(&huart2, (uint8_t*)y_buffer, strlen(y_buffer), HAL_UART_TIMEOUT_VALUE); + HAL_UART_Transmit(&huart2, (uint8_t*)&comma, 1, HAL_UART_TIMEOUT_VALUE); + HAL_UART_Transmit(&huart2, (uint8_t*)z_buffer, strlen(z_buffer), HAL_UART_TIMEOUT_VALUE); + HAL_UART_Transmit(&huart2, (uint8_t*)new_line, sizeof(new_line), HAL_UART_TIMEOUT_VALUE); } +// Hard iron offsets are biases in the magnetometer that are created by magnetic + // fields emitted by permanent magnets (motors, inductors, etc) +// Soft iron offsets are biases caused by materials near the magnetometer that + // warp/disturb the surrounding magnetic fields +void MMC5983MA_Calibrate_Data(vector_t* mag_data) { + float mag_temp[3] = {0}; + for (uint8_t i = 0; i < 3; i++) { + mag_temp[i] = mag_data->v[i] - hard_iron[i]; + } + + for (uint8_t i = 0; i < 3; i++) { + mag_data->v[i] = + soft_iron[i][0] * mag_temp[0] + + soft_iron[i][1] * mag_temp[1] + + soft_iron[i][2] * mag_temp[2]; + } +} diff --git a/MMC5983MA.h b/MMC5983MA.h index e66fd95..a4aebe8 100644 --- a/MMC5983MA.h +++ b/MMC5983MA.h @@ -21,9 +21,12 @@ #include "stm32g0xx_hal.h" #include "sensors_defs.h" #include +#include +#include // SPI Handle extern SPI_HandleTypeDef hspi1; +extern UART_HandleTypeDef huart2; // Pin Definitions #define MMC5983MA_CS_PORT GPIOA @@ -153,5 +156,16 @@ float MMC5983MA_Get_Temp(void); */ void MMC5983MA_SW_Reset(void); +/** + * @brief Transmits out the x, y, z values of the magnetometer to externally calibrate with MotionCal + * @param mag_data: The output data container for the magnetometer in milligauss + */ +void MMC5983MA_Calibrate_MotionCal(vector_t* mag_data); + +/** + * @brief Applies calibration values to the magnetometer data + * @param mag_data: The output data container for the magnetometer in milligauss + */ +void MMC5983MA_Calibrate_Data(vector_t* mag_data); #endif // MMC5983MA_DRIVER_H_ diff --git a/main.c b/main.c new file mode 100644 index 0000000..6b4a1f7 --- /dev/null +++ b/main.c @@ -0,0 +1,338 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file : main.c + * @brief : Main program body + ****************************************************************************** + * @attention + * + * Copyright (c) 2025 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ +/* Includes ------------------------------------------------------------------*/ +#include +#include "main.h" + +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ +#include +/* USER CODE END Includes */ + +/* Private typedef -----------------------------------------------------------*/ +/* USER CODE BEGIN PTD */ + +/* USER CODE END PTD */ + +/* Private define ------------------------------------------------------------*/ +/* USER CODE BEGIN PD */ +/* USER CODE END PD */ + +/* Private macro -------------------------------------------------------------*/ +/* USER CODE BEGIN PM */ + +/* USER CODE END PM */ + +/* Private variables ---------------------------------------------------------*/ +SPI_HandleTypeDef hspi1; + +UART_HandleTypeDef huart2; + +/* USER CODE BEGIN PV */ + +/* USER CODE END PV */ + +/* Private function prototypes -----------------------------------------------*/ +void SystemClock_Config(void); +static void MX_GPIO_Init(void); +static void MX_USART2_UART_Init(void); +static void MX_SPI1_Init(void); +/* USER CODE BEGIN PFP */ + +/* USER CODE END PFP */ + +/* Private user code ---------------------------------------------------------*/ +/* USER CODE BEGIN 0 */ + +/* USER CODE END 0 */ + +/** + * @brief The application entry point. + * @retval int + */ +int main(void) +{ + + /* USER CODE BEGIN 1 */ + vector_t mag_data = { .v = {0} }; + extern uint8_t MMC5983MA_SETTINGS[3]; + + /* USER CODE END 1 */ + + /* MCU Configuration--------------------------------------------------------*/ + + /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ + HAL_Init(); + + /* USER CODE BEGIN Init */ + + /* USER CODE END Init */ + + /* Configure the system clock */ + SystemClock_Config(); + + /* USER CODE BEGIN SysInit */ + + /* USER CODE END SysInit */ + + /* Initialize all configured peripherals */ + MX_GPIO_Init(); + MX_USART2_UART_Init(); + MX_SPI1_Init(); + /* USER CODE BEGIN 2 */ + MMC5983MA_SW_Reset(); + MMC5983MA_Init(); + + /* USER CODE END 2 */ + + /* Infinite loop */ + /* USER CODE BEGIN WHILE */ + while (1) + { + MMC5983MA_ReadMagneticField18(&mag_data); + printf("x:%f y:%f z:%f", mag_data.v[0], mag_data.v[1], mag_data.v[2]); + HAL_Delay(5); + /* USER CODE END WHILE */ + + /* USER CODE BEGIN 3 */ + } + /* USER CODE END 3 */ +} + +/** + * @brief System Clock Configuration + * @retval None + */ +void SystemClock_Config(void) +{ + RCC_OscInitTypeDef RCC_OscInitStruct = {0}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; + + /** Configure the main internal regulator output voltage + */ + HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1); + + /** Initializes the RCC Oscillators according to the specified parameters + * in the RCC_OscInitTypeDef structure. + */ + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI; + RCC_OscInitStruct.HSIState = RCC_HSI_ON; + RCC_OscInitStruct.HSIDiv = RCC_HSI_DIV1; + RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE; + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) + { + Error_Handler(); + } + + /** Initializes the CPU, AHB and APB buses clocks + */ + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK + |RCC_CLOCKTYPE_PCLK1; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI; + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK) + { + Error_Handler(); + } +} + +/** + * @brief SPI1 Initialization Function + * @param None + * @retval None + */ +static void MX_SPI1_Init(void) +{ + + /* USER CODE BEGIN SPI1_Init 0 */ + + /* USER CODE END SPI1_Init 0 */ + + /* USER CODE BEGIN SPI1_Init 1 */ + + /* USER CODE END SPI1_Init 1 */ + /* SPI1 parameter configuration*/ + hspi1.Instance = SPI1; + hspi1.Init.Mode = SPI_MODE_MASTER; + hspi1.Init.Direction = SPI_DIRECTION_2LINES; + hspi1.Init.DataSize = SPI_DATASIZE_8BIT; + hspi1.Init.CLKPolarity = SPI_POLARITY_LOW; + hspi1.Init.CLKPhase = SPI_PHASE_1EDGE; + hspi1.Init.NSS = SPI_NSS_SOFT; + hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_64; + hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB; + hspi1.Init.TIMode = SPI_TIMODE_DISABLE; + hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; + hspi1.Init.CRCPolynomial = 7; + hspi1.Init.CRCLength = SPI_CRC_LENGTH_DATASIZE; + hspi1.Init.NSSPMode = SPI_NSS_PULSE_ENABLE; + if (HAL_SPI_Init(&hspi1) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN SPI1_Init 2 */ + + /* USER CODE END SPI1_Init 2 */ + +} + +/** + * @brief USART2 Initialization Function + * @param None + * @retval None + */ +static void MX_USART2_UART_Init(void) +{ + + /* USER CODE BEGIN USART2_Init 0 */ + + /* USER CODE END USART2_Init 0 */ + + /* USER CODE BEGIN USART2_Init 1 */ + + /* USER CODE END USART2_Init 1 */ + huart2.Instance = USART2; + huart2.Init.BaudRate = 115200; + huart2.Init.WordLength = UART_WORDLENGTH_8B; + huart2.Init.StopBits = UART_STOPBITS_1; + huart2.Init.Parity = UART_PARITY_NONE; + huart2.Init.Mode = UART_MODE_TX_RX; + huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE; + huart2.Init.OverSampling = UART_OVERSAMPLING_16; + huart2.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE; + huart2.Init.ClockPrescaler = UART_PRESCALER_DIV1; + huart2.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT; + if (HAL_UART_Init(&huart2) != HAL_OK) + { + Error_Handler(); + } + if (HAL_UARTEx_SetTxFifoThreshold(&huart2, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK) + { + Error_Handler(); + } + if (HAL_UARTEx_SetRxFifoThreshold(&huart2, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK) + { + Error_Handler(); + } + if (HAL_UARTEx_DisableFifoMode(&huart2) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN USART2_Init 2 */ + + /* USER CODE END USART2_Init 2 */ + +} + +/** + * @brief GPIO Initialization Function + * @param None + * @retval None + */ +static void MX_GPIO_Init(void) +{ + GPIO_InitTypeDef GPIO_InitStruct = {0}; + /* USER CODE BEGIN MX_GPIO_Init_1 */ + + /* USER CODE END MX_GPIO_Init_1 */ + + /* GPIO Ports Clock Enable */ + __HAL_RCC_GPIOC_CLK_ENABLE(); + __HAL_RCC_GPIOF_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(SPI1_CS_GPIO_Port, SPI1_CS_Pin, GPIO_PIN_SET); + + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin, GPIO_PIN_RESET); + + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET); + + /*Configure GPIO pin : B1_Pin */ + GPIO_InitStruct.Pin = B1_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(B1_GPIO_Port, &GPIO_InitStruct); + + /*Configure GPIO pin : SPI1_CS_Pin */ + GPIO_InitStruct.Pin = SPI1_CS_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + HAL_GPIO_Init(SPI1_CS_GPIO_Port, &GPIO_InitStruct); + + /*Configure GPIO pin : LED_GREEN_Pin */ + GPIO_InitStruct.Pin = LED_GREEN_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + HAL_GPIO_Init(LED_GREEN_GPIO_Port, &GPIO_InitStruct); + + /*Configure GPIO pin : PB12 */ + GPIO_InitStruct.Pin = GPIO_PIN_12; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + /* USER CODE BEGIN MX_GPIO_Init_2 */ + + /* USER CODE END MX_GPIO_Init_2 */ +} + +/* USER CODE BEGIN 4 */ + +/* USER CODE END 4 */ + +/** + * @brief This function is executed in case of error occurrence. + * @retval None + */ +void Error_Handler(void) +{ + /* USER CODE BEGIN Error_Handler_Debug */ + /* User can add his own implementation to report the HAL error return state */ + __disable_irq(); + while (1) + { + } + /* USER CODE END Error_Handler_Debug */ +} +#ifdef USE_FULL_ASSERT +/** + * @brief Reports the name of the source file and the source line number + * where the assert_param error has occurred. + * @param file: pointer to the source file name + * @param line: assert_param error line source number + * @retval None + */ +void assert_failed(uint8_t *file, uint32_t line) +{ + /* USER CODE BEGIN 6 */ + /* User can add his own implementation to report the file name and line number, + ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */ + /* USER CODE END 6 */ +} +#endif /* USE_FULL_ASSERT */ diff --git a/main.h b/main.h new file mode 100644 index 0000000..5ef0cd6 --- /dev/null +++ b/main.h @@ -0,0 +1,85 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file : main.h + * @brief : Header for main.c file. + * This file contains the common defines of the application. + ****************************************************************************** + * @attention + * + * Copyright (c) 2025 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __MAIN_H +#define __MAIN_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32g0xx_hal.h" + +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ + +/* USER CODE END Includes */ + +/* Exported types ------------------------------------------------------------*/ +/* USER CODE BEGIN ET */ + +/* USER CODE END ET */ + +/* Exported constants --------------------------------------------------------*/ +/* USER CODE BEGIN EC */ + +/* USER CODE END EC */ + +/* Exported macro ------------------------------------------------------------*/ +/* USER CODE BEGIN EM */ + +/* USER CODE END EM */ + +/* Exported functions prototypes ---------------------------------------------*/ +void Error_Handler(void); + +/* USER CODE BEGIN EFP */ + +/* USER CODE END EFP */ + +/* Private defines -----------------------------------------------------------*/ +#define B1_Pin GPIO_PIN_13 +#define B1_GPIO_Port GPIOC +#define MCO_Pin GPIO_PIN_0 +#define MCO_GPIO_Port GPIOF +#define SPI1_CS_Pin GPIO_PIN_0 +#define SPI1_CS_GPIO_Port GPIOA +#define USART2_TX_Pin GPIO_PIN_2 +#define USART2_TX_GPIO_Port GPIOA +#define USART2_RX_Pin GPIO_PIN_3 +#define USART2_RX_GPIO_Port GPIOA +#define LED_GREEN_Pin GPIO_PIN_5 +#define LED_GREEN_GPIO_Port GPIOA +#define TMS_Pin GPIO_PIN_13 +#define TMS_GPIO_Port GPIOA +#define TCK_Pin GPIO_PIN_14 +#define TCK_GPIO_Port GPIOA + +/* USER CODE BEGIN Private defines */ + +/* USER CODE END Private defines */ + +#ifdef __cplusplus +} +#endif + +#endif /* __MAIN_H */