From 165170c3433e93d1358e26ba9af340b93267c242 Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Tue, 12 Mar 2024 22:31:53 +0900 Subject: [PATCH] [Spinal][IMU][ICM20948][WIP] create a new interface of icm-20948 (this can be compiled, but stil not work) --- .../mcu_project/boards/stm32H7/Inc/main.h | 2 + .../mcu_project/boards/stm32H7/Src/main.c | 17 +- .../mcu_project/boards/stm32H7/spinal.ioc | 24 +- .../lib/Jsk_Lib/sensors/imu/icm_20948.cpp | 462 ++++++++++++++++++ .../lib/Jsk_Lib/sensors/imu/icm_20948.h | 294 +++++++++++ 5 files changed, 783 insertions(+), 16 deletions(-) create mode 100644 aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/icm_20948.cpp create mode 100644 aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/icm_20948.h diff --git a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Inc/main.h b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Inc/main.h index d8f1012c0..7e5cf72a7 100644 --- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Inc/main.h +++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Inc/main.h @@ -86,6 +86,8 @@ void Error_Handler(void); #define RMII_TXD1_GPIO_Port GPIOB #define IMUCS_Pin GPIO_PIN_6 #define IMUCS_GPIO_Port GPIOB +#define SPI1_CS_Pin GPIO_PIN_7 +#define SPI1_CS_GPIO_Port GPIOB #define BAROCS_Pin GPIO_PIN_1 #define BAROCS_GPIO_Port GPIOE /* USER CODE BEGIN Private defines */ diff --git a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Src/main.c b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Src/main.c index 57db08036..cce6cd82e 100644 --- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Src/main.c +++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Src/main.c @@ -35,7 +35,8 @@ #include #include "flashmemory/flashmemory.h" -#include "sensors/imu/imu_mpu9250.h" +/* #include "sensors/imu/imu_mpu9250.h" */ +#include "sensors/imu/icm_20948.h" #include "sensors/imu/imu_ros_cmd.h" #include "sensors/baro/baro_ms5611.h" #include "sensors/gps/gps_ublox.h" @@ -100,7 +101,8 @@ osMailQId canMsgMailHandle; ros::NodeHandle nh_; /* sensor instances */ -IMUOnboard imu_; +/* IMUOnboard imu_; */ +ICM20948 imu_; Baro baro_; GPS gps_; BatteryStatus battery_status_; @@ -228,7 +230,8 @@ int main(void) FlashMemory::read(); #endif - imu_.init(&hspi1, &hi2c3, &nh_, IMUCS_GPIO_Port, IMUCS_Pin, LED0_GPIO_Port, LED0_Pin); + /* imu_.init(&hspi1, &hi2c3, &nh_, IMUCS_GPIO_Port, IMUCS_Pin, LED0_GPIO_Port, LED0_Pin); */ + imu_.init(&hspi1, &hi2c3, &nh_, SPI1_CS_GPIO_Port, SPI1_CS_Pin, LED0_GPIO_Port, LED0_Pin); IMU_ROS_CMD::init(&nh_); IMU_ROS_CMD::addImu(&imu_); baro_.init(&hi2c1, &nh_, BAROCS_GPIO_Port, BAROCS_Pin); @@ -981,7 +984,7 @@ static void MX_GPIO_Init(void) HAL_GPIO_WritePin(GPIOE, LED0_Pin|LED1_Pin|LED2_Pin|BAROCS_Pin, GPIO_PIN_RESET); /*Configure GPIO pin Output Level */ - HAL_GPIO_WritePin(IMUCS_GPIO_Port, IMUCS_Pin, GPIO_PIN_RESET); + HAL_GPIO_WritePin(GPIOB, IMUCS_Pin|SPI1_CS_Pin, GPIO_PIN_RESET); /*Configure GPIO pins : LED0_Pin LED1_Pin LED2_Pin */ GPIO_InitStruct.Pin = LED0_Pin|LED1_Pin|LED2_Pin; @@ -990,12 +993,12 @@ static void MX_GPIO_Init(void) GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM; HAL_GPIO_Init(GPIOE, &GPIO_InitStruct); - /*Configure GPIO pin : IMUCS_Pin */ - GPIO_InitStruct.Pin = IMUCS_Pin; + /*Configure GPIO pins : IMUCS_Pin SPI1_CS_Pin */ + GPIO_InitStruct.Pin = IMUCS_Pin|SPI1_CS_Pin; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM; - HAL_GPIO_Init(IMUCS_GPIO_Port, &GPIO_InitStruct); + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /*Configure GPIO pin : BAROCS_Pin */ GPIO_InitStruct.Pin = BAROCS_Pin; diff --git a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/spinal.ioc b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/spinal.ioc index cc192e53b..0db3bd168 100644 --- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/spinal.ioc +++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/spinal.ioc @@ -211,21 +211,22 @@ Mcu.Pin35=PB3 (JTDO/TRACESWO) Mcu.Pin36=PB4 (NJTRST) Mcu.Pin37=PB5 Mcu.Pin38=PB6 -Mcu.Pin39=PB8 +Mcu.Pin39=PB7 Mcu.Pin4=PH1-OSC_OUT (PH1) -Mcu.Pin40=PB9 -Mcu.Pin41=PE1 -Mcu.Pin42=VP_FREERTOS_VS_CMSIS_V1 -Mcu.Pin43=VP_LWIP_VS_Enabled -Mcu.Pin44=VP_SYS_VS_tim12 -Mcu.Pin45=VP_TIM1_VS_ClockSourceINT -Mcu.Pin46=VP_TIM4_VS_ClockSourceINT +Mcu.Pin40=PB8 +Mcu.Pin41=PB9 +Mcu.Pin42=PE1 +Mcu.Pin43=VP_FREERTOS_VS_CMSIS_V1 +Mcu.Pin44=VP_LWIP_VS_Enabled +Mcu.Pin45=VP_SYS_VS_tim12 +Mcu.Pin46=VP_TIM1_VS_ClockSourceINT +Mcu.Pin47=VP_TIM4_VS_ClockSourceINT Mcu.Pin5=PC1 Mcu.Pin6=PA1 Mcu.Pin7=PA2 Mcu.Pin8=PA3 Mcu.Pin9=PA4 -Mcu.PinsNb=47 +Mcu.PinsNb=48 Mcu.ThirdPartyNb=0 Mcu.UserConstants= Mcu.UserName=STM32H743VITx @@ -328,6 +329,11 @@ PB6.GPIO_Label=IMUCS PB6.GPIO_Speed=GPIO_SPEED_FREQ_MEDIUM PB6.Locked=true PB6.Signal=GPIO_Output +PB7.GPIOParameters=GPIO_Speed,GPIO_Label +PB7.GPIO_Label=SPI1_CS +PB7.GPIO_Speed=GPIO_SPEED_FREQ_MEDIUM +PB7.Locked=true +PB7.Signal=GPIO_Output PB8.Locked=true PB8.Mode=I2C PB8.Signal=I2C1_SCL diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/icm_20948.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/icm_20948.cpp new file mode 100644 index 000000000..325b5af1a --- /dev/null +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/icm_20948.cpp @@ -0,0 +1,462 @@ +/* +****************************************************************************** +* File Name : icm_20948.cpp +* Description : IMU(ICM20948) Interface +* Author : J.Sugihara 2024/3/11 +****************************************************************************** +*/ + +#ifndef __cplusplus +#error "Please define __cplusplus, because this is a c++ based file " +#endif + +#include "sensors/imu/icm_20948.h" + +void ICM20948::init(SPI_HandleTypeDef* hspi, I2C_HandleTypeDef* hi2c, ros::NodeHandle* nh, + GPIO_TypeDef* spi_cs_port, uint16_t spi_cs_pin, + GPIO_TypeDef* led_port, uint16_t led_pin) +{ + nh_ = nh; + IMU::init(); + + spi_cs_port_ = spi_cs_port; + spi_cs_pin_ = spi_cs_pin; + led_port_ = led_port; + led_pin_ = led_pin; + + hspi_ = hspi; + hi2c_ = hi2c; + + calib_indicator_time_ = HAL_GetTick(); + + gyroInit(); + accInit(); + magInit(); + +} + +void ICM20948::gyroInit(void) +{ + HAL_Delay(100); + uint32_t search_start_time = HAL_GetTick(); + + /* Waiting for finding Imu */ + while(!getIcm20948WhoAmI()){ + // uint32_t search_curr_time = HAL_GetTick(); + // HAL_Delay(100); + // if(search_curr_time - search_curr_time > IMU_FIND_TIMEOUT){ + // nh_->logerror("IMU is not found"); + // HAL_NVIC_SystemReset(); + // } + } + + /* 1.Clear all bits in ub0-ub3 register */ + deviceReset(); + + /* 2.Wakeup icm20948 */ + wakeUp(); + + /* 3.Set fundamental properties */ + setClockSource(1); // Clock source is automatically selected. + odrAlignEnable(); // Synchronize odr with sampling rates. + spiModeEnable(); // Use only SPI mode + + /* 4.Gyro initialization */ + setGyroFullScale(_2000dps); + +} + +void ICM20948::accInit (void) +{ + /* 5.Acc initialization */ + setAccelFullScale(_8g); +} + +void ICM20948::magInit(void) +{ + /* 6.1 Check whether use external magnetometer */ + mag_id_ = IMUOnboard::checkExternalMag(); + + if(mag_id_ == INTERNAL_MAG) + { + /* 6.2 Enable communication between ICM20948 and AK09916*/ + // ICM20948 can get magnetometer value from AK09916 via I2C using AUX_CL and AUX_DA. Then, we configure ICM20948 as a master device and AK09916 as a slave device. + + /* 6.2.1 Reset I2C bus*/ + i2cMasterReset(); + /* 6.2.2 Enable I2C bus*/ + i2cMasterEnable(); + /* 6.2.3 Set communication freq of I2C */ + i2cMasterClkFrq(7); // 345.6 kHz / 46.67% dyty cycle (recoomended) + + /* Waiting for finding Magnetometer */ + while(!getAk09916WhoAmI()){ + uint32_t search_curr_time = HAL_GetTick(); + if(search_curr_time - search_curr_time > MAG_FIND_TIMEOUT){ + nh_->logerror("Magnetometer is not found"); + HAL_NVIC_SystemReset(); + } + } + + /* 6.3 Clear all bits in mag register */ + magSoftReset(); + /* 6.4 Wakeup and set operation mode to magnetometer */ + setMagOperationMode(continuous_measurement_100hz); + } +} + +void ICM20948::updateRawData() +{ + gyroRead(&raw_gyro_adc_); + accelRead(&raw_acc_adc_); + magRead(&raw_mag_adc_); +} + +void ICM20948::gyroRead(Vector3f* data) +{ + uint8_t* temp = readMultipleIcm20948(ub_0, B0_GYRO_XOUT_H, 6); + + data->x = (int16_t)(temp[0] << 8 | temp[1]); + data->y = (int16_t)(temp[2] << 8 | temp[3]); + data->z = (int16_t)(temp[4] << 8 | temp[5]); +} + +void ICM20948::accelRead(Vector3f* data) +{ + uint8_t* temp = readMultipleIcm20948(ub_0, B0_ACCEL_XOUT_H, 6); + + data->x = (int16_t)(temp[0] << 8 | temp[1]); + data->y = (int16_t)(temp[2] << 8 | temp[3]); + data->z = (int16_t)(temp[4] << 8 | temp[5]) + accel_scale_factor_; + // Add scale factor because calibraiton function offset gravity acceleration. +} + +bool ICM20948::magRead(Vector3f* data) +{ + uint8_t* temp; + uint8_t drdy, hofl; // data ready, overflow + + drdy = readSingleAk09916(MAG_ST1) & 0x01; + if(!drdy) return false; + + temp = readMultipleAk09916(MAG_HXL, 6); + + hofl = readSingleAk09916(MAG_ST2) & 0x08; + if(hofl) return false; + + data->x = (int16_t)(temp[1] << 8 | temp[0]); + data->y = (int16_t)(temp[3] << 8 | temp[2]); + data->z = (int16_t)(temp[5] << 8 | temp[4]); + + return true; +} + +void ICM20948::gyroReadDps(Vector3f* data) +{ + gyroRead(data); + + data->x /= gyro_scale_factor_; + data->y /= gyro_scale_factor_; + data->z /= gyro_scale_factor_; +} + +void ICM20948::accelReadG(Vector3f* data) +{ + accelRead(data); + + data->x /= accel_scale_factor_; + data->y /= accel_scale_factor_; + data->z /= accel_scale_factor_; +} + +bool ICM20948::magReadUT(Vector3f* data) +{ + Vector3f temp; + bool new_data = magRead(&temp); + if(!new_data) return false; + + data->x = (float)(temp.x * 0.15); + data->y = (float)(temp.y * 0.15); + data->z = (float)(temp.z * 0.15); + + return true; +} + + +/* Sub Functions */ +bool ICM20948::getIcm20948WhoAmI() +{ + uint8_t icm20948_id = readSingleIcm20948(ub_0, B0_WHO_AM_I); + + if(icm20948_id == ICM20948_ID) + return true; + else + return false; +} + +bool ICM20948::getAk09916WhoAmI() +{ + uint8_t ak09916_id = readSingleAk09916(MAG_WIA2); + + if(ak09916_id == AK09916_ID) + return true; + else + return false; +} + +void ICM20948::deviceReset() +{ + writeSingleIcm20948(ub_0, B0_PWR_MGMT_1, 0x80 | 0x41); + HAL_Delay(100); +} + +void ICM20948::magSoftReset() +{ + writeSingleAk09916(MAG_CNTL3, 0x01); + HAL_Delay(100); +} + +void ICM20948::wakeUp() +{ + uint8_t new_val = readSingleIcm20948(ub_0, B0_PWR_MGMT_1); + new_val &= 0xBF; + + writeSingleIcm20948(ub_0, B0_PWR_MGMT_1, new_val); + HAL_Delay(100); +} + +void ICM20948::sleep() +{ + uint8_t new_val = readSingleIcm20948(ub_0, B0_PWR_MGMT_1); + new_val |= 0x40; + + writeSingleIcm20948(ub_0, B0_PWR_MGMT_1, new_val); + HAL_Delay(100); +} + +void ICM20948::spiModeEnable() +{ + uint8_t new_val = readSingleIcm20948(ub_0, B0_USER_CTRL); + new_val |= 0x10; + + writeSingleIcm20948(ub_0, B0_USER_CTRL, new_val); +} + +void ICM20948::i2cMasterReset() +{ + uint8_t new_val = readSingleIcm20948(ub_0, B0_USER_CTRL); + new_val |= 0x02; + + writeSingleIcm20948(ub_0, B0_USER_CTRL, new_val); +} + +void ICM20948::i2cMasterEnable() +{ + uint8_t new_val = readSingleIcm20948(ub_0, B0_USER_CTRL); + new_val |= 0x20; + + writeSingleIcm20948(ub_0, B0_USER_CTRL, new_val); + HAL_Delay(100); +} + +void ICM20948::i2cMasterClkFrq(uint8_t config) +{ + uint8_t new_val = readSingleIcm20948(ub_3, B3_I2C_MST_CTRL); + new_val |= config; + + writeSingleIcm20948(ub_3, B3_I2C_MST_CTRL, new_val); +} + +void ICM20948::setClockSource(uint8_t source) +{ + uint8_t new_val = readSingleIcm20948(ub_0, B0_PWR_MGMT_1); + new_val |= source; + + writeSingleIcm20948(ub_0, B0_PWR_MGMT_1, new_val); +} + +void ICM20948::odrAlignEnable() +{ + writeSingleIcm20948(ub_2, B2_ODR_ALIGN_EN, 0x01); +} + +void ICM20948::setGyroLpf(uint8_t config) +{ + uint8_t new_val = readSingleIcm20948(ub_2, B2_GYRO_CONFIG_1); + new_val |= config << 3; + + writeSingleIcm20948(ub_2, B2_GYRO_CONFIG_1, new_val); +} + +void ICM20948::setAccelLpf(uint8_t config) +{ + uint8_t new_val = readSingleIcm20948(ub_2, B2_ACCEL_CONFIG); + new_val |= config << 3; + + writeSingleIcm20948(ub_2, B2_GYRO_CONFIG_1, new_val); +} + +void ICM20948::setGyroSampleRate(uint8_t divider) +{ + writeSingleIcm20948(ub_2, B2_GYRO_SMPLRT_DIV, divider); +} + +void ICM20948::setAccelSampleRate(uint16_t divider) +{ + uint8_t divider_1 = (uint8_t)(divider >> 8); + uint8_t divider_2 = (uint8_t)(0x0F & divider); + + writeSingleIcm20948(ub_2, B2_ACCEL_SMPLRT_DIV_1, divider_1); + writeSingleIcm20948(ub_2, B2_ACCEL_SMPLRT_DIV_2, divider_2); +} + +void ICM20948::setMagOperationMode(operation_mode mode) +{ + writeSingleAk09916(MAG_CNTL2, mode); + HAL_Delay(100); +} + + +void ICM20948::setGyroFullScale(gyro_full_scale full_scale) +{ + uint8_t new_val = readSingleIcm20948(ub_2, B2_GYRO_CONFIG_1); + + switch(full_scale) + { + case _250dps : + new_val |= 0x00; + gyro_scale_factor_ = 131.0; + break; + case _500dps : + new_val |= 0x02; + gyro_scale_factor_ = 65.5; + break; + case _1000dps : + new_val |= 0x04; + gyro_scale_factor_ = 32.8; + break; + case _2000dps : + new_val |= 0x06; + gyro_scale_factor_ = 16.4; + break; + } + + writeSingleIcm20948(ub_2, B2_GYRO_CONFIG_1, new_val); +} + +void ICM20948::setAccelFullScale(accel_full_scale full_scale) +{ + uint8_t new_val = readSingleIcm20948(ub_2, B2_ACCEL_CONFIG); + + switch(full_scale) + { + case _2g : + new_val |= 0x00; + accel_scale_factor_ = 16384; + break; + case _4g : + new_val |= 0x02; + accel_scale_factor_ = 8192; + break; + case _8g : + new_val |= 0x04; + accel_scale_factor_ = 4096; + break; + case _16g : + new_val |= 0x06; + accel_scale_factor_ = 2048; + break; + } + + writeSingleIcm20948(ub_2, B2_ACCEL_CONFIG, new_val); +} + +void ICM20948::selectUserBank(userbank ub) +{ + uint8_t write_reg[2]; + write_reg[0] = WRITE | REG_BANK_SEL; + write_reg[1] = ub; + + GPIO_L(spi_cs_port_, spi_cs_pin_); + HAL_SPI_Transmit(hspi_, write_reg, 2, 10); + GPIO_H(spi_cs_port_, spi_cs_pin_); +} + +uint8_t ICM20948::readSingleIcm20948(userbank ub, uint8_t reg) +{ + uint8_t read_reg = READ | reg; + uint8_t reg_val; + selectUserBank(ub); + GPIO_L(spi_cs_port_, spi_cs_pin_); + HAL_SPI_Transmit(hspi_, &read_reg, 1, 1000); + HAL_SPI_Receive(hspi_, ®_val, 1, 1000); + GPIO_H(spi_cs_port_, spi_cs_pin_); + + return reg_val; +} + +void ICM20948::writeSingleIcm20948(userbank ub, uint8_t reg, uint8_t val) +{ + uint8_t write_reg[2]; + write_reg[0] = WRITE | reg; + write_reg[1] = val; + + selectUserBank(ub); + + GPIO_L(spi_cs_port_, spi_cs_pin_); + HAL_SPI_Transmit(hspi_, write_reg, 2, 1000); + GPIO_H(spi_cs_port_, spi_cs_pin_); +} + +uint8_t* ICM20948::readMultipleIcm20948(userbank ub, uint8_t reg, uint8_t len) +{ + uint8_t read_reg = READ | reg; + static uint8_t reg_val[6]; + selectUserBank(ub); + + GPIO_L(spi_cs_port_, spi_cs_pin_); + HAL_SPI_Transmit(hspi_, &read_reg, 1, 1000); + HAL_SPI_Receive(hspi_, reg_val, len, 1000); + GPIO_H(spi_cs_port_, spi_cs_pin_); + + return reg_val; +} + +void ICM20948::writeMultipleIcm20948(userbank ub, uint8_t reg, uint8_t* val, uint8_t len) +{ + uint8_t write_reg = WRITE | reg; + selectUserBank(ub); + + GPIO_L(spi_cs_port_, spi_cs_pin_); + HAL_SPI_Transmit(hspi_, &write_reg, 1, 1000); + HAL_SPI_Transmit(hspi_, val, len, 1000); + GPIO_H(spi_cs_port_, spi_cs_pin_); +} + +uint8_t ICM20948::readSingleAk09916(uint8_t reg) +{ + writeSingleIcm20948(ub_3, B3_I2C_SLV0_ADDR, READ | MAG_SLAVE_ADDR); + writeSingleIcm20948(ub_3, B3_I2C_SLV0_REG, reg); + writeSingleIcm20948(ub_3, B3_I2C_SLV0_CTRL, 0x81); + + HAL_Delay(1); + return readSingleIcm20948(ub_0, B0_EXT_SLV_SENS_DATA_00); +} + +void ICM20948::writeSingleAk09916(uint8_t reg, uint8_t val) +{ + writeSingleIcm20948(ub_3, B3_I2C_SLV0_ADDR, WRITE | MAG_SLAVE_ADDR); + writeSingleIcm20948(ub_3, B3_I2C_SLV0_REG, reg); + writeSingleIcm20948(ub_3, B3_I2C_SLV0_DO, val); + writeSingleIcm20948(ub_3, B3_I2C_SLV0_CTRL, 0x81); +} + +uint8_t* ICM20948::readMultipleAk09916(uint8_t reg, uint8_t len) +{ + writeSingleIcm20948(ub_3, B3_I2C_SLV0_ADDR, READ | MAG_SLAVE_ADDR); + writeSingleIcm20948(ub_3, B3_I2C_SLV0_REG, reg); + writeSingleIcm20948(ub_3, B3_I2C_SLV0_CTRL, 0x80 | len); + + HAL_Delay(1); + return readMultipleIcm20948(ub_0, B0_EXT_SLV_SENS_DATA_00, len); +} diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/icm_20948.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/icm_20948.h new file mode 100644 index 000000000..21100e5a0 --- /dev/null +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/icm_20948.h @@ -0,0 +1,294 @@ +/* +****************************************************************************** +* File Name : icm_mpu9250.h +* Description : IMU(ICM20948) Interface +* Author : J.Sugihara 2024/3/11 +****************************************************************************** +*/ + +#ifndef __cplusplus +#error "Please define __cplusplus, because this is a c++ based file " +#endif + +#ifndef __IMU_ICM_H +#define __IMU_ICM_H + +#include "config.h" +#include "imu_mpu9250.h" + + +#define SENSOR_DATA_LENGTH 7 +#define IMU_FIND_TIMEOUT 10000 +#define MAG_FIND_TIMEOUT 10000 + +/* ICM-20948 Registers */ +#define ICM20948_ID 0xEA +#define REG_BANK_SEL 0x7F + +// USER BANK 0 +#define B0_WHO_AM_I 0x00 +#define B0_USER_CTRL 0x03 +#define B0_LP_CONFIG 0x05 +#define B0_PWR_MGMT_1 0x06 +#define B0_PWR_MGMT_2 0x07 +#define B0_INT_PIN_CFG 0x0F +#define B0_INT_ENABLE 0x10 +#define B0_INT_ENABLE_1 0x11 +#define B0_INT_ENABLE_2 0x12 +#define B0_INT_ENABLE_3 0x13 +#define B0_I2C_MST_STATUS 0x17 +#define B0_INT_STATUS 0x19 +#define B0_INT_STATUS_1 0x1A +#define B0_INT_STATUS_2 0x1B +#define B0_INT_STATUS_3 0x1C +#define B0_DELAY_TIMEH 0x28 +#define B0_DELAY_TIMEL 0x29 +#define B0_ACCEL_XOUT_H 0x2D +#define B0_ACCEL_XOUT_L 0x2E +#define B0_ACCEL_YOUT_H 0x2F +#define B0_ACCEL_YOUT_L 0x30 +#define B0_ACCEL_ZOUT_H 0x31 +#define B0_ACCEL_ZOUT_L 0x32 +#define B0_GYRO_XOUT_H 0x33 +#define B0_GYRO_XOUT_L 0x34 +#define B0_GYRO_YOUT_H 0x35 +#define B0_GYRO_YOUT_L 0x36 +#define B0_GYRO_ZOUT_H 0x37 +#define B0_GYRO_ZOUT_L 0x38 +#define B0_TEMP_OUT_H 0x39 +#define B0_TEMP_OUT_L 0x3A +#define B0_EXT_SLV_SENS_DATA_00 0x3B +#define B0_EXT_SLV_SENS_DATA_01 0x3C +#define B0_EXT_SLV_SENS_DATA_02 0x3D +#define B0_EXT_SLV_SENS_DATA_03 0x3E +#define B0_EXT_SLV_SENS_DATA_04 0x3F +#define B0_EXT_SLV_SENS_DATA_05 0x40 +#define B0_EXT_SLV_SENS_DATA_06 0x41 +#define B0_EXT_SLV_SENS_DATA_07 0x42 +#define B0_EXT_SLV_SENS_DATA_08 0x43 +#define B0_EXT_SLV_SENS_DATA_09 0x44 +#define B0_EXT_SLV_SENS_DATA_10 0x45 +#define B0_EXT_SLV_SENS_DATA_11 0x46 +#define B0_EXT_SLV_SENS_DATA_12 0x47 +#define B0_EXT_SLV_SENS_DATA_13 0x48 +#define B0_EXT_SLV_SENS_DATA_14 0x49 +#define B0_EXT_SLV_SENS_DATA_15 0x4A +#define B0_EXT_SLV_SENS_DATA_16 0x4B +#define B0_EXT_SLV_SENS_DATA_17 0x4C +#define B0_EXT_SLV_SENS_DATA_18 0x4D +#define B0_EXT_SLV_SENS_DATA_19 0x4E +#define B0_EXT_SLV_SENS_DATA_20 0x4F +#define B0_EXT_SLV_SENS_DATA_21 0x50 +#define B0_EXT_SLV_SENS_DATA_22 0x51 +#define B0_EXT_SLV_SENS_DATA_23 0x52 +#define B0_FIFO_EN_1 0x66 +#define B0_FIFO_EN_2 0x67 +#define B0_FIFO_RST 0x68 +#define B0_FIFO_MODE 0x69 +#define B0_FIFO_COUNTH 0X70 +#define B0_FIFO_COUNTL 0X71 +#define B0_FIFO_R_W 0x72 +#define B0_DATA_RDY_STATUS 0x74 +#define B0_FIFO_CFG 0x76 + +// USER BANK 1 +#define B1_SELF_TEST_X_GYRO 0x02 +#define B1_SELF_TEST_Y_GYRO 0x03 +#define B1_SELF_TEST_Z_GYRO 0x04 +#define B1_SELF_TEST_X_ACCEL 0x0E +#define B1_SELF_TEST_Y_ACCEL 0x0F +#define B1_SELF_TEST_Z_ACCEL 0x10 +#define B1_XA_OFFS_H 0x14 +#define B1_XA_OFFS_L 0x15 +#define B1_YA_OFFS_H 0x17 +#define B1_YA_OFFS_L 0x18 +#define B1_ZA_OFFS_H 0x1A +#define B1_ZA_OFFS_L 0x1B +#define B1_TIMEBASE_CORRECTION_PLL 0x28 + +// USER BANK 2 +#define B2_GYRO_SMPLRT_DIV 0x00 +#define B2_GYRO_CONFIG_1 0x01 +#define B2_GYRO_CONFIG_2 0x02 +#define B2_XG_OFFS_USRH 0x03 +#define B2_XG_OFFS_USRL 0x04 +#define B2_YG_OFFS_USRH 0x05 +#define B2_YG_OFFS_USRL 0x06 +#define B2_ZG_OFFS_USRH 0x07 +#define B2_ZG_OFFS_USRL 0x08 +#define B2_ODR_ALIGN_EN 0x09 +#define B2_ACCEL_SMPLRT_DIV_1 0x10 +#define B2_ACCEL_SMPLRT_DIV_2 0x11 +#define B2_ACCEL_INTEL_CTRL 0x12 +#define B2_ACCEL_WOM_THR 0x13 +#define B2_ACCEL_CONFIG 0x14 +#define B2_ACCEL_CONFIG_2 0x15 +#define B2_FSYNC_CONFIG 0x52 +#define B2_TEMP_CONFIG 0x53 +#define B2_MOD_CTRL_USR 0X54 + +// USER BANK 3 +#define B3_I2C_MST_ODR_CONFIG 0x00 +#define B3_I2C_MST_CTRL 0x01 +#define B3_I2C_MST_DELAY_CTRL 0x02 +#define B3_I2C_SLV0_ADDR 0x03 +#define B3_I2C_SLV0_REG 0x04 +#define B3_I2C_SLV0_CTRL 0x05 +#define B3_I2C_SLV0_DO 0x06 +#define B3_I2C_SLV1_ADDR 0x07 +#define B3_I2C_SLV1_REG 0x08 +#define B3_I2C_SLV1_CTRL 0x09 +#define B3_I2C_SLV1_DO 0x0A +#define B3_I2C_SLV2_ADDR 0x0B +#define B3_I2C_SLV2_REG 0x0C +#define B3_I2C_SLV2_CTRL 0x0D +#define B3_I2C_SLV2_DO 0x0E +#define B3_I2C_SLV3_ADDR 0x0F +#define B3_I2C_SLV3_REG 0x10 +#define B3_I2C_SLV3_CTRL 0x11 +#define B3_I2C_SLV3_DO 0x12 +#define B3_I2C_SLV4_ADDR 0x13 +#define B3_I2C_SLV4_REG 0x14 +#define B3_I2C_SLV4_CTRL 0x15 +#define B3_I2C_SLV4_DO 0x16 +#define B3_I2C_SLV4_DI 0x17 + + +/* AK09916 Registers */ +#define AK09916_ID 0x09 +#define MAG_SLAVE_ADDR 0x0C + +#define MAG_WIA2 0x01 +#define MAG_ST1 0x10 +#define MAG_HXL 0x11 +#define MAG_HXH 0x12 +#define MAG_HYL 0x13 +#define MAG_HYH 0x14 +#define MAG_HZL 0x15 +#define MAG_HZH 0x16 +#define MAG_ST2 0x18 +#define MAG_CNTL2 0x31 +#define MAG_CNTL3 0x32 +#define MAG_TS1 0x33 +#define MAG_TS2 0x34 + +/* Intructions */ +#define READ 0x80 +#define WRITE 0x00 + +/* Typedefs */ +typedef enum + { + ub_0 = 0 << 4, + ub_1 = 1 << 4, + ub_2 = 2 << 4, + ub_3 = 3 << 4 + } userbank; + +typedef enum + { + _250dps, + _500dps, + _1000dps, + _2000dps + } gyro_full_scale; + +typedef enum + { + _2g, + _4g, + _8g, + _16g + } accel_full_scale; + +typedef enum + { + /* + You can find this informatoin at https://www.y-ic.es/datasheet/78/SMDSW.020-2OZ.pdf + */ + power_down_mode = 0, + single_measurement_mode = 1, + continuous_measurement_10hz = 2, + continuous_measurement_20hz = 4, + continuous_measurement_50hz = 6, + continuous_measurement_100hz = 8 + } operation_mode; + +class ICM20948 : public IMUOnboard { +public: + ICM20948():IMUOnboard(){} + ~ICM20948(){} + void init(SPI_HandleTypeDef* hspi, I2C_HandleTypeDef* hi2c, ros::NodeHandle* nh, + GPIO_TypeDef* spi_cs_port, uint16_t spi_cs_pin, + GPIO_TypeDef* led_port, uint16_t led_pin); + + enum{INTERNAL_MAG = 0, LIS3MDL = 1, HMC58X3 = 2,}; + +private: + float gyro_scale_factor_; + float accel_scale_factor_; + + /* Initialization */ + void gyroInit(void) override; + void accInit(void) override; + void magInit(void) override; + + /* Update */ + void updateRawData (void) override; + + /* Raw 16 bits adc data reading function */ + void gyroRead(Vector3f* data); + void accelRead(Vector3f* data); + bool magRead(Vector3f* data); + + /* Conversion from raw adc data to proper units */ + void gyroReadDps(Vector3f* data); + void accelReadG(Vector3f* data); + bool magReadUT(Vector3f* data); + + + /* Sub functions */ + bool getIcm20948WhoAmI(); + bool getAk09916WhoAmI(); + + void deviceReset(); + void magSoftReset(); + + void wakeUp(); + void sleep(); + + void spiModeEnable(); + + void i2cMasterReset(); + void i2cMasterEnable(); + void i2cMasterClkFrq(uint8_t config); // 0 - 15 + + void setClockSource(uint8_t source); + void odrAlignEnable(); + + void setGyroLpf(uint8_t config); // 0 - 7 + void setAccelLpf(uint8_t config); // 0 - 7 + + void setGyroSampleRate(uint8_t divider); + void setAccelSampleRate(uint16_t divider); + void setMagOperationMode(operation_mode mode); + + void setGyroFullScale(gyro_full_scale full_scale); + void setAccelFullScale(accel_full_scale full_scale); + + /* Fundamental functions */ + void selectUserBank(userbank ub); + + uint8_t readSingleIcm20948(userbank ub, uint8_t reg); + void writeSingleIcm20948(userbank ub, uint8_t reg, uint8_t val); + + uint8_t* readMultipleIcm20948(userbank ub, uint8_t reg, uint8_t len); + void writeMultipleIcm20948(userbank ub, uint8_t reg, uint8_t* val, uint8_t len); + + uint8_t readSingleAk09916(uint8_t reg); + void writeSingleAk09916(uint8_t reg, uint8_t val); + + uint8_t* readMultipleAk09916(uint8_t reg, uint8_t len); + +}; +#endif