diff --git a/aerial_robot_nerve/spinal/mcu_project/boards/stm32F7/Src/main.c b/aerial_robot_nerve/spinal/mcu_project/boards/stm32F7/Src/main.c
index 0f0b45c79..436a63f0a 100644
--- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32F7/Src/main.c
+++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32F7/Src/main.c
@@ -42,7 +42,8 @@
/* Sensors */
#if IMU_FLAG
#include "sensors/imu/imu_ros_cmd.h"
-#include "sensors/imu/imu_mpu9250.h"
+#include "sensors/imu/drivers/mpu9250/imu_mpu9250.h"
+#include "sensors/imu/drivers/icm20948/icm_20948.h"
#endif
#if BARO_FLAG
@@ -98,8 +99,10 @@ bool start_processing_flag_ = false; //to prevent systick_callback starting bef
ros::NodeHandle nh_;
-#if IMU_FLAG
+#if IMU_MPU
IMUOnboard imu_;
+#elif IMU_ICM
+ICM20948 imu_;
#endif
#if BARO_FLAG
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
index d8f1012c0..7e5cf72a7 100644
--- 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/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 @@
-
+
@@ -209,7 +209,7 @@
-
+
@@ -341,4 +341,5 @@
+
\ No newline at end of file
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
index 57db08036..5821159b6 100644
--- 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
#include "flashmemory/flashmemory.h"
-#include "sensors/imu/imu_mpu9250.h"
+#include "sensors/imu/drivers/mpu9250/imu_mpu9250.h"
+#include "sensors/imu/drivers/icm20948/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,11 @@ osMailQId canMsgMailHandle;
ros::NodeHandle nh_;
/* sensor instances */
+#if IMU_MPU
IMUOnboard imu_;
+#elif IMU_ICM
+ICM20948 imu_;
+#endif
Baro baro_;
GPS gps_;
BatteryStatus battery_status_;
@@ -293,7 +298,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 */
@@ -650,10 +655,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;
@@ -981,7 +986,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_SET);
/*Configure GPIO pins : LED0_Pin LED1_Pin LED2_Pin */
GPIO_InitStruct.Pin = LED0_Pin|LED1_Pin|LED2_Pin;
@@ -997,6 +1002,13 @@ static void MX_GPIO_Init(void)
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM;
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;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
@@ -1055,7 +1067,6 @@ void coreTaskFunc(void const * argument)
#if NERVE_COMM
Spine::send();
#endif
-
imu_.update();
baro_.update();
gps_.update();
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 cc192e53b..00842a327 100644
--- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/spinal.ioc
+++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/spinal.ioc
@@ -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
@@ -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
@@ -322,12 +323,19 @@ 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
+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
PB8.Signal=I2C1_SCL
@@ -536,11 +544,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
index 498c8f6d3..a7d1e5007 100644
--- 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
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/icm20948/icm_20948.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/icm20948/icm_20948.cpp
new file mode 100644
index 000000000..ec0d60bb0
--- /dev/null
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/icm20948/icm_20948.cpp
@@ -0,0 +1,511 @@
+/*
+******************************************************************************
+* File Name : icm_20948.cpp
+* Description : IMU(ICM20948) Interface
+* Author : J.Sugihara 2024/3/11
+******************************************************************************
+*/
+
+#ifndef __cplusplus
+#error "Please define __cplusplus, because this is a c++ based file "
+#endif
+
+#include "sensors/imu/drivers/icm20948/icm_20948.h"
+
+void ICM20948::init(SPI_HandleTypeDef* hspi, I2C_HandleTypeDef* hi2c, ros::NodeHandle* nh,
+ GPIO_TypeDef* spi_cs_port, uint16_t spi_cs_pin,
+ GPIO_TypeDef* led_port, uint16_t led_pin)
+{
+ nh_ = nh;
+ IMU::init();
+
+ spi_cs_port_ = spi_cs_port;
+ spi_cs_pin_ = spi_cs_pin;
+ led_port_ = led_port;
+ led_pin_ = led_pin;
+
+ hspi_ = hspi;
+ hi2c_ = hi2c;
+
+ calib_indicator_time_ = HAL_GetTick();
+
+#ifdef STM32H7
+ hspi_->Instance->CR1 |= (uint32_t)(SPI_BAUDRATEPRESCALER_64);
+#endif
+
+ gyroInit();
+ accInit();
+ magInit();
+
+}
+
+void ICM20948::gyroInit(void)
+{
+ HAL_Delay(100);
+
+ /* Waiting for finding Imu */
+
+ while(!getIcm20948WhoAmI());
+
+ /* 1.Clear all bits in ub0-ub3 register */
+ deviceReset();
+
+ /* 2.Wakeup icm20948 and set fundamental properties */
+ setClockSource(1); // Clock source is automatically selected.
+ odrAlignEnable(); // Synchronize odr with sampling rates.
+ spiModeEnable(); // Use only SPI mode
+
+ /* 3.Gyro initialization (Do not change following order) */
+ setGyroLpf(3);
+ setGyroSampleRate(0);
+ setGyroFullScale(_2000dps);
+}
+
+void ICM20948::accInit (void)
+{
+ HAL_Delay(100);
+ /* 5.Acc initialization (Do not change following order) */
+ setAccelLpf(6);
+ setAccelSampleRate(0);
+ setAccelFullScale(_8g);
+}
+
+void ICM20948::magInit(void)
+{
+ HAL_Delay(100);
+ /* 6.1 Check whether use external magnetometer */
+ mag_id_ = IMUOnboard::checkExternalMag();
+
+ if(mag_id_ == INTERNAL_MAG)
+ {
+ /* 6.2 Enable communication between ICM20948 and AK09916*/
+ // ICM20948 can get magnetometer value from AK09916 via I2C using AUX_CL and AUX_DA. Then, we configure ICM20948 as a master device and AK09916 as a slave device.
+
+ // intPinBpEnable();
+ /* 6.2.1 Reset I2C bus*/
+ i2cMasterReset();
+ /* 6.2.2 Enable I2C bus*/
+ i2cMasterEnable();
+ /* 6.2.3 Set communication freq of I2C */
+ i2cMasterClkFrq(7); // 345.6 kHz / 46.67% dyty cycle (recoomended)
+
+ /* Waiting for finding Magnetometer */
+ while(!getAk09916WhoAmI());
+
+ /* 6.3 Clear all bits in mag register */
+ magSoftReset();
+ /* 6.4 Wakeup and set operation mode to magnetometer */
+ setMagOperationMode(continuous_measurement_100hz);
+ }
+}
+
+void ICM20948::updateRawData()
+{
+ gyroReadRad(&raw_gyro_adc_);
+ accelReadG(&raw_acc_adc_);
+ magReadUT(&raw_mag_adc_);
+ tempReadC(&raw_temp_adc_);
+}
+
+void ICM20948::gyroRead(Vector3f* data)
+{
+ uint8_t read_reg = READ | B0_GYRO_XOUT_H;
+ selectUserBank(ub_0);
+ readMultipleIcm20948(ub_0, B0_GYRO_XOUT_H);
+
+ data->x = (int16_t)(multi_adc_[0] << 8 | multi_adc_[1]);
+ data->y = (int16_t)(multi_adc_[2] << 8 | multi_adc_[3]);
+ data->z = (int16_t)(multi_adc_[4] << 8 | multi_adc_[5]);
+}
+
+void ICM20948::accelRead(Vector3f* data)
+{
+ readMultipleIcm20948(ub_0, B0_ACCEL_XOUT_H);
+
+ data->x = (int16_t)(multi_adc_[0] << 8 | multi_adc_[1]);
+ data->y = (int16_t)(multi_adc_[2] << 8 | multi_adc_[3]);
+ data->z = (int16_t)(multi_adc_[4] << 8 | multi_adc_[5]);
+}
+
+bool ICM20948::magRead(Vector3f* data)
+{
+ static uint32_t pre_mag_read;
+
+ /* following AK09916's update frequency*/
+ if(HAL_GetTick() - pre_mag_read > MAG_READ_INTERVAL)
+ pre_mag_read = HAL_GetTick();
+ else
+ return false;
+
+ uint8_t* temp;
+ uint8_t drdy, hofl; // data ready, overflow
+
+ readSingleAk09916(MAG_ST1);
+ drdy = single_adc_ & 0x01;
+ if(!drdy) return false;
+
+ readMultipleAk09916(MAG_HXL);
+
+ readSingleAk09916(MAG_ST2);
+ hofl = single_adc_ & 0x08;
+ if(hofl) return false;
+
+ data->x = (int16_t)(multi_adc_[1] << 8 | multi_adc_[0]);
+ data->y = (int16_t)(multi_adc_[3] << 8 | multi_adc_[2]);
+ data->z = (int16_t)(multi_adc_[5] << 8 | multi_adc_[4]);
+ return true;
+}
+
+void ICM20948::tempRead(float* data)
+{
+ readSingleIcm20948(ub_0, B0_TEMP_OUT_H);
+ uint8_t temp_high = single_adc_;
+ readSingleIcm20948(ub_0, B0_TEMP_OUT_L);
+ uint8_t temp_low = single_adc_;
+ *data = (float)(temp_high << 8 | temp_low);
+}
+
+void ICM20948::gyroReadRad(Vector3f* data)
+{
+ gyroRead(data);
+
+ data->x = data->x / gyro_scale_factor_ * M_PI / 180.0f;
+ data->y = data->y / gyro_scale_factor_ * M_PI / 180.0f;
+ data->z = data->z / gyro_scale_factor_ * M_PI / 180.0f;
+}
+
+void ICM20948::accelReadG(Vector3f* data)
+{
+ accelRead(data);
+ /*Temperature correction*/
+ float corrected_scale_factor = accel_scale_factor_+ accel_scale_factor_ * (0.026f * raw_temp_adc_ )/100.0f;
+ data->x = data->x / corrected_scale_factor * GRAVITY_MSS;
+ data->y = data->y / corrected_scale_factor * GRAVITY_MSS;
+ data->z = data->z / corrected_scale_factor * GRAVITY_MSS;
+}
+
+bool ICM20948::magReadUT(Vector3f* data)
+{
+ Vector3f temp;
+ bool new_data = magRead(&temp);
+ if(!new_data) return false;
+
+ data->x = (float)(temp.x * 0.15);
+ data->y = (float)(temp.y * 0.15);
+ data->z = (float)(temp.z * -0.15);
+
+ return true;
+}
+
+void ICM20948::tempReadC(float* data)
+{
+ tempRead(data);
+ *data = *data/333.87f + 21.0f;
+}
+
+/* Sub Functions */
+bool ICM20948::getIcm20948WhoAmI()
+{
+ readSingleIcm20948(ub_0, B0_WHO_AM_I);
+
+ if(single_adc_ == ICM20948_ID)
+ return true;
+ else
+ return false;
+}
+
+bool ICM20948::getAk09916WhoAmI()
+{
+ readSingleAk09916(MAG_WIA2);
+ if(single_adc_ == AK09916_ID)
+ return true;
+ else
+ return false;
+}
+
+void ICM20948::deviceReset()
+{
+ writeSingleIcm20948(ub_0, B0_PWR_MGMT_1, 0x80 | 0x41);
+ HAL_Delay(100);
+}
+
+void ICM20948::magSoftReset()
+{
+ writeSingleAk09916(MAG_CNTL3, 0x01);
+ HAL_Delay(100);
+}
+
+void ICM20948::wakeUp()
+{
+ readSingleIcm20948(ub_0, B0_PWR_MGMT_1);
+ uint8_t new_val = single_adc_ & 0xBF;
+
+ writeSingleIcm20948(ub_0, B0_PWR_MGMT_1, new_val);
+ HAL_Delay(100);
+}
+
+void ICM20948::sleep()
+{
+ readSingleIcm20948(ub_0, B0_PWR_MGMT_1);
+ uint8_t new_val = single_adc_ | 0x40;
+
+ writeSingleIcm20948(ub_0, B0_PWR_MGMT_1, new_val);
+ HAL_Delay(100);
+}
+
+void ICM20948::spiModeEnable()
+{
+ readSingleIcm20948(ub_0, B0_USER_CTRL);
+ uint8_t new_val = single_adc_ | 0x10;
+
+ HAL_Delay(100);
+
+ writeSingleIcm20948(ub_0, B0_USER_CTRL, new_val);
+ HAL_Delay(100);
+}
+
+void ICM20948::intPinBpEnable()
+{
+ uint8_t new_val = 0x01;
+ writeSingleIcm20948(ub_0, B0_INT_PIN_CFG, new_val);
+ HAL_Delay(100);
+}
+
+void ICM20948::i2cMasterReset()
+{
+ readSingleIcm20948(ub_0, B0_USER_CTRL);
+ uint8_t new_val = single_adc_ | 0x02;
+
+ HAL_Delay(100);
+
+ writeSingleIcm20948(ub_0, B0_USER_CTRL, new_val);
+ HAL_Delay(100);
+}
+
+void ICM20948::i2cMasterEnable()
+{
+ readSingleIcm20948(ub_0, B0_USER_CTRL);
+ uint8_t new_val = single_adc_ | 0x20;
+
+ HAL_Delay(100);
+
+ writeSingleIcm20948(ub_0, B0_USER_CTRL, new_val);
+ HAL_Delay(100);
+}
+
+void ICM20948::i2cMasterClkFrq(uint8_t config)
+{
+ readSingleIcm20948(ub_3, B3_I2C_MST_CTRL);
+ uint8_t new_val = single_adc_ | config;
+
+ HAL_Delay(100);
+
+ writeSingleIcm20948(ub_3, B3_I2C_MST_CTRL, new_val);
+ HAL_Delay(100);
+}
+
+void ICM20948::i2cOdrCfg(uint8_t config)
+{
+ /* ODR = 1.1 kHz/(2^config) */
+ uint8_t new_val = config;
+ writeSingleIcm20948(ub_0, B3_I2C_MST_ODR_CONFIG, new_val);
+ HAL_Delay(100);
+}
+
+void ICM20948::setClockSource(uint8_t source)
+{
+ uint8_t new_val = 0x01;
+
+ 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)
+{
+ readSingleIcm20948(ub_2, B2_GYRO_CONFIG_1);
+ uint8_t new_val = single_adc_ | (config << 3);
+
+ writeSingleIcm20948(ub_2, B2_GYRO_CONFIG_1, new_val);
+}
+
+void ICM20948::setAccelLpf(uint8_t config)
+{
+ readSingleIcm20948(ub_2, B2_ACCEL_CONFIG);
+ uint8_t new_val = single_adc_ | (config << 3);
+
+ writeSingleIcm20948(ub_2, B2_ACCEL_CONFIG, new_val);
+}
+
+void ICM20948::setGyroSampleRate(uint8_t divider)
+{
+ writeSingleIcm20948(ub_2, B2_GYRO_SMPLRT_DIV, divider);
+ HAL_Delay(100);
+}
+
+void ICM20948::setAccelSampleRate(uint16_t divider)
+{
+ uint8_t divider_1 = (uint8_t)(divider >> 8);
+ uint8_t divider_2 = (uint8_t)(0x0F & 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)
+{
+ writeSingleAk09916(MAG_CNTL2, mode);
+ HAL_Delay(100);
+}
+
+
+void ICM20948::setGyroFullScale(gyro_full_scale full_scale)
+{
+ readSingleIcm20948(ub_2, B2_GYRO_CONFIG_1);
+ uint8_t new_val = single_adc_;
+
+ switch(full_scale)
+ {
+ case _250dps :
+ new_val |= 0x00;
+ gyro_scale_factor_ = 131.0;
+ break;
+ case _500dps :
+ new_val |= 0x02;
+ gyro_scale_factor_ = 65.5;
+ break;
+ case _1000dps :
+ new_val |= 0x04;
+ gyro_scale_factor_ = 32.8;
+ break;
+ case _2000dps :
+ new_val |= 0x06;
+ gyro_scale_factor_ = 16.4;
+ break;
+ }
+
+ writeSingleIcm20948(ub_2, B2_GYRO_CONFIG_1, new_val);
+ HAL_Delay(100);
+}
+
+void ICM20948::setAccelFullScale(accel_full_scale full_scale)
+{
+ readSingleIcm20948(ub_2, B2_ACCEL_CONFIG);
+ uint8_t new_val = single_adc_;
+
+ HAL_Delay(100);
+
+ switch(full_scale)
+ {
+ case _2g :
+ new_val |= 0x00;
+ accel_scale_factor_ = 16384;
+ break;
+ case _4g :
+ new_val |= 0x02;
+ accel_scale_factor_ = 8192;
+ break;
+ case _8g :
+ new_val |= 0x04;
+ accel_scale_factor_ = 4096;
+ break;
+ case _16g :
+ new_val |= 0x06;
+ accel_scale_factor_ = 2048;
+ break;
+ }
+
+ writeSingleIcm20948(ub_2, B2_ACCEL_CONFIG, new_val);
+ HAL_Delay(100);
+}
+
+void ICM20948::selectUserBank(userbank ub)
+{
+ uint8_t write_reg[2];
+ write_reg[0] = WRITE | REG_BANK_SEL;
+ write_reg[1] = ub;
+
+ GPIO_L(spi_cs_port_, spi_cs_pin_);
+ HAL_SPI_Transmit(hspi_, write_reg, 2, 10);
+ GPIO_H(spi_cs_port_, spi_cs_pin_);
+}
+
+void ICM20948::readSingleIcm20948(userbank ub, uint8_t reg)
+{
+ uint8_t read_reg = READ | reg;
+ selectUserBank(ub);
+
+ GPIO_L(spi_cs_port_, spi_cs_pin_);
+ HAL_SPI_Transmit(hspi_, &read_reg, 1, 1000);
+ HAL_SPI_Receive(hspi_, &single_adc_, 1, 1000);
+ GPIO_H(spi_cs_port_, spi_cs_pin_);
+}
+
+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_);
+}
+
+void ICM20948::readMultipleIcm20948(userbank ub, uint8_t reg)
+{
+ uint8_t read_reg = READ | reg;
+ uint8_t len = (uint8_t)(sizeof(multi_adc_)/sizeof(multi_adc_[0]));
+ selectUserBank(ub);
+
+ GPIO_L(spi_cs_port_, spi_cs_pin_);
+ HAL_SPI_Transmit(hspi_, &read_reg, 1, 1000);
+ HAL_SPI_Receive(hspi_, multi_adc_, len, 1000);
+ GPIO_H(spi_cs_port_, spi_cs_pin_);
+
+}
+
+void ICM20948::writeMultipleIcm20948(userbank ub, uint8_t reg, uint8_t* val, uint8_t len)
+{
+ uint8_t write_reg = WRITE | reg;
+ selectUserBank(ub);
+
+ GPIO_L(spi_cs_port_, spi_cs_pin_);
+ HAL_SPI_Transmit(hspi_, &write_reg, 1, 1000);
+ HAL_SPI_Transmit(hspi_, val, len, 1000);
+ GPIO_H(spi_cs_port_, spi_cs_pin_);
+}
+
+void 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);
+ readSingleIcm20948(ub_0, B0_EXT_SLV_SENS_DATA_00);
+}
+
+void ICM20948::writeSingleAk09916(uint8_t reg, uint8_t val)
+{
+ writeSingleIcm20948(ub_3, B3_I2C_SLV0_ADDR, WRITE | MAG_SLAVE_ADDR);
+ writeSingleIcm20948(ub_3, B3_I2C_SLV0_REG, reg);
+ writeSingleIcm20948(ub_3, B3_I2C_SLV0_DO, val);
+ HAL_Delay(1);
+ writeSingleIcm20948(ub_3, B3_I2C_SLV0_CTRL, 0x81);
+}
+
+void ICM20948::readMultipleAk09916(uint8_t reg)
+{
+ uint8_t len = (uint8_t)(sizeof(multi_adc_)/sizeof(multi_adc_[0]));
+ 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);
+ HAL_Delay(1);
+ readMultipleIcm20948(ub_0, B0_EXT_SLV_SENS_DATA_00);
+}
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/icm20948/icm_20948.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/icm20948/icm_20948.h
new file mode 100644
index 000000000..9ed7178ed
--- /dev/null
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/icm20948/icm_20948.h
@@ -0,0 +1,306 @@
+/*
+******************************************************************************
+* File Name : icm_mpu9250.h
+* Description : IMU(ICM20948) Interface
+* Author : J.Sugihara 2024/3/11
+******************************************************************************
+*/
+
+#ifndef __cplusplus
+#error "Please define __cplusplus, because this is a c++ based file "
+#endif
+
+#ifndef __IMU_ICM_H
+#define __IMU_ICM_H
+
+#include "config.h"
+#include "sensors/imu/drivers/mpu9250/imu_mpu9250.h"
+
+
+#define SENSOR_DATA_LENGTH 7
+#define IMU_FIND_TIMEOUT 10000
+#define MAG_FIND_TIMEOUT 10000
+
+#define MAG_READ_INTERVAL 10
+
+/* ICM-20948 Registers */
+#define ICM20948_ID 0xEA
+#define REG_BANK_SEL 0x7F
+
+// USER BANK 0
+#define B0_WHO_AM_I 0x00
+#define B0_USER_CTRL 0x03
+#define B0_LP_CONFIG 0x05
+#define B0_PWR_MGMT_1 0x06
+#define B0_PWR_MGMT_2 0x07
+#define B0_INT_PIN_CFG 0x0F
+#define B0_INT_ENABLE 0x10
+#define B0_INT_ENABLE_1 0x11
+#define B0_INT_ENABLE_2 0x12
+#define B0_INT_ENABLE_3 0x13
+#define B0_I2C_MST_STATUS 0x17
+#define B0_INT_STATUS 0x19
+#define B0_INT_STATUS_1 0x1A
+#define B0_INT_STATUS_2 0x1B
+#define B0_INT_STATUS_3 0x1C
+#define B0_DELAY_TIMEH 0x28
+#define B0_DELAY_TIMEL 0x29
+#define B0_ACCEL_XOUT_H 0x2D
+#define B0_ACCEL_XOUT_L 0x2E
+#define B0_ACCEL_YOUT_H 0x2F
+#define B0_ACCEL_YOUT_L 0x30
+#define B0_ACCEL_ZOUT_H 0x31
+#define B0_ACCEL_ZOUT_L 0x32
+#define B0_GYRO_XOUT_H 0x33
+#define B0_GYRO_XOUT_L 0x34
+#define B0_GYRO_YOUT_H 0x35
+#define B0_GYRO_YOUT_L 0x36
+#define B0_GYRO_ZOUT_H 0x37
+#define B0_GYRO_ZOUT_L 0x38
+#define B0_TEMP_OUT_H 0x39
+#define B0_TEMP_OUT_L 0x3A
+#define B0_EXT_SLV_SENS_DATA_00 0x3B
+#define B0_EXT_SLV_SENS_DATA_01 0x3C
+#define B0_EXT_SLV_SENS_DATA_02 0x3D
+#define B0_EXT_SLV_SENS_DATA_03 0x3E
+#define B0_EXT_SLV_SENS_DATA_04 0x3F
+#define B0_EXT_SLV_SENS_DATA_05 0x40
+#define B0_EXT_SLV_SENS_DATA_06 0x41
+#define B0_EXT_SLV_SENS_DATA_07 0x42
+#define B0_EXT_SLV_SENS_DATA_08 0x43
+#define B0_EXT_SLV_SENS_DATA_09 0x44
+#define B0_EXT_SLV_SENS_DATA_10 0x45
+#define B0_EXT_SLV_SENS_DATA_11 0x46
+#define B0_EXT_SLV_SENS_DATA_12 0x47
+#define B0_EXT_SLV_SENS_DATA_13 0x48
+#define B0_EXT_SLV_SENS_DATA_14 0x49
+#define B0_EXT_SLV_SENS_DATA_15 0x4A
+#define B0_EXT_SLV_SENS_DATA_16 0x4B
+#define B0_EXT_SLV_SENS_DATA_17 0x4C
+#define B0_EXT_SLV_SENS_DATA_18 0x4D
+#define B0_EXT_SLV_SENS_DATA_19 0x4E
+#define B0_EXT_SLV_SENS_DATA_20 0x4F
+#define B0_EXT_SLV_SENS_DATA_21 0x50
+#define B0_EXT_SLV_SENS_DATA_22 0x51
+#define B0_EXT_SLV_SENS_DATA_23 0x52
+#define B0_FIFO_EN_1 0x66
+#define B0_FIFO_EN_2 0x67
+#define B0_FIFO_RST 0x68
+#define B0_FIFO_MODE 0x69
+#define B0_FIFO_COUNTH 0X70
+#define B0_FIFO_COUNTL 0X71
+#define B0_FIFO_R_W 0x72
+#define B0_DATA_RDY_STATUS 0x74
+#define B0_FIFO_CFG 0x76
+
+// USER BANK 1
+#define B1_SELF_TEST_X_GYRO 0x02
+#define B1_SELF_TEST_Y_GYRO 0x03
+#define B1_SELF_TEST_Z_GYRO 0x04
+#define B1_SELF_TEST_X_ACCEL 0x0E
+#define B1_SELF_TEST_Y_ACCEL 0x0F
+#define B1_SELF_TEST_Z_ACCEL 0x10
+#define B1_XA_OFFS_H 0x14
+#define B1_XA_OFFS_L 0x15
+#define B1_YA_OFFS_H 0x17
+#define B1_YA_OFFS_L 0x18
+#define B1_ZA_OFFS_H 0x1A
+#define B1_ZA_OFFS_L 0x1B
+#define B1_TIMEBASE_CORRECTION_PLL 0x28
+
+// USER BANK 2
+#define B2_GYRO_SMPLRT_DIV 0x00
+#define B2_GYRO_CONFIG_1 0x01
+#define B2_GYRO_CONFIG_2 0x02
+#define B2_XG_OFFS_USRH 0x03
+#define B2_XG_OFFS_USRL 0x04
+#define B2_YG_OFFS_USRH 0x05
+#define B2_YG_OFFS_USRL 0x06
+#define B2_ZG_OFFS_USRH 0x07
+#define B2_ZG_OFFS_USRL 0x08
+#define B2_ODR_ALIGN_EN 0x09
+#define B2_ACCEL_SMPLRT_DIV_1 0x10
+#define B2_ACCEL_SMPLRT_DIV_2 0x11
+#define B2_ACCEL_INTEL_CTRL 0x12
+#define B2_ACCEL_WOM_THR 0x13
+#define B2_ACCEL_CONFIG 0x14
+#define B2_ACCEL_CONFIG_2 0x15
+#define B2_FSYNC_CONFIG 0x52
+#define B2_TEMP_CONFIG 0x53
+#define B2_MOD_CTRL_USR 0X54
+
+// USER BANK 3#
+#define B3_I2C_MST_ODR_CONFIG 0x00
+#define B3_I2C_MST_CTRL 0x01
+#define B3_I2C_MST_DELAY_CTRL 0x02
+#define B3_I2C_SLV0_ADDR 0x03
+#define B3_I2C_SLV0_REG 0x04
+#define B3_I2C_SLV0_CTRL 0x05
+#define B3_I2C_SLV0_DO 0x06
+#define B3_I2C_SLV1_ADDR 0x07
+#define B3_I2C_SLV1_REG 0x08
+#define B3_I2C_SLV1_CTRL 0x09
+#define B3_I2C_SLV1_DO 0x0A
+#define B3_I2C_SLV2_ADDR 0x0B
+#define B3_I2C_SLV2_REG 0x0C
+#define B3_I2C_SLV2_CTRL 0x0D
+#define B3_I2C_SLV2_DO 0x0E
+#define B3_I2C_SLV3_ADDR 0x0F
+#define B3_I2C_SLV3_REG 0x10
+#define B3_I2C_SLV3_CTRL 0x11
+#define B3_I2C_SLV3_DO 0x12
+#define B3_I2C_SLV4_ADDR 0x13
+#define B3_I2C_SLV4_REG 0x14
+#define B3_I2C_SLV4_CTRL 0x15
+#define B3_I2C_SLV4_DO 0x16
+#define B3_I2C_SLV4_DI 0x17
+
+
+/* AK09916 Registers */
+#define AK09916_ID 0x09
+#define MAG_SLAVE_ADDR 0x0C
+
+#define MAG_WIA2 0x01
+#define MAG_ST1 0x10
+
+#define MAG_HXL 0x11
+#define MAG_HXH 0x12
+#define MAG_HYL 0x13
+#define MAG_HYH 0x14
+#define MAG_HZL 0x15
+#define MAG_HZH 0x16
+
+#define MAG_ST2 0x18
+#define MAG_CNTL2 0x31
+#define MAG_CNTL3 0x32
+#define MAG_TS1 0x33
+#define MAG_TS2 0x34
+
+/* Intructions */
+#define READ 0x80
+#define WRITE 0x00
+
+/* Typedefs */
+typedef enum
+ {
+ ub_0 = 0 << 4,
+ ub_1 = 1 << 4,
+ ub_2 = 2 << 4,
+ ub_3 = 3 << 4
+ } userbank;
+
+typedef enum
+ {
+ _250dps,
+ _500dps,
+ _1000dps,
+ _2000dps
+ } gyro_full_scale;
+
+typedef enum
+ {
+ _2g,
+ _4g,
+ _8g,
+ _16g
+ } accel_full_scale;
+
+typedef enum
+ {
+ /*
+ You can find this informatoin at https://www.y-ic.es/datasheet/78/SMDSW.020-2OZ.pdf
+ */
+ power_down_mode = 0,
+ single_measurement_mode = 1,
+ continuous_measurement_10hz = 2,
+ continuous_measurement_20hz = 4,
+ continuous_measurement_50hz = 6,
+ continuous_measurement_100hz = 8
+ } operation_mode;
+
+class ICM20948 : public IMUOnboard {
+public:
+ ICM20948():IMUOnboard(){}
+ ~ICM20948(){}
+ void init(SPI_HandleTypeDef* hspi, I2C_HandleTypeDef* hi2c, ros::NodeHandle* nh,
+ GPIO_TypeDef* spi_cs_port, uint16_t spi_cs_pin,
+ GPIO_TypeDef* led_port, uint16_t led_pin);
+
+ enum{INTERNAL_MAG = 0, LIS3MDL = 1, HMC58X3 = 2,};
+
+private:
+ float gyro_scale_factor_;
+ float accel_scale_factor_;
+
+ /* raw adc data */
+ uint8_t single_adc_;
+ uint8_t multi_adc_[6];
+ float raw_temp_adc_;
+
+ /* Initialization */
+ void gyroInit(void) override;
+ void accInit(void) override;
+ void magInit(void) override;
+
+ /* Update */
+ void updateRawData (void) override;
+
+ /* Raw 16 bits adc data reading function */
+ void gyroRead(Vector3f* data);
+ void accelRead(Vector3f* data);
+ bool magRead(Vector3f* data);
+ void tempRead(float* data);
+
+ /* Conversion from raw adc data to proper units */
+ void gyroReadRad(Vector3f* data);
+ void accelReadG(Vector3f* data);
+ bool magReadUT(Vector3f* data);
+ void tempReadC(float* data);
+
+ /* Sub functions */
+ bool getIcm20948WhoAmI();
+ bool getAk09916WhoAmI();
+
+ void deviceReset();
+ void magSoftReset();
+
+ void wakeUp();
+ void sleep();
+
+ void spiModeEnable();
+
+ void intPinBpEnable();
+ void i2cMasterReset();
+ void i2cMasterEnable();
+ void i2cMasterClkFrq(uint8_t config); // 0 - 15
+ void i2cOdrCfg(uint8_t config);
+
+ void setClockSource(uint8_t source);
+ void odrAlignEnable();
+
+ void setGyroLpf(uint8_t config); // 0 - 7
+ void setAccelLpf(uint8_t config); // 0 - 7
+
+ void setGyroSampleRate(uint8_t divider);
+ void setAccelSampleRate(uint16_t divider);
+ void setMagOperationMode(operation_mode mode);
+
+ void setGyroFullScale(gyro_full_scale full_scale);
+ void setAccelFullScale(accel_full_scale full_scale);
+
+ /* Fundamental functions */
+ void selectUserBank(userbank ub);
+
+ void readSingleIcm20948(userbank ub, uint8_t reg);
+ void writeSingleIcm20948(userbank ub, uint8_t reg, uint8_t val);
+
+ void readMultipleIcm20948(userbank ub, uint8_t reg);
+ void writeMultipleIcm20948(userbank ub, uint8_t reg, uint8_t* val, uint8_t len);
+
+ void readSingleAk09916(uint8_t reg);
+ void writeSingleAk09916(uint8_t reg, uint8_t val);
+
+ void readMultipleAk09916(uint8_t reg);
+
+};
+#endif
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/icm20948/references/AK09916.pdf b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/icm20948/references/AK09916.pdf
new file mode 100644
index 000000000..07ec8f623
Binary files /dev/null and b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/icm20948/references/AK09916.pdf differ
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/icm20948/references/DS-000189-ICM-20948-v1.3.pdf b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/icm20948/references/DS-000189-ICM-20948-v1.3.pdf
new file mode 100644
index 000000000..f8194c139
Binary files /dev/null and b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/icm20948/references/DS-000189-ICM-20948-v1.3.pdf differ
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_mpu9250.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/mpu9250/imu_mpu9250.cpp
similarity index 97%
rename from aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_mpu9250.cpp
rename to aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/mpu9250/imu_mpu9250.cpp
index d3f71a83e..87bc975bf 100644
--- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_mpu9250.cpp
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/mpu9250/imu_mpu9250.cpp
@@ -8,8 +8,7 @@
#ifndef __cplusplus
#error "Please define __cplusplus, because this is a c++ based file "
#endif
-
-#include "sensors/imu/imu_mpu9250.h"
+#include "sensors/imu/drivers/mpu9250/imu_mpu9250.h"
uint8_t IMUOnboard::adc_[SENSOR_DATA_LENGTH];
uint32_t IMUOnboard::last_mag_time_;
@@ -105,52 +104,11 @@ void IMUOnboard::accInit (void)
void IMUOnboard::magInit(void)
{
/* check whether use external magnetometer */
- /*
- * we need check the i2c connectio with external mag(HMC5883L) several times initially,
- * since the module UBlox M8N has some problem with the gps/mag simultaneous polling.
- */
-
- HAL_Delay(4000); // wait for the setup of the Blox M8N module, especially for winter
- mag_id_ = INTERNAL_MAG;
- uint8_t val[2];
- int i2c_status = 1;
-
- for(int i = 0; i < EXTERNAL_MAG_CHECK_COUNT; i ++)
- {
- /* check the existence of external magnetometer */
-
- /* 1. LIS3MDL */
- val[0] = LIS3MDL_PING;
- i2c_status = HAL_I2C_Master_Transmit(hi2c_, LIS3MDL_MAG_REGISTER, val, 1, 100);
- HAL_I2C_Master_Receive(hi2c_, LIS3MDL_MAG_REGISTER, val, 1, 100);
-
- if(i2c_status == HAL_OK && val[0] == 61) //0b111101
- {
- mag_id_ = LIS3MDL;
- HAL_Delay(10);
- lis3mdlInit();
- break;
- }
-
- /* 2. HMC58X3 */
- val[0] = HMC58X3_R_CONFB;
- val[1] = 0x20;
- i2c_status = HAL_I2C_Master_Transmit(hi2c_, HMC58X3_MAG_REGISTER, val, 2, 100);
-
- if(i2c_status == HAL_OK)
- {
- mag_id_ = HMC58X3;
- HAL_Delay(10);
- hmc58x3Init();
- break;
- }
- HAL_Delay(10);
- }
-
+ mag_id_ = checkExternalMag();
if(mag_id_ == INTERNAL_MAG)
- {/* use internal mag */
-
+ {
+ /* use internal mag */
HAL_Delay(10);
//at this stage, the MAG is configured via the original MAG init function in I2C bypass mode
//now we configure MPU as a I2C Master device to handle the MAG via the I2C AUX port (done here for HMC5883)
@@ -189,6 +147,49 @@ void IMUOnboard::magInit(void)
last_mag_time_ = HAL_GetTick();
}
+uint8_t IMUOnboard::checkExternalMag(void)
+{
+ HAL_Delay(4000); // wait for the setup of the Blox M8N module, especially for winter
+ uint8_t val[2];
+ int i2c_status = 1;
+
+ /*
+ * we need check the i2c connectio with external mag(HMC5883L) several times initially,
+ * since the module UBlox M8N has some problem with the gps/mag simultaneous polling.
+ */
+
+ for(int i = 0; i < EXTERNAL_MAG_CHECK_COUNT; i ++)
+ {
+ /* check the existence of external magnetometer */
+
+ /* 1. LIS3MDL */
+ val[0] = LIS3MDL_PING;
+ i2c_status = HAL_I2C_Master_Transmit(hi2c_, LIS3MDL_MAG_REGISTER, val, 1, 100);
+ HAL_I2C_Master_Receive(hi2c_, LIS3MDL_MAG_REGISTER, val, 1, 100);
+
+ if(i2c_status == HAL_OK && val[0] == 61) //0b111101
+ {
+ HAL_Delay(10);
+ lis3mdlInit();
+ return LIS3MDL;
+ }
+
+ /* 2. HMC58X3 */
+ val[0] = HMC58X3_R_CONFB;
+ val[1] = 0x20;
+ i2c_status = HAL_I2C_Master_Transmit(hi2c_, HMC58X3_MAG_REGISTER, val, 2, 100);
+
+ if(i2c_status == HAL_OK)
+ {
+ HAL_Delay(10);
+ hmc58x3Init();
+ return HMC58X3;
+ }
+ HAL_Delay(10);
+ }
+ return INTERNAL_MAG;
+}
+
void IMUOnboard::lis3mdlInit(void)
{
uint8_t val[2];
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_mpu9250.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/mpu9250/imu_mpu9250.h
similarity index 94%
rename from aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_mpu9250.h
rename to aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/mpu9250/imu_mpu9250.h
index 152ef3000..2e879a5cc 100644
--- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_mpu9250.h
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/mpu9250/imu_mpu9250.h
@@ -9,12 +9,12 @@
#error "Please define __cplusplus, because this is a c++ based file "
#endif
-#ifndef __IMU_H
-#define __IMU_H
+#ifndef __IMU_MPU_H
+#define __IMU_MPU_H
#include "config.h"
#include "math/AP_Math.h"
-#include "imu_basic.h"
+#include "sensors/imu/imu_basic.h"
#define SENSOR_DATA_LENGTH 7
@@ -28,6 +28,25 @@ class IMUOnboard : public IMU {
enum{INTERNAL_MAG = 0, LIS3MDL = 1, HMC58X3 = 2,};
+protected:
+ static uint8_t adc_[SENSOR_DATA_LENGTH];
+ static uint32_t last_mag_time_;
+
+ uint8_t mag_id_;
+
+ SPI_HandleTypeDef* hspi_;
+ I2C_HandleTypeDef* hi2c_;
+ ros::NodeHandle* nh_;
+
+ GPIO_TypeDef* spi_cs_port_;
+ uint16_t spi_cs_pin_;
+ GPIO_TypeDef* led_port_;
+ uint16_t led_pin_;
+
+ bool use_external_mag_flag_;
+ uint32_t calib_indicator_time_; // ms
+
+ uint8_t checkExternalMag(void);
private:
static const uint8_t GYRO_DLPF_CFG = 0x03;//0x01 //0: 250Hz, 0.97ms; 3: 41Hz, 5.9ms(kduino); 4: 20Hz: 9.9ms
static const uint8_t ACC_DLPF_CFG = 0x05; //0x03: 41Hz, 11.80ms
@@ -73,26 +92,9 @@ class IMUOnboard : public IMU {
static const uint32_t HMC58X3_READ_PRESCALER = 13; // 1000Hz/13= 75Hz
static constexpr float HMC58X3_RATE = 0.092; // 0.92mG/LSb (1.3G) -> 1G = 10e-4T -> 0.092mT/LSb
- static uint8_t adc_[SENSOR_DATA_LENGTH];
- static uint32_t last_mag_time_;
-
- uint8_t mag_id_;
-
- SPI_HandleTypeDef* hspi_;
- I2C_HandleTypeDef* hi2c_;
- ros::NodeHandle* nh_;
-
- GPIO_TypeDef* spi_cs_port_;
- uint16_t spi_cs_pin_;
- GPIO_TypeDef* led_port_;
- uint16_t led_pin_;
-
- bool use_external_mag_flag_;
- uint32_t calib_indicator_time_; // ms
-
- void gyroInit(void);
- void accInit(void);
- void magInit(void);
+ virtual void gyroInit(void);
+ virtual void accInit(void);
+ virtual void magInit(void);
// external mag
void hmc58x3Init(void);
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/mpu9250/references/PS-MPU-9250A-01-v1.1.pdf b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/mpu9250/references/PS-MPU-9250A-01-v1.1.pdf
new file mode 100644
index 000000000..ea405c73c
Binary files /dev/null and b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/mpu9250/references/PS-MPU-9250A-01-v1.1.pdf differ
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/mpu9250/references/RM-MPU-9250A-00-v1.6.pdf b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/mpu9250/references/RM-MPU-9250A-00-v1.6.pdf
new file mode 100644
index 000000000..7dff038a5
Binary files /dev/null and b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/mpu9250/references/RM-MPU-9250A-00-v1.6.pdf differ
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/mpu9250/references/SMDSW.020-2OZ.pdf b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/mpu9250/references/SMDSW.020-2OZ.pdf
new file mode 100644
index 000000000..07ec8f623
Binary files /dev/null and b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/mpu9250/references/SMDSW.020-2OZ.pdf differ
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/altitude/altitude_estimate.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/altitude/altitude_estimate.h
index 0789270eb..38ba2e9d1 100644
--- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/altitude/altitude_estimate.h
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/altitude/altitude_estimate.h
@@ -21,7 +21,8 @@
////////////////////////////////////////
//TODO: should include the super class//
////////////////////////////////////////
-#include "sensors/imu/imu_mpu9250.h"
+#include "sensors/imu/drivers/mpu9250/imu_mpu9250.h"
+#include "sensors/imu/drivers/icm20948/icm_20948.h"
#include "sensors/baro/baro_ms5611.h"
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/attitude/attitude_estimate.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/attitude/attitude_estimate.h
index f9cf37518..b21f029b8 100644
--- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/attitude/attitude_estimate.h
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/attitude/attitude_estimate.h
@@ -30,7 +30,8 @@
#ifdef SIMULATION
#include
#else
-#include "sensors/imu/imu_mpu9250.h"
+#include "sensors/imu/drivers/mpu9250/imu_mpu9250.h"
+#include "sensors/imu/drivers/icm20948/icm_20948.h"
#include "sensors/gps/gps_ublox.h"
#endif
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/pos/pos_estimate.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/pos/pos_estimate.h
index bb01e80c3..0e2dc43bb 100644
--- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/pos/pos_estimate.h
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/pos/pos_estimate.h
@@ -20,7 +20,8 @@
////////////////////////////////////////
//TODO: should include the super class//
////////////////////////////////////////
-#include "sensors/imu/imu_mpu9250.h"
+#include "sensors/imu/drivers/mpu9250/imu_mpu9250.h"
+#include "sensors/imu/drivers/icm20948/icm_20948.h"
#include "sensors/gps/gps_ublox.h"
class PosEstimate
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/state_estimate.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/state_estimate.h
index dffd32702..0ff14f987 100644
--- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/state_estimate.h
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/state_estimate.h
@@ -18,7 +18,8 @@
#include
/* sensors */
-#include "sensors/imu/imu_mpu9250.h"
+#include "sensors/imu/drivers/mpu9250/imu_mpu9250.h"
+#include "sensors/imu/drivers/icm20948/icm_20948.h"
#include "sensors/baro/baro_ms5611.h"
#include "sensors/gps/gps_ublox.h"
#endif