From 25247ca970b7729681f0fcd2f57f48be4826900f Mon Sep 17 00:00:00 2001 From: elizabethszent Date: Sun, 8 Jun 2025 12:24:22 -0600 Subject: [PATCH] Initial commit of LSM6DSO32 IMU integration and related modules --- Components/Flash/FlashTask.cpp | 3 +- Components/Sensors/IMUTask.cpp | 72 +++++++++------------------------- Components/Sensors/Inc/Data.h | 3 -- 3 files changed, 20 insertions(+), 58 deletions(-) diff --git a/Components/Flash/FlashTask.cpp b/Components/Flash/FlashTask.cpp index 9ce4e828..ba86cb73 100644 --- a/Components/Flash/FlashTask.cpp +++ b/Components/Flash/FlashTask.cpp @@ -168,8 +168,7 @@ bool FlashTask::ReadLogDataFromFlash() AccelGyroMagnetismData* IMURead = (AccelGyroMagnetismData*)dataRead; SOAR_PRINT("%03d %08d %04d %04d %04d %04d %04d %04d %04d %04d %04d\n", length, IMURead->time, IMURead->accelX_, IMURead->accelY_, IMURead->accelZ_, - IMURead->gyroX_, IMURead->gyroY_, IMURead->gyroZ_, IMURead->magnetoX_, - IMURead->magnetoY_, IMURead->magnetoZ_); + IMURead->gyroX_, IMURead->gyroY_, IMURead->gyroZ_); } else if (length == sizeof(BarometerData)) { uint8_t dataRead[sizeof(BarometerData)]; diff --git a/Components/Sensors/IMUTask.cpp b/Components/Sensors/IMUTask.cpp index bbeae3df..63cd00fb 100644 --- a/Components/Sensors/IMUTask.cpp +++ b/Components/Sensors/IMUTask.cpp @@ -6,7 +6,7 @@ * Andromeda_V3.31_Legacy/Core/Src/ReadAccelGyroMagnetism.c * * Description : This file contains constants and functions designed to - * obtain accurate pressure and temperature readings from + * obtain accurate acceleration and angular rate readings from * the IMU on the flight board. ****************************************************************************** */ @@ -34,41 +34,36 @@ constexpr int CMD_TIMEOUT = 150; #define READ_CMD_MASK 0x80 #define WRITE_CMD_MASK 0x00 #define ACCEL_GYRO_MASK 0x00 -#define MAGNETO_MASK 0x40 // Register addresses -#define G1_CTRL_REGISTER_ADDR 0x10 // CTRL_REG1_G (10h) -#define XL6_CTRL_REGISTER_ADDR 0x20 // CTRL_REG6_XL (20h) -#define M3_CTRL_REGISTER_ADDR 0x22 // CTRL_REG3_M (22h) +//#define G1_CTRL_REGISTER_ADDR 0x10 // CTRL_REG1_G (10h) +//#define XL6_CTRL_REGISTER_ADDR 0x20 // CTRL_REG6_XL (20h) +#define CTRL1_XL_ADDR 0x10 // LSM6DSO32: Controls accelerometer +#define CTRL2_G_ADDR 0x11 // LSM6DSO32: Controls gyroscope +//#define M3_CTRL_REGISTER_ADDR 0x22 // CTRL_REG3_M (22h) #define WHOAMI_REGISTER_ADDR 0x0F // WHO_AM_I_A/G (Accel/Gyro - expected value is 104) -#define WHOAMIM_REGISTER_ADDR 0x0F // WHO_AM_I_M (Magnetometer - expected value is 61) #define GYRO_X_G_LOW_REGISTER_ADDR 0x18 #define ACCEL_X_LOW_REGISTER_ADDR 0x28 -#define MAGNETO_X_LOW_REGISTER_ADDR 0x28 #define ACCEL_SENSITIVITY 0.732 // Unit is mg/LSB #define GYRO_SENSITIVITY 8.75 // Unit is mdps/LSB -#define MAGENTO_SENSITIVITY 0.14 // Unit is mgauss/LSB // Full Commands -static uint8_t ACTIVATE_GYRO_ACCEL_CMD = G1_CTRL_REGISTER_ADDR | WRITE_CMD_MASK; +static uint8_t ACTIVATE_GYRO_CMD = CTRL2_G_ADDR | WRITE_CMD_MASK; // 011 00 0 00 -> ODR 119, 245 DPS -static uint8_t ACTIVATE_GYRO_ACCEL_DATA = 0x60; // ODR 119 has cutoff of 38Hz +static uint8_t ACTIVATE_GYRO_DATA = 0x4C; // 104 Hz, 245 dps -static uint8_t SET_ACCEL_SCALE_CMD = XL6_CTRL_REGISTER_ADDR | WRITE_CMD_MASK; -// 011 01 0 00 -> ODR 119, +/- 16G -static uint8_t SET_ACCEL_SCALE_DATA = 0x68; +static uint8_t ACTIVATE_ACCEL_CMD = CTRL1_XL_ADDR | WRITE_CMD_MASK; +static uint8_t ACTIVATE_ACCEL_DATA = 0x48; // 104 Hz, ±4g -static uint8_t ACTIVATE_MAGNETO_CMD = M3_CTRL_REGISTER_ADDR | WRITE_CMD_MASK; +//static uint8_t ACTIVATE_MAGNETO_CMD = M3_CTRL_REGISTER_ADDR | WRITE_CMD_MASK; // 1 0 0 00 0 00 -> I2C Disable, Low power mode disabled, SPI write enable, Continuous-conversion mode -static uint8_t ACTIVATE_MAGNETO_DATA = 0x80; +//static uint8_t ACTIVATE_MAGNETO_DATA = 0x80; static uint8_t READ_GYRO_X_G_LOW_CMD = GYRO_X_G_LOW_REGISTER_ADDR | READ_CMD_MASK | ACCEL_GYRO_MASK; static uint8_t READ_ACCEL_X_LOW_CMD = ACCEL_X_LOW_REGISTER_ADDR | READ_CMD_MASK | ACCEL_GYRO_MASK; -static uint8_t READ_MAGNETO_X_LOW_CMD = MAGNETO_X_LOW_REGISTER_ADDR | READ_CMD_MASK | MAGNETO_MASK; static uint8_t READ_WHOAMI_CMD = WHOAMI_REGISTER_ADDR | READ_CMD_MASK | ACCEL_GYRO_MASK; -static uint8_t READ_WHOAMIM_CMD = WHOAMIM_REGISTER_ADDR | READ_CMD_MASK | MAGNETO_MASK; /* Variables -----------------------------------------------------------------*/ @@ -176,7 +171,6 @@ void IMUTask::HandleRequestCommand(uint16_t taskCommand) SOAR_PRINT("\t-- IMU Data --\n"); SOAR_PRINT(" Accel (x,y,z) : (%d, %d, %d) milli-Gs\n", data->accelX_, data->accelY_, data->accelZ_); SOAR_PRINT(" Gyro (x,y,z) : (%d, %d, %d) milli-deg/s\n", data->gyroX_, data->gyroY_, data->gyroZ_); - SOAR_PRINT(" Mag (x,y,z) : (%d, %d, %d) milli-gauss\n", data->magnetoX_, data->magnetoY_, data->magnetoZ_); break; default: SOAR_PRINT("IMUTask - Received Unsupported REQUEST_COMMAND {%d}\n", taskCommand); @@ -202,9 +196,6 @@ void IMUTask::TransmitProtocolData() imuData.set_gyro_x(data->gyroX_); imuData.set_gyro_y(data->gyroY_); imuData.set_gyro_z(data->gyroZ_); - imuData.set_mag_x(data->magnetoX_); - imuData.set_mag_y(data->magnetoY_); - imuData.set_mag_z(data->magnetoZ_); msg.set_imu(imuData); EmbeddedProto::WriteBufferFixedSize writeBuffer; @@ -233,7 +224,6 @@ void IMUTask::SampleIMU() uint8_t dataBuffer[6]; int16_t accelX, accelY, accelZ; int16_t gyroX, gyroY, gyroZ; - int16_t magnetoX, magnetoY, magnetoZ; data->time = TICKS_TO_MS(xTaskGetTickCount()); // ms @@ -254,13 +244,6 @@ void IMUTask::SampleIMU() accelY = (dataBuffer[3] << 8) | (dataBuffer[2]); accelZ = (dataBuffer[5] << 8) | (dataBuffer[4]); - HAL_GPIO_WritePin(IMU_MAG_CS_GPIO_Port, IMU_MAG_CS_Pin, GPIO_PIN_RESET); - HAL_SPI_Transmit(SystemHandles::SPI_IMU, &READ_MAGNETO_X_LOW_CMD, 1, CMD_TIMEOUT); - HAL_SPI_Receive(SystemHandles::SPI_IMU, &dataBuffer[0], 6, CMD_TIMEOUT); - HAL_GPIO_WritePin(IMU_MAG_CS_GPIO_Port, IMU_MAG_CS_Pin, GPIO_PIN_SET); - magnetoX = (dataBuffer[1] << 8) | (dataBuffer[0]); - magnetoY = (dataBuffer[3] << 8) | (dataBuffer[2]); - magnetoZ = (dataBuffer[5] << 8) | (dataBuffer[4]); // Write to storage data->accelX_ = accelX * ACCEL_SENSITIVITY; // mg @@ -269,9 +252,7 @@ void IMUTask::SampleIMU() data->gyroX_ = gyroX * GYRO_SENSITIVITY; // mdps data->gyroY_ = gyroY * GYRO_SENSITIVITY; // mdps data->gyroZ_ = gyroZ * GYRO_SENSITIVITY; // mdps - data->magnetoX_ = magnetoX * MAGENTO_SENSITIVITY; // mgauss - data->magnetoY_ = magnetoY * MAGENTO_SENSITIVITY; // mgauss - data->magnetoZ_ = magnetoZ * MAGENTO_SENSITIVITY; // mgauss + } /** @@ -286,41 +267,26 @@ uint8_t IMUTask::SetupIMU() HAL_GPIO_WritePin(IMU_XL_GY_CS_GPIO_Port, IMU_XL_GY_CS_Pin, GPIO_PIN_SET); HAL_GPIO_WritePin(IMU_XL_GY_CS_GPIO_Port, IMU_XL_GY_CS_Pin, GPIO_PIN_RESET); - HAL_SPI_Transmit(SystemHandles::SPI_IMU, &ACTIVATE_GYRO_ACCEL_CMD, 1, CMD_TIMEOUT); - HAL_SPI_Transmit(SystemHandles::SPI_IMU, &ACTIVATE_GYRO_ACCEL_DATA, 1, CMD_TIMEOUT); + HAL_SPI_Transmit(SystemHandles::SPI_IMU, &ACTIVATE_GYRO_CMD, 1, CMD_TIMEOUT); + HAL_SPI_Transmit(SystemHandles::SPI_IMU, &ACTIVATE_GYRO_DATA, 1, CMD_TIMEOUT); HAL_GPIO_WritePin(IMU_XL_GY_CS_GPIO_Port, IMU_XL_GY_CS_Pin, GPIO_PIN_SET); HAL_GPIO_WritePin(IMU_XL_GY_CS_GPIO_Port, IMU_XL_GY_CS_Pin, GPIO_PIN_RESET); - HAL_SPI_Transmit(SystemHandles::SPI_IMU, &SET_ACCEL_SCALE_CMD, 1, CMD_TIMEOUT); - HAL_SPI_Transmit(SystemHandles::SPI_IMU, &SET_ACCEL_SCALE_DATA, 1, CMD_TIMEOUT); + HAL_SPI_Transmit(SystemHandles::SPI_IMU, &ACTIVATE_ACCEL_CMD, 1, CMD_TIMEOUT); + HAL_SPI_Transmit(SystemHandles::SPI_IMU, &ACTIVATE_ACCEL_DATA, 1, CMD_TIMEOUT); HAL_GPIO_WritePin(IMU_XL_GY_CS_GPIO_Port, IMU_XL_GY_CS_Pin, GPIO_PIN_SET); - /* Read WHO AM I register for verification, should read 104. */ + /* Read WHO AM I register for verification, should read 10. */ uint8_t whoami = 0xDE; HAL_GPIO_WritePin(IMU_XL_GY_CS_GPIO_Port, IMU_XL_GY_CS_Pin, GPIO_PIN_RESET); HAL_SPI_Transmit(SystemHandles::SPI_IMU, &READ_WHOAMI_CMD, 1, CMD_TIMEOUT); HAL_SPI_Receive(SystemHandles::SPI_IMU, &whoami, 1, CMD_TIMEOUT); HAL_GPIO_WritePin(IMU_XL_GY_CS_GPIO_Port, IMU_XL_GY_CS_Pin, GPIO_PIN_SET); - /* Setup the Magnetometer */ - HAL_GPIO_WritePin(IMU_MAG_CS_GPIO_Port, IMU_MAG_CS_Pin, GPIO_PIN_RESET); - HAL_SPI_Transmit(SystemHandles::SPI_IMU, &ACTIVATE_MAGNETO_CMD, 1, CMD_TIMEOUT); - HAL_SPI_Transmit(SystemHandles::SPI_IMU, &ACTIVATE_MAGNETO_DATA, 1, CMD_TIMEOUT); - HAL_GPIO_WritePin(IMU_MAG_CS_GPIO_Port, IMU_MAG_CS_Pin, GPIO_PIN_SET); - - /* Read WHO AM I MAG register for verification, should read 61. */ - uint8_t whoami_m = 0xDE; - HAL_GPIO_WritePin(IMU_MAG_CS_GPIO_Port, IMU_MAG_CS_Pin, GPIO_PIN_RESET); - HAL_SPI_Transmit(SystemHandles::SPI_IMU, &READ_WHOAMIM_CMD, 1, CMD_TIMEOUT); - HAL_SPI_Receive(SystemHandles::SPI_IMU, &whoami_m, 1, CMD_TIMEOUT); - HAL_GPIO_WritePin(IMU_MAG_CS_GPIO_Port, IMU_MAG_CS_Pin, GPIO_PIN_SET); /* Verify the two WHOAMI registers */ - if(whoami != 104) + if(whoami != 108) SOAR_PRINT("Non-Fatal-Warning: IMU WHOAMI failed [%d]\n", whoami); - if (whoami_m != 61) - SOAR_PRINT("Non-Fatal-Warning: IMU WHOAMI_M failed [%d]\n", whoami_m); - return whoami; } diff --git a/Components/Sensors/Inc/Data.h b/Components/Sensors/Inc/Data.h index 11259d81..fb6eb276 100644 --- a/Components/Sensors/Inc/Data.h +++ b/Components/Sensors/Inc/Data.h @@ -22,9 +22,6 @@ typedef struct int32_t gyroX_; int32_t gyroY_; int32_t gyroZ_; - int32_t magnetoX_; - int32_t magnetoY_; - int32_t magnetoZ_; int32_t time; } AccelGyroMagnetismData;