From a7d46100ead5a040856d584a86a51b7b7d34982f Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Sat, 23 Mar 2024 19:56:15 +0900 Subject: [PATCH] [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