[Spinal][ICM20948] create a new flag to determine imu type

sugihara-16 · Mar 23, 2024 · a7d4610 · a7d4610
1 parent 8983b43
commit a7d4610
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 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
@@ -35,7 +35,7 @@
 #include <spinal/Imu.h>
 
 #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
@@ -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
@@ -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