From 3eac22f0b3dcf2a94d3ee7e5bfb03da46f92ae29 Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Tue, 12 Mar 2024 22:29:56 +0900 Subject: [PATCH] [Spinal][IMU][MPU9250] modified sevral parts to make it easier to inherit --- .../lib/Jsk_Lib/sensors/imu/imu_mpu9250.cpp | 90 ++++++++++--------- .../lib/Jsk_Lib/sensors/imu/imu_mpu9250.h | 46 +++++----- 2 files changed, 70 insertions(+), 66 deletions(-) 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/imu_mpu9250.cpp index d3f71a83e..b772aa6f5 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/imu_mpu9250.cpp @@ -105,52 +105,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 +148,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/imu_mpu9250.h index 152ef3000..3c1c6577b 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/imu_mpu9250.h @@ -9,8 +9,8 @@ #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" @@ -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);