Skip to content

Imu driver LSM6DSO32 #1

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Jgerbrandt wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Imu driver LSM6DSO32 #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 1 addition & 2 deletions Components/Flash/FlashTask.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)];
Expand Down
72 changes: 19 additions & 53 deletions Components/Sensors/IMUTask.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
******************************************************************************
*/
Expand Down Expand Up @@ -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 -----------------------------------------------------------------*/

Expand Down Expand Up @@ -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);
Expand All @@ -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<DEFAULT_PROTOCOL_WRITE_BUFFER_SIZE> writeBuffer;
Expand Down Expand Up @@ -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

Expand All @@ -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
Expand All @@ -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

}

/**
Expand All @@ -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;
}
3 changes: 0 additions & 3 deletions Components/Sensors/Inc/Data.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down