Skip to content

Commit

Permalink
[Spinal][ICM20948][WIP] device connection is completed, but it's stil…
Browse files Browse the repository at this point in the history 
…l very unstable.
  • Loading branch information
@sugihara-16
sugihara-16 committed Mar 24, 2024
1 parent a7d4610 commit 2f77c27
Show file tree
Hide file tree
Showing 3 changed files with 25 additions and 21 deletions.
7 changes: 2 additions & 5 deletions aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Src/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -302,7 +302,7 @@ int main(void)

/* Create the thread(s) */
/* definition and creation of coreTask */
osThreadDef(coreTask, coreTaskFunc, osPriorityRealtime, 0, 512);
osThreadDef(coreTask, coreTaskFunc, osPriorityRealtime, 0, 1024);
coreTaskHandle = osThreadCreate(osThread(coreTask), NULL);

/* definition and creation of rosSpinTask */
Expand Down Expand Up @@ -990,10 +990,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);

/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(SPI1_CS_GPIO_Port, SPI1_CS_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOB, IMUCS_Pin|SPI1_CS_Pin, GPIO_PIN_SET);

/*Configure GPIO pins : LED0_Pin LED1_Pin LED2_Pin */
GPIO_InitStruct.Pin = LED0_Pin|LED1_Pin|LED2_Pin;
Expand Down
6 changes: 4 additions & 2 deletions aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/spinal.ioc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -123,7 +123,7 @@ FREERTOS.BinarySemaphores01=coreTaskSem,Dynamic,NULL;uartTxSem,Dynamic,NULL
FREERTOS.FootprintOK=true
FREERTOS.IPParameters=Tasks01,configTOTAL_HEAP_SIZE,FootprintOK,configUSE_TIMERS,configTIMER_TASK_PRIORITY,Timers01,BinarySemaphores01,Mutexes01
FREERTOS.Mutexes01=rosPubMutex,Dynamic,NULL;flightControlMutex,Dynamic,NULL
FREERTOS.Tasks01=coreTask,3,512,coreTaskFunc,Default,NULL,Dynamic,NULL,NULL;rosSpinTask,0,256,rosSpinTaskFunc,Default,NULL,Dynamic,NULL,NULL;idleTask,-3,128,idleTaskFunc,Default,NULL,Dynamic,NULL,NULL;rosPublish,-1,128,rosPublishTask,Default,NULL,Dynamic,NULL,NULL;voltage,-2,256,voltageTask,Default,NULL,Dynamic,NULL,NULL;canRx,3,256,canRxTask,As weak,NULL,Dynamic,NULL,NULL
FREERTOS.Tasks01=coreTask,3,1024,coreTaskFunc,Default,NULL,Dynamic,NULL,NULL;rosSpinTask,0,256,rosSpinTaskFunc,Default,NULL,Dynamic,NULL,NULL;idleTask,-3,128,idleTaskFunc,Default,NULL,Dynamic,NULL,NULL;rosPublish,-1,128,rosPublishTask,Default,NULL,Dynamic,NULL,NULL;voltage,-2,256,voltageTask,Default,NULL,Dynamic,NULL,NULL;canRx,3,256,canRxTask,As weak,NULL,Dynamic,NULL,NULL
FREERTOS.Timers01=coreTaskTimer,coreTaskEvokeCb,osTimerPeriodic,Default,NULL,Dynamic,NULL
FREERTOS.configTIMER_TASK_PRIORITY=6
FREERTOS.configTOTAL_HEAP_SIZE=25600
Expand Down Expand Up @@ -323,11 +323,12 @@ PB5.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
PB5.Locked=true
PB5.Mode=Full_Duplex_Master
PB5.Signal=SPI1_MOSI
PB6.GPIOParameters=GPIO_Speed,GPIO_FM6,GPIO_Label
PB6.GPIOParameters=GPIO_Speed,GPIO_FM6,PinState,GPIO_Label
PB6.GPIO_FM6=__NULL
PB6.GPIO_Label=IMUCS
PB6.GPIO_Speed=GPIO_SPEED_FREQ_MEDIUM
PB6.Locked=true
PB6.PinState=GPIO_PIN_SET
PB6.Signal=GPIO_Output
PB7.GPIOParameters=GPIO_Speed,PinState,GPIO_Label
PB7.GPIO_Label=SPI1_CS
Expand Down Expand Up @@ -589,3 +590,4 @@ 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
33 changes: 19 additions & 14 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 @@ -29,6 +29,10 @@ void ICM20948::init(SPI_HandleTypeDef* hspi, I2C_HandleTypeDef* hi2c, ros::NodeH

calib_indicator_time_ = HAL_GetTick();

#ifdef STM32H7
hspi_->Instance->CR1 |= (uint32_t)(SPI_BAUDRATEPRESCALER_64); //16 = 12.5Mhz, 200MHz
#endif

gyroInit();
accInit();
magInit();
Expand All @@ -49,8 +53,7 @@ void ICM20948::gyroInit(void)
// HAL_NVIC_SystemReset();
// }
}



/* 1.Clear all bits in ub0-ub3 register */
deviceReset();

Expand Down Expand Up @@ -140,16 +143,15 @@ bool ICM20948::magRead(Vector3f* data)

drdy = readSingleAk09916(MAG_ST1) & 0x01;
if(!drdy){
nh_->logerror("not ready");
// nh_->logerror("not ready");
return false;
}

temp = readMultipleAk09916(MAG_HXL, 6);
// HAL_Delay(1);

hofl = readSingleAk09916(MAG_ST2) & 0x08;
if(hofl){
nh_->logerror("overflow");
// nh_->logerror("overflow");
return false;
}

Expand Down Expand Up @@ -249,6 +251,7 @@ void ICM20948::spiModeEnable()
new_val |= 0x10;

writeSingleIcm20948(ub_0, B0_USER_CTRL, new_val);
HAL_Delay(100);
}

void ICM20948::i2cMasterReset()
Expand All @@ -257,6 +260,7 @@ void ICM20948::i2cMasterReset()
new_val |= 0x02;

writeSingleIcm20948(ub_0, B0_USER_CTRL, new_val);
HAL_Delay(100);
}

void ICM20948::i2cMasterEnable()
Expand All @@ -273,7 +277,8 @@ void ICM20948::i2cMasterClkFrq(uint8_t config)
uint8_t new_val = readSingleIcm20948(ub_3, B3_I2C_MST_CTRL);
new_val |= config;

writeSingleIcm20948(ub_3, B3_I2C_MST_CTRL, new_val);
writeSingleIcm20948(ub_3, B3_I2C_MST_CTRL, new_val);
HAL_Delay(100);
}

void ICM20948::setClockSource(uint8_t source)
Expand All @@ -282,11 +287,13 @@ void ICM20948::setClockSource(uint8_t source)
new_val |= source;

writeSingleIcm20948(ub_0, B0_PWR_MGMT_1, new_val);
HAL_Delay(100);
}

void ICM20948::odrAlignEnable()
{
writeSingleIcm20948(ub_2, B2_ODR_ALIGN_EN, 0x01);
HAL_Delay(100);
}

void ICM20948::setGyroLpf(uint8_t config)
Expand All @@ -308,6 +315,7 @@ void ICM20948::setAccelLpf(uint8_t config)
void ICM20948::setGyroSampleRate(uint8_t divider)
{
writeSingleIcm20948(ub_2, B2_GYRO_SMPLRT_DIV, divider);
HAL_Delay(100);
}

void ICM20948::setAccelSampleRate(uint16_t divider)
Expand All @@ -317,6 +325,7 @@ void ICM20948::setAccelSampleRate(uint16_t divider)

writeSingleIcm20948(ub_2, B2_ACCEL_SMPLRT_DIV_1, divider_1);
writeSingleIcm20948(ub_2, B2_ACCEL_SMPLRT_DIV_2, divider_2);
HAL_Delay(100);
}

void ICM20948::setMagOperationMode(operation_mode mode)
Expand Down Expand Up @@ -351,6 +360,7 @@ void ICM20948::setGyroFullScale(gyro_full_scale full_scale)
}

writeSingleIcm20948(ub_2, B2_GYRO_CONFIG_1, new_val);
HAL_Delay(100);
}

void ICM20948::setAccelFullScale(accel_full_scale full_scale)
Expand Down Expand Up @@ -378,6 +388,7 @@ void ICM20948::setAccelFullScale(accel_full_scale full_scale)
}

writeSingleIcm20948(ub_2, B2_ACCEL_CONFIG, new_val);
HAL_Delay(100);
}

void ICM20948::selectUserBank(userbank ub)
Expand All @@ -401,7 +412,6 @@ uint8_t ICM20948::readSingleIcm20948(userbank ub, uint8_t reg)
HAL_SPI_Transmit(hspi_, &read_reg, 1, 1000);
HAL_SPI_Receive(hspi_, &reg_val, 1, 1000);
GPIO_H(spi_cs_port_, spi_cs_pin_);

return reg_val;

}
Expand All @@ -411,9 +421,7 @@ void ICM20948::writeSingleIcm20948(userbank ub, uint8_t reg, uint8_t val)
uint8_t write_reg[2];
write_reg[0] = WRITE | reg;
write_reg[1] = val;

selectUserBank(ub);

GPIO_L(spi_cs_port_, spi_cs_pin_);
HAL_SPI_Transmit(hspi_, write_reg, 2, 1000);
GPIO_H(spi_cs_port_, spi_cs_pin_);
Expand All @@ -429,7 +437,6 @@ uint8_t* ICM20948::readMultipleIcm20948(userbank ub, uint8_t reg, uint8_t len)
HAL_SPI_Transmit(hspi_, &read_reg, 1, 1000);
HAL_SPI_Receive(hspi_, reg_val, len, 1000);
GPIO_H(spi_cs_port_, spi_cs_pin_);

return reg_val;
}

Expand All @@ -449,8 +456,7 @@ uint8_t ICM20948::readSingleAk09916(uint8_t reg)
writeSingleIcm20948(ub_3, B3_I2C_SLV0_ADDR, READ | MAG_SLAVE_ADDR);
writeSingleIcm20948(ub_3, B3_I2C_SLV0_REG, reg);
writeSingleIcm20948(ub_3, B3_I2C_SLV0_CTRL, 0x81);

return readSingleIcm20948(ub_0, B0_EXT_SLV_SENS_DATA_00);
return readSingleIcm20948(ub_0, B0_EXT_SLV_SENS_DATA_00);
}

void ICM20948::writeSingleAk09916(uint8_t reg, uint8_t val)
Expand All @@ -466,7 +472,6 @@ uint8_t* ICM20948::readMultipleAk09916(uint8_t reg, uint8_t len)
writeSingleIcm20948(ub_3, B3_I2C_SLV0_ADDR, READ | MAG_SLAVE_ADDR);
writeSingleIcm20948(ub_3, B3_I2C_SLV0_REG, reg);
writeSingleIcm20948(ub_3, B3_I2C_SLV0_CTRL, 0x80 | len);

return readMultipleIcm20948(ub_0, B0_EXT_SLV_SENS_DATA_00, len);
return readMultipleIcm20948(ub_0, B0_EXT_SLV_SENS_DATA_00, len);

}

0 comments on commit 2f77c27

Please sign in to comment.