Skip to content

Commit

Permalink
[Spinal][ICM20948] modified the method of waking up icm
Browse files Browse the repository at this point in the history
  • Loading branch information
@sugihara-16
sugihara-16 committed Mar 31, 2024
1 parent 2f77c27 commit 371e375
Show file tree
Hide file tree
Showing 4 changed files with 32 additions and 27 deletions.
5 changes: 3 additions & 2 deletions aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.cproject
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"/>
Expand Down Expand Up @@ -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">
Expand Down Expand Up @@ -341,4 +341,5 @@
<autodiscovery enabled="false" problemReportingEnabled="true" selectedProfileId=""/>
</scannerConfigBuildInfo>
</storageModule>
<storageModule moduleId="refreshScope"/>
</cproject>
5 changes: 5 additions & 0 deletions aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.project
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
1 change: 0 additions & 1 deletion aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/spinal.ioc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
48 changes: 24 additions & 24 deletions aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/icm_20948.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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();

Expand All @@ -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();
Expand All @@ -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)
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
}
Expand All @@ -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);
}
Expand All @@ -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);
}
Expand All @@ -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);
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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_);
Expand Down

0 comments on commit 371e375

Please sign in to comment.