[Spinal][IMU][MPU9250] modified sevral parts to make it easier to inh…

…erit
sugihara-16 · Mar 12, 2024 · 3eac22f · 3eac22f
1 parent 8375287
commit 3eac22f
Show file tree

Hide file tree

Showing 2 changed files with 70 additions and 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
@@ -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
@@ -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);