From 371e3757cab4677ea957d684a8d2ec933b17236b Mon Sep 17 00:00:00 2001
From: Junichiro Sugihara <j-sugihara@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 31 Mar 2024 16:31:01 +0900
Subject: [PATCH] [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 @@
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.1560105099" name="Floating-point unit" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.value.fpv5-d16" valueType="enumerated"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.451674053" name="Floating-point ABI" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.value.hard" valueType="enumerated"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board.1123785686" name="Board" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board" useByScannerDiscovery="false" value="genericBoard" valueType="string"/>
-							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.785140283" name="Defaults" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" useByScannerDiscovery="false" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.5 || Debug || true || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.base.gnu-tools-for-stm32 || STM32H743VITx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../../Drivers/CMSIS/Include | ../../Middlewares/Third_Party/LwIP/src/include/lwip/priv | ../../Middlewares/Third_Party/LwIP/src/include/netif/ppp | ../../Middlewares/Third_Party/LwIP/src/include/lwip/apps | ../../Middlewares/Third_Party/LwIP/src/include/compat/posix/net | ../../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS | ../../Middlewares/Third_Party/LwIP/src/include/lwip | ../../Middlewares/Third_Party/LwIP/src/include/lwip/prot | ../../Drivers/STM32H7xx_HAL_Driver/Inc | ../../Middlewares/Third_Party/LwIP/src/include/compat/stdc | ../../Middlewares/Third_Party/LwIP/src/include/compat/posix | ../../Drivers/CMSIS/Device/ST/STM32H7xx/Include | ../../Middlewares/Third_Party/LwIP/system/arch | ../../Middlewares/Third_Party/FreeRTOS/Source/include | ../../Middlewares/Third_Party/LwIP/src/include/compat/posix/sys | ../../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F | ../../Inc | ../../Drivers/STM32H7xx_HAL_Driver/Inc/Legacy | ../../Middlewares/Third_Party/LwIP/system | ../../Middlewares/Third_Party/LwIP/src/include | ../../Middlewares/Third_Party/LwIP/src/include/netif | ../../Middlewares/Third_Party/LwIP/src/include/compat/posix/arpa | ../../Drivers/BSP/Components/lan8742 ||  ||  || USE_HAL_DRIVER | STM32H743xx ||  ||  ||  ||  || ${workspace_loc:/${ProjName}/STM32H743VITX_FLASH.ld} || true || NonSecure ||  || secure_nsclib.o ||  || None || " valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.785140283" name="Defaults" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" useByScannerDiscovery="false" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.5 || Debug || true || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.base.gnu-tools-for-stm32 || STM32H743VITx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../../Drivers/CMSIS/Include | ../../Middlewares/Third_Party/LwIP/src/include/lwip/priv | ../../Middlewares/Third_Party/LwIP/src/include/netif/ppp | ../../Middlewares/Third_Party/LwIP/src/include/lwip/apps | ../../Middlewares/Third_Party/LwIP/src/include/compat/posix/net | ../../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS | ../../Middlewares/Third_Party/LwIP/src/include/lwip | ../../Middlewares/Third_Party/LwIP/src/include/lwip/prot | ../../Drivers/STM32H7xx_HAL_Driver/Inc | ../../Middlewares/Third_Party/LwIP/src/include/compat/stdc | ../../Middlewares/Third_Party/LwIP/src/include/compat/posix | ../../Drivers/BSP/Components/lan8742 | ../../Drivers/CMSIS/Device/ST/STM32H7xx/Include | ../../Middlewares/Third_Party/LwIP/system/arch | ../../Middlewares/Third_Party/FreeRTOS/Source/include | ../../Middlewares/Third_Party/LwIP/src/include/compat/posix/sys | ../../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F | ../../Inc | ../../Drivers/STM32H7xx_HAL_Driver/Inc/Legacy | ../../Middlewares/Third_Party/LwIP/system | ../../Middlewares/Third_Party/LwIP/src/include | ../../Middlewares/Third_Party/LwIP/src/include/netif | ../../Middlewares/Third_Party/LwIP/src/include/compat/posix/arpa ||  ||  || USE_HAL_DRIVER | STM32H743xx ||  ||  ||  ||  || ${workspace_loc:/${ProjName}/STM32H743VITX_FLASH.ld} || true || NonSecure ||  || secure_nsclib.o ||  || None || " valueType="string"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.toolchain.1808345384" name="Toolchain" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.toolchain" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.base.gnu-tools-for-stm32.9-2020-q2-update" valueType="string"/>
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.1732425603" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
 							<builder buildPath="${workspace_loc:/spinal}/Debug" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.530361035" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder"/>
@@ -209,7 +209,7 @@
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.619623993" name="Floating-point unit" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.value.fpv5-d16" valueType="enumerated"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.579314840" name="Floating-point ABI" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.value.hard" valueType="enumerated"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board.1946193872" name="Board" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board" useByScannerDiscovery="false" value="genericBoard" valueType="string"/>
-							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.15398046" name="Defaults" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" useByScannerDiscovery="false" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.5 || Release || false || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.base.gnu-tools-for-stm32 || STM32H743VITx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../../Drivers/CMSIS/Include | ../../Middlewares/Third_Party/LwIP/src/include/lwip/priv | ../../Middlewares/Third_Party/LwIP/src/include/netif/ppp | ../../Middlewares/Third_Party/LwIP/src/include/lwip/apps | ../../Middlewares/Third_Party/LwIP/src/include/compat/posix/net | ../../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS | ../../Middlewares/Third_Party/LwIP/src/include/lwip | ../../Middlewares/Third_Party/LwIP/src/include/lwip/prot | ../../Drivers/STM32H7xx_HAL_Driver/Inc | ../../Middlewares/Third_Party/LwIP/src/include/compat/stdc | ../../Middlewares/Third_Party/LwIP/src/include/compat/posix | ../../Drivers/CMSIS/Device/ST/STM32H7xx/Include | ../../Middlewares/Third_Party/LwIP/system/arch | ../../Middlewares/Third_Party/FreeRTOS/Source/include | ../../Middlewares/Third_Party/LwIP/src/include/compat/posix/sys | ../../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F | ../../Inc | ../../Drivers/STM32H7xx_HAL_Driver/Inc/Legacy | ../../Middlewares/Third_Party/LwIP/system | ../../Middlewares/Third_Party/LwIP/src/include | ../../Middlewares/Third_Party/LwIP/src/include/netif | ../../Middlewares/Third_Party/LwIP/src/include/compat/posix/arpa | ../../Drivers/BSP/Components/lan8742 ||  ||  || USE_HAL_DRIVER | STM32H743xx ||  ||  ||  ||  || ${workspace_loc:/${ProjName}/STM32H743VITX_FLASH.ld} || true || NonSecure ||  || secure_nsclib.o ||  || None || " valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.15398046" name="Defaults" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" useByScannerDiscovery="false" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.5 || Release || false || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.base.gnu-tools-for-stm32 || STM32H743VITx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../../Drivers/CMSIS/Include | ../../Middlewares/Third_Party/LwIP/src/include/lwip/priv | ../../Middlewares/Third_Party/LwIP/src/include/netif/ppp | ../../Middlewares/Third_Party/LwIP/src/include/lwip/apps | ../../Middlewares/Third_Party/LwIP/src/include/compat/posix/net | ../../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS | ../../Middlewares/Third_Party/LwIP/src/include/lwip | ../../Middlewares/Third_Party/LwIP/src/include/lwip/prot | ../../Drivers/STM32H7xx_HAL_Driver/Inc | ../../Middlewares/Third_Party/LwIP/src/include/compat/stdc | ../../Middlewares/Third_Party/LwIP/src/include/compat/posix | ../../Drivers/BSP/Components/lan8742 | ../../Drivers/CMSIS/Device/ST/STM32H7xx/Include | ../../Middlewares/Third_Party/LwIP/system/arch | ../../Middlewares/Third_Party/FreeRTOS/Source/include | ../../Middlewares/Third_Party/LwIP/src/include/compat/posix/sys | ../../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F | ../../Inc | ../../Drivers/STM32H7xx_HAL_Driver/Inc/Legacy | ../../Middlewares/Third_Party/LwIP/system | ../../Middlewares/Third_Party/LwIP/src/include | ../../Middlewares/Third_Party/LwIP/src/include/netif | ../../Middlewares/Third_Party/LwIP/src/include/compat/posix/arpa ||  ||  || USE_HAL_DRIVER | STM32H743xx ||  ||  ||  ||  || ${workspace_loc:/${ProjName}/STM32H743VITX_FLASH.ld} || true || NonSecure ||  || secure_nsclib.o ||  || None || " valueType="string"/>
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.2052134197" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
 							<builder buildPath="${workspace_loc:/spinal}/Release" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.843691260" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder"/>
 							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.1567965632" name="MCU GCC Assembler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler">
@@ -341,4 +341,5 @@
 			<autodiscovery enabled="false" problemReportingEnabled="true" selectedProfileId=""/>
 		</scannerConfigBuildInfo>
 	</storageModule>
+	<storageModule moduleId="refreshScope"/>
 </cproject>
\ No newline at end of file
diff --git a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.project b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.project
index 544127d9b..b9117635b 100644
--- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.project
+++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.project
@@ -67,6 +67,11 @@
 			<type>1</type>
 			<locationURI>PARENT-1-PROJECT_LOC/Src/lwip.c</locationURI>
 		</link>
+		<link>
+			<name>Application/User/main.c</name>
+			<type>1</type>
+			<locationURI>PARENT-1-PROJECT_LOC/Src/main.c</locationURI>
+		</link>
 		<link>
 			<name>Application/User/main.cpp</name>
 			<type>1</type>
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 587319812..00842a327 100644
--- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/spinal.ioc
+++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/spinal.ioc
@@ -590,4 +590,3 @@ 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 58e7a00a0..3a4a08a71 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
@@ -45,20 +45,14 @@ void ICM20948::gyroInit(void)
   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();
-    // }
-  }
+
+  while(!getIcm20948WhoAmI());
   
   /* 1.Clear all bits in ub0-ub3 register */
   deviceReset();
 
   /* 2.Wakeup icm20948 */
-  wakeUp();
+  // wakeUp();
 
   /* 3.Set fundamental properties */
   setClockSource(1); // Clock source is automatically selected.
@@ -72,12 +66,14 @@ void ICM20948::gyroInit(void)
 
 void ICM20948::accInit (void)
 {
+  HAL_Delay(100);
   /* 5.Acc initialization */
   setAccelFullScale(_8g);
 }
 
 void ICM20948::magInit(void)
 {
+  HAL_Delay(100);
   /* 6.1 Check whether use external magnetometer */
   mag_id_ = IMUOnboard::checkExternalMag();
 
@@ -94,14 +90,7 @@ 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();
-        // }
-      }
+      while(!getAk09916WhoAmI());
 
       /* 6.3 Clear all bits in mag register */
       magSoftReset();
@@ -112,9 +101,12 @@ void ICM20948::magInit(void)
 
 void ICM20948::updateRawData()
 {
-  gyroReadDps(&raw_gyro_adc_);
-  accelReadG(&raw_acc_adc_);
-  magReadUT(&raw_mag_adc_);
+  // if(getIcm20948WhoAmI()) nh_->logerror("ok icm");
+  if(getAk09916WhoAmI()) nh_->logerror("ok mag");
+  gyroRead(&raw_gyro_adc_);
+  // gyroReadDps(&raw_gyro_adc_);
+  // accelReadG(&raw_acc_adc_);
+  // magReadUT(&raw_mag_adc_);
 }
 
 void ICM20948::gyroRead(Vector3f* data)
@@ -208,7 +200,6 @@ bool ICM20948::getIcm20948WhoAmI()
 bool ICM20948::getAk09916WhoAmI()
 {
   uint8_t ak09916_id = readSingleAk09916(MAG_WIA2);
-
   if(ak09916_id == AK09916_ID)
     return true;
   else
@@ -250,6 +241,8 @@ void ICM20948::spiModeEnable()
   uint8_t new_val = readSingleIcm20948(ub_0, B0_USER_CTRL);
   new_val |= 0x10;
 
+  HAL_Delay(100);
+
   writeSingleIcm20948(ub_0, B0_USER_CTRL, new_val);
   HAL_Delay(100);
 }
@@ -259,6 +252,8 @@ void ICM20948::i2cMasterReset()
   uint8_t new_val = readSingleIcm20948(ub_0, B0_USER_CTRL);
   new_val |= 0x02;
 
+  HAL_Delay(100);
+
   writeSingleIcm20948(ub_0, B0_USER_CTRL, new_val);
   HAL_Delay(100);
 }
@@ -268,6 +263,8 @@ void ICM20948::i2cMasterEnable()
   uint8_t new_val = readSingleIcm20948(ub_0, B0_USER_CTRL);
   new_val |= 0x20;
 
+  HAL_Delay(100);
+
   writeSingleIcm20948(ub_0, B0_USER_CTRL, new_val);
   HAL_Delay(100);
 }
@@ -277,14 +274,15 @@ void ICM20948::i2cMasterClkFrq(uint8_t config)
   uint8_t new_val = readSingleIcm20948(ub_3, B3_I2C_MST_CTRL);
   new_val |= config;
 
+  HAL_Delay(100);
+
   writeSingleIcm20948(ub_3, B3_I2C_MST_CTRL, new_val);
   HAL_Delay(100);
 }
 
 void ICM20948::setClockSource(uint8_t source)
 {
-  uint8_t new_val = readSingleIcm20948(ub_0, B0_PWR_MGMT_1);
-  new_val |= source;
+  uint8_t new_val = 0x01;
 
   writeSingleIcm20948(ub_0, B0_PWR_MGMT_1, new_val);
   HAL_Delay(100);
@@ -366,6 +364,8 @@ void ICM20948::setGyroFullScale(gyro_full_scale full_scale)
 void ICM20948::setAccelFullScale(accel_full_scale full_scale)
 {
   uint8_t new_val = readSingleIcm20948(ub_2, B2_ACCEL_CONFIG);
+
+  HAL_Delay(100);
 	
   switch(full_scale)
     {
@@ -430,7 +430,7 @@ void ICM20948::writeSingleIcm20948(userbank ub, uint8_t reg, uint8_t val)
 uint8_t* ICM20948::readMultipleIcm20948(userbank ub, uint8_t reg, uint8_t len)
 {
   uint8_t read_reg = READ | reg;
-  static uint8_t reg_val[6];
+  uint8_t reg_val[6];
   selectUserBank(ub);
 
   GPIO_L(spi_cs_port_, spi_cs_pin_);