From 3eac22f0b3dcf2a94d3ee7e5bfb03da46f92ae29 Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Tue, 12 Mar 2024 22:29:56 +0900 Subject: [PATCH 01/21] [Spinal][IMU][MPU9250] modified sevral parts to make it easier to inherit --- .../lib/Jsk_Lib/sensors/imu/imu_mpu9250.cpp | 90 ++++++++++--------- .../lib/Jsk_Lib/sensors/imu/imu_mpu9250.h | 46 +++++----- 2 files changed, 70 insertions(+), 66 deletions(-) diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_mpu9250.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_mpu9250.cpp index d3f71a83e..b772aa6f5 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_mpu9250.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_mpu9250.cpp @@ -105,52 +105,11 @@ void IMUOnboard::accInit (void) void IMUOnboard::magInit(void) { /* check whether use external magnetometer */ - /* - * we need check the i2c connectio with external mag(HMC5883L) several times initially, - * since the module UBlox M8N has some problem with the gps/mag simultaneous polling. - */ - - HAL_Delay(4000); // wait for the setup of the Blox M8N module, especially for winter - mag_id_ = INTERNAL_MAG; - uint8_t val[2]; - int i2c_status = 1; - - for(int i = 0; i < EXTERNAL_MAG_CHECK_COUNT; i ++) - { - /* check the existence of external magnetometer */ - - /* 1. LIS3MDL */ - val[0] = LIS3MDL_PING; - i2c_status = HAL_I2C_Master_Transmit(hi2c_, LIS3MDL_MAG_REGISTER, val, 1, 100); - HAL_I2C_Master_Receive(hi2c_, LIS3MDL_MAG_REGISTER, val, 1, 100); - - if(i2c_status == HAL_OK && val[0] == 61) //0b111101 - { - mag_id_ = LIS3MDL; - HAL_Delay(10); - lis3mdlInit(); - break; - } - - /* 2. HMC58X3 */ - val[0] = HMC58X3_R_CONFB; - val[1] = 0x20; - i2c_status = HAL_I2C_Master_Transmit(hi2c_, HMC58X3_MAG_REGISTER, val, 2, 100); - - if(i2c_status == HAL_OK) - { - mag_id_ = HMC58X3; - HAL_Delay(10); - hmc58x3Init(); - break; - } - HAL_Delay(10); - } - + mag_id_ = checkExternalMag(); if(mag_id_ == INTERNAL_MAG) - {/* use internal mag */ - + { + /* use internal mag */ HAL_Delay(10); //at this stage, the MAG is configured via the original MAG init function in I2C bypass mode //now we configure MPU as a I2C Master device to handle the MAG via the I2C AUX port (done here for HMC5883) @@ -189,6 +148,49 @@ void IMUOnboard::magInit(void) last_mag_time_ = HAL_GetTick(); } +uint8_t IMUOnboard::checkExternalMag(void) +{ + HAL_Delay(4000); // wait for the setup of the Blox M8N module, especially for winter + uint8_t val[2]; + int i2c_status = 1; + + /* + * we need check the i2c connectio with external mag(HMC5883L) several times initially, + * since the module UBlox M8N has some problem with the gps/mag simultaneous polling. + */ + + for(int i = 0; i < EXTERNAL_MAG_CHECK_COUNT; i ++) + { + /* check the existence of external magnetometer */ + + /* 1. LIS3MDL */ + val[0] = LIS3MDL_PING; + i2c_status = HAL_I2C_Master_Transmit(hi2c_, LIS3MDL_MAG_REGISTER, val, 1, 100); + HAL_I2C_Master_Receive(hi2c_, LIS3MDL_MAG_REGISTER, val, 1, 100); + + if(i2c_status == HAL_OK && val[0] == 61) //0b111101 + { + HAL_Delay(10); + lis3mdlInit(); + return LIS3MDL; + } + + /* 2. HMC58X3 */ + val[0] = HMC58X3_R_CONFB; + val[1] = 0x20; + i2c_status = HAL_I2C_Master_Transmit(hi2c_, HMC58X3_MAG_REGISTER, val, 2, 100); + + if(i2c_status == HAL_OK) + { + HAL_Delay(10); + hmc58x3Init(); + return HMC58X3; + } + HAL_Delay(10); + } + return INTERNAL_MAG; +} + void IMUOnboard::lis3mdlInit(void) { uint8_t val[2]; diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_mpu9250.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_mpu9250.h index 152ef3000..3c1c6577b 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_mpu9250.h +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_mpu9250.h @@ -9,8 +9,8 @@ #error "Please define __cplusplus, because this is a c++ based file " #endif -#ifndef __IMU_H -#define __IMU_H +#ifndef __IMU_MPU_H +#define __IMU_MPU_H #include "config.h" #include "math/AP_Math.h" @@ -28,6 +28,25 @@ class IMUOnboard : public IMU { enum{INTERNAL_MAG = 0, LIS3MDL = 1, HMC58X3 = 2,}; +protected: + static uint8_t adc_[SENSOR_DATA_LENGTH]; + static uint32_t last_mag_time_; + + uint8_t mag_id_; + + 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_; + + bool use_external_mag_flag_; + uint32_t calib_indicator_time_; // ms + + uint8_t checkExternalMag(void); private: static const uint8_t GYRO_DLPF_CFG = 0x03;//0x01 //0: 250Hz, 0.97ms; 3: 41Hz, 5.9ms(kduino); 4: 20Hz: 9.9ms static const uint8_t ACC_DLPF_CFG = 0x05; //0x03: 41Hz, 11.80ms @@ -73,26 +92,9 @@ class IMUOnboard : public IMU { static const uint32_t HMC58X3_READ_PRESCALER = 13; // 1000Hz/13= 75Hz static constexpr float HMC58X3_RATE = 0.092; // 0.92mG/LSb (1.3G) -> 1G = 10e-4T -> 0.092mT/LSb - static uint8_t adc_[SENSOR_DATA_LENGTH]; - static uint32_t last_mag_time_; - - uint8_t mag_id_; - - 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_; - - bool use_external_mag_flag_; - uint32_t calib_indicator_time_; // ms - - void gyroInit(void); - void accInit(void); - void magInit(void); + virtual void gyroInit(void); + virtual void accInit(void); + virtual void magInit(void); // external mag void hmc58x3Init(void); From 165170c3433e93d1358e26ba9af340b93267c242 Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Tue, 12 Mar 2024 22:31:53 +0900 Subject: [PATCH 02/21] [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 From 8983b434e1592d14a715ba6636f66147d0d4db78 Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Sat, 23 Mar 2024 19:54:48 +0900 Subject: [PATCH 03/21] [Spinal][ICM20948][WIP] workaround enabling mup reading --- .../lib/Jsk_Lib/sensors/imu/icm_20948.cpp | 36 ++++++++++++------- 1 file changed, 23 insertions(+), 13 deletions(-) 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 index 325b5af1a..0808de5fa 100644 --- 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 @@ -50,6 +50,7 @@ void ICM20948::gyroInit(void) // } } + /* 1.Clear all bits in ub0-ub3 register */ deviceReset(); @@ -90,12 +91,13 @@ void ICM20948::magInit(void) i2cMasterClkFrq(7); // 345.6 kHz / 46.67% dyty cycle (recoomended) /* Waiting for finding Magnetometer */ + // uint32_t search_start_time = HAL_GetTick(); 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(); - } + // 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 */ @@ -107,9 +109,9 @@ void ICM20948::magInit(void) void ICM20948::updateRawData() { - gyroRead(&raw_gyro_adc_); - accelRead(&raw_acc_adc_); - magRead(&raw_mag_adc_); + gyroReadDps(&raw_gyro_adc_); + accelReadG(&raw_acc_adc_); + magReadUT(&raw_mag_adc_); } void ICM20948::gyroRead(Vector3f* data) @@ -127,7 +129,7 @@ void ICM20948::accelRead(Vector3f* data) 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_; + data->z = (int16_t)(temp[4] << 8 | temp[5]); // Add scale factor because calibraiton function offset gravity acceleration. } @@ -137,12 +139,19 @@ bool ICM20948::magRead(Vector3f* data) uint8_t drdy, hofl; // data ready, overflow drdy = readSingleAk09916(MAG_ST1) & 0x01; - if(!drdy) return false; + if(!drdy){ + nh_->logerror("not ready"); + return false; + } temp = readMultipleAk09916(MAG_HXL, 6); + // HAL_Delay(1); hofl = readSingleAk09916(MAG_ST2) & 0x08; - if(hofl) return false; + if(hofl){ + nh_->logerror("overflow"); + return false; + } data->x = (int16_t)(temp[1] << 8 | temp[0]); data->y = (int16_t)(temp[3] << 8 | temp[2]); @@ -387,12 +396,14 @@ 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) @@ -439,7 +450,6 @@ uint8_t ICM20948::readSingleAk09916(uint8_t reg) 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); } @@ -457,6 +467,6 @@ uint8_t* ICM20948::readMultipleAk09916(uint8_t reg, uint8_t len) 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); + } From a7d46100ead5a040856d584a86a51b7b7d34982f Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Sat, 23 Mar 2024 19:56:15 +0900 Subject: [PATCH 04/21] [Spinal][ICM20948] create a new flag to determine imu type --- .../mcu_project/boards/stm32H7/Src/main.c | 36 +++++++++++++------ .../mcu_project/boards/stm32H7/spinal.ioc | 13 ++++--- .../spinal/mcu_project/lib/Jsk_Lib/config.h | 2 ++ 3 files changed, 36 insertions(+), 15 deletions(-) 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 cce6cd82e..fdbac7df0 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,7 @@ #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" @@ -101,8 +101,11 @@ osMailQId canMsgMailHandle; ros::NodeHandle nh_; /* sensor instances */ -/* IMUOnboard imu_; */ +#if IMU_MPU +IMUOnboard imu_; +#elif IMU_ICM ICM20948 imu_; +#endif Baro baro_; GPS gps_; BatteryStatus battery_status_; @@ -230,8 +233,11 @@ int main(void) FlashMemory::read(); #endif - /* imu_.init(&hspi1, &hi2c3, &nh_, IMUCS_GPIO_Port, IMUCS_Pin, LED0_GPIO_Port, LED0_Pin); */ +#if IMU_MPU + imu_.init(&hspi1, &hi2c3, &nh_, IMUCS_GPIO_Port, IMUCS_Pin, LED0_GPIO_Port, LED0_Pin); +#elif IMU_ICM imu_.init(&hspi1, &hi2c3, &nh_, SPI1_CS_GPIO_Port, SPI1_CS_Pin, LED0_GPIO_Port, LED0_Pin); +#endif IMU_ROS_CMD::init(&nh_); IMU_ROS_CMD::addImu(&imu_); baro_.init(&hi2c1, &nh_, BAROCS_GPIO_Port, BAROCS_Pin); @@ -653,10 +659,10 @@ static void MX_SPI1_Init(void) 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.CLKPolarity = SPI_POLARITY_HIGH; + hspi1.Init.CLKPhase = SPI_PHASE_2EDGE; hspi1.Init.NSS = SPI_NSS_SOFT; - hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256; + hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32; hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB; hspi1.Init.TIMode = SPI_TIMODE_DISABLE; hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; @@ -984,7 +990,10 @@ 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(GPIOB, IMUCS_Pin|SPI1_CS_Pin, GPIO_PIN_RESET); + HAL_GPIO_WritePin(IMUCS_GPIO_Port, IMUCS_Pin, GPIO_PIN_RESET); + + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(SPI1_CS_GPIO_Port, SPI1_CS_Pin, GPIO_PIN_SET); /*Configure GPIO pins : LED0_Pin LED1_Pin LED2_Pin */ GPIO_InitStruct.Pin = LED0_Pin|LED1_Pin|LED2_Pin; @@ -993,12 +1002,19 @@ static void MX_GPIO_Init(void) GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM; HAL_GPIO_Init(GPIOE, &GPIO_InitStruct); - /*Configure GPIO pins : IMUCS_Pin SPI1_CS_Pin */ - GPIO_InitStruct.Pin = IMUCS_Pin|SPI1_CS_Pin; + /*Configure GPIO pin : IMUCS_Pin */ + GPIO_InitStruct.Pin = IMUCS_Pin; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM; - HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + HAL_GPIO_Init(IMUCS_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_HIGH; + HAL_GPIO_Init(SPI1_CS_GPIO_Port, &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 0db3bd168..7278050ea 100644 --- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/spinal.ioc +++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/spinal.ioc @@ -329,10 +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.GPIOParameters=GPIO_Speed,PinState,GPIO_Label PB7.GPIO_Label=SPI1_CS -PB7.GPIO_Speed=GPIO_SPEED_FREQ_MEDIUM +PB7.GPIO_Speed=GPIO_SPEED_FREQ_HIGH PB7.Locked=true +PB7.PinState=GPIO_PIN_SET PB7.Signal=GPIO_Output PB8.Locked=true PB8.Mode=I2C @@ -542,11 +543,13 @@ SH.S_TIM4_CH3.0=TIM4_CH3,PWM Generation3 CH3 SH.S_TIM4_CH3.ConfNb=1 SH.S_TIM4_CH4.0=TIM4_CH4,PWM Generation4 CH4 SH.S_TIM4_CH4.ConfNb=1 -SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_256 -SPI1.CalculateBaudRate=781.25 KBits/s +SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_32 +SPI1.CLKPhase=SPI_PHASE_2EDGE +SPI1.CLKPolarity=SPI_POLARITY_HIGH +SPI1.CalculateBaudRate=6.25 MBits/s SPI1.DataSize=SPI_DATASIZE_8BIT SPI1.Direction=SPI_DIRECTION_2LINES -SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,BaudRatePrescaler,DataSize +SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,BaudRatePrescaler,DataSize,CLKPolarity,CLKPhase SPI1.Mode=SPI_MODE_MASTER SPI1.VirtualType=VM_MASTER TIM1.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1 diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/config.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/config.h index 498c8f6d3..a7d1e5007 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/config.h +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/config.h @@ -36,6 +36,8 @@ //2.1 Sensors //2.1.1 IMU Sensor #define IMU_FLAG 1 +#define IMU_MPU 0 +#define IMU_ICM 1 //2.1.2 Barometer Sensor #define BARO_FLAG 1 //2.1.3 GPS Sensor From 2f77c27739a017a7d0813dc267628f1b6cfc0c19 Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Sun, 24 Mar 2024 22:40:40 +0900 Subject: [PATCH 05/21] [Spinal][ICM20948][WIP] device connection is completed, but it's still very unstable. --- .../mcu_project/boards/stm32H7/Src/main.c | 7 ++-- .../mcu_project/boards/stm32H7/spinal.ioc | 6 ++-- .../lib/Jsk_Lib/sensors/imu/icm_20948.cpp | 33 +++++++++++-------- 3 files changed, 25 insertions(+), 21 deletions(-) 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 fdbac7df0..f5e88dd8d 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 @@ -302,7 +302,7 @@ int main(void) /* Create the thread(s) */ /* definition and creation of coreTask */ - osThreadDef(coreTask, coreTaskFunc, osPriorityRealtime, 0, 512); + osThreadDef(coreTask, coreTaskFunc, osPriorityRealtime, 0, 1024); coreTaskHandle = osThreadCreate(osThread(coreTask), NULL); /* definition and creation of rosSpinTask */ @@ -990,10 +990,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); - - /*Configure GPIO pin Output Level */ - HAL_GPIO_WritePin(SPI1_CS_GPIO_Port, SPI1_CS_Pin, GPIO_PIN_SET); + HAL_GPIO_WritePin(GPIOB, IMUCS_Pin|SPI1_CS_Pin, GPIO_PIN_SET); /*Configure GPIO pins : LED0_Pin LED1_Pin LED2_Pin */ GPIO_InitStruct.Pin = LED0_Pin|LED1_Pin|LED2_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 7278050ea..587319812 100644 --- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/spinal.ioc +++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/spinal.ioc @@ -123,7 +123,7 @@ FREERTOS.BinarySemaphores01=coreTaskSem,Dynamic,NULL;uartTxSem,Dynamic,NULL FREERTOS.FootprintOK=true FREERTOS.IPParameters=Tasks01,configTOTAL_HEAP_SIZE,FootprintOK,configUSE_TIMERS,configTIMER_TASK_PRIORITY,Timers01,BinarySemaphores01,Mutexes01 FREERTOS.Mutexes01=rosPubMutex,Dynamic,NULL;flightControlMutex,Dynamic,NULL -FREERTOS.Tasks01=coreTask,3,512,coreTaskFunc,Default,NULL,Dynamic,NULL,NULL;rosSpinTask,0,256,rosSpinTaskFunc,Default,NULL,Dynamic,NULL,NULL;idleTask,-3,128,idleTaskFunc,Default,NULL,Dynamic,NULL,NULL;rosPublish,-1,128,rosPublishTask,Default,NULL,Dynamic,NULL,NULL;voltage,-2,256,voltageTask,Default,NULL,Dynamic,NULL,NULL;canRx,3,256,canRxTask,As weak,NULL,Dynamic,NULL,NULL +FREERTOS.Tasks01=coreTask,3,1024,coreTaskFunc,Default,NULL,Dynamic,NULL,NULL;rosSpinTask,0,256,rosSpinTaskFunc,Default,NULL,Dynamic,NULL,NULL;idleTask,-3,128,idleTaskFunc,Default,NULL,Dynamic,NULL,NULL;rosPublish,-1,128,rosPublishTask,Default,NULL,Dynamic,NULL,NULL;voltage,-2,256,voltageTask,Default,NULL,Dynamic,NULL,NULL;canRx,3,256,canRxTask,As weak,NULL,Dynamic,NULL,NULL FREERTOS.Timers01=coreTaskTimer,coreTaskEvokeCb,osTimerPeriodic,Default,NULL,Dynamic,NULL FREERTOS.configTIMER_TASK_PRIORITY=6 FREERTOS.configTOTAL_HEAP_SIZE=25600 @@ -323,11 +323,12 @@ PB5.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH PB5.Locked=true PB5.Mode=Full_Duplex_Master PB5.Signal=SPI1_MOSI -PB6.GPIOParameters=GPIO_Speed,GPIO_FM6,GPIO_Label +PB6.GPIOParameters=GPIO_Speed,GPIO_FM6,PinState,GPIO_Label PB6.GPIO_FM6=__NULL PB6.GPIO_Label=IMUCS PB6.GPIO_Speed=GPIO_SPEED_FREQ_MEDIUM PB6.Locked=true +PB6.PinState=GPIO_PIN_SET PB6.Signal=GPIO_Output PB7.GPIOParameters=GPIO_Speed,PinState,GPIO_Label PB7.GPIO_Label=SPI1_CS @@ -589,3 +590,4 @@ VP_TIM1_VS_ClockSourceINT.Signal=TIM1_VS_ClockSourceINT VP_TIM4_VS_ClockSourceINT.Mode=Internal VP_TIM4_VS_ClockSourceINT.Signal=TIM4_VS_ClockSourceINT board=custom +isbadioc=false 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 index 0808de5fa..58e7a00a0 100644 --- 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 @@ -29,6 +29,10 @@ void ICM20948::init(SPI_HandleTypeDef* hspi, I2C_HandleTypeDef* hi2c, ros::NodeH calib_indicator_time_ = HAL_GetTick(); +#ifdef STM32H7 + hspi_->Instance->CR1 |= (uint32_t)(SPI_BAUDRATEPRESCALER_64); //16 = 12.5Mhz, 200MHz +#endif + gyroInit(); accInit(); magInit(); @@ -49,8 +53,7 @@ void ICM20948::gyroInit(void) // HAL_NVIC_SystemReset(); // } } - - + /* 1.Clear all bits in ub0-ub3 register */ deviceReset(); @@ -140,16 +143,15 @@ bool ICM20948::magRead(Vector3f* data) drdy = readSingleAk09916(MAG_ST1) & 0x01; if(!drdy){ - nh_->logerror("not ready"); + // nh_->logerror("not ready"); return false; } temp = readMultipleAk09916(MAG_HXL, 6); - // HAL_Delay(1); hofl = readSingleAk09916(MAG_ST2) & 0x08; if(hofl){ - nh_->logerror("overflow"); + // nh_->logerror("overflow"); return false; } @@ -249,6 +251,7 @@ void ICM20948::spiModeEnable() new_val |= 0x10; writeSingleIcm20948(ub_0, B0_USER_CTRL, new_val); + HAL_Delay(100); } void ICM20948::i2cMasterReset() @@ -257,6 +260,7 @@ void ICM20948::i2cMasterReset() new_val |= 0x02; writeSingleIcm20948(ub_0, B0_USER_CTRL, new_val); + HAL_Delay(100); } void ICM20948::i2cMasterEnable() @@ -273,7 +277,8 @@ 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); + writeSingleIcm20948(ub_3, B3_I2C_MST_CTRL, new_val); + HAL_Delay(100); } void ICM20948::setClockSource(uint8_t source) @@ -282,11 +287,13 @@ void ICM20948::setClockSource(uint8_t source) new_val |= source; writeSingleIcm20948(ub_0, B0_PWR_MGMT_1, new_val); + HAL_Delay(100); } void ICM20948::odrAlignEnable() { writeSingleIcm20948(ub_2, B2_ODR_ALIGN_EN, 0x01); + HAL_Delay(100); } void ICM20948::setGyroLpf(uint8_t config) @@ -308,6 +315,7 @@ void ICM20948::setAccelLpf(uint8_t config) void ICM20948::setGyroSampleRate(uint8_t divider) { writeSingleIcm20948(ub_2, B2_GYRO_SMPLRT_DIV, divider); + HAL_Delay(100); } void ICM20948::setAccelSampleRate(uint16_t divider) @@ -317,6 +325,7 @@ void ICM20948::setAccelSampleRate(uint16_t divider) writeSingleIcm20948(ub_2, B2_ACCEL_SMPLRT_DIV_1, divider_1); writeSingleIcm20948(ub_2, B2_ACCEL_SMPLRT_DIV_2, divider_2); + HAL_Delay(100); } void ICM20948::setMagOperationMode(operation_mode mode) @@ -351,6 +360,7 @@ void ICM20948::setGyroFullScale(gyro_full_scale full_scale) } writeSingleIcm20948(ub_2, B2_GYRO_CONFIG_1, new_val); + HAL_Delay(100); } void ICM20948::setAccelFullScale(accel_full_scale full_scale) @@ -378,6 +388,7 @@ void ICM20948::setAccelFullScale(accel_full_scale full_scale) } writeSingleIcm20948(ub_2, B2_ACCEL_CONFIG, new_val); + HAL_Delay(100); } void ICM20948::selectUserBank(userbank ub) @@ -401,7 +412,6 @@ uint8_t ICM20948::readSingleIcm20948(userbank ub, uint8_t reg) 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; } @@ -411,9 +421,7 @@ 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_); @@ -429,7 +437,6 @@ uint8_t* ICM20948::readMultipleIcm20948(userbank ub, uint8_t reg, uint8_t len) 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; } @@ -449,8 +456,7 @@ 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); - - return readSingleIcm20948(ub_0, B0_EXT_SLV_SENS_DATA_00); + return readSingleIcm20948(ub_0, B0_EXT_SLV_SENS_DATA_00); } void ICM20948::writeSingleAk09916(uint8_t reg, uint8_t val) @@ -466,7 +472,6 @@ 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); - - return readMultipleIcm20948(ub_0, B0_EXT_SLV_SENS_DATA_00, len); + return readMultipleIcm20948(ub_0, B0_EXT_SLV_SENS_DATA_00, len); } From 371e3757cab4677ea957d684a8d2ec933b17236b Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Sun, 31 Mar 2024 16:31:01 +0900 Subject: [PATCH 06/21] [Spinal][ICM20948] modified the method of waking up icm --- .../boards/stm32H7/STM32CubeIDE/.cproject | 5 +- .../boards/stm32H7/STM32CubeIDE/.project | 5 ++ .../mcu_project/boards/stm32H7/spinal.ioc | 1 - .../lib/Jsk_Lib/sensors/imu/icm_20948.cpp | 48 +++++++++---------- 4 files changed, 32 insertions(+), 27 deletions(-) diff --git a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.cproject b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.cproject index a83aeedc7..771decefa 100644 --- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.cproject +++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.cproject @@ -25,7 +25,7 @@