[Spinal][IMU][ICM20948][WIP] create a new interface of icm-20948 (thi…

…s can be compiled, but stil not work)
sugihara-16 · Mar 12, 2024 · 165170c · 165170c
1 parent 3eac22f
commit 165170c
Show file tree

Hide file tree

Showing 5 changed files with 783 additions and 16 deletions.
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
@@ -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
@@ -35,7 +35,8 @@
 #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"
 #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
@@ -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