[spinal][imu] set flag to get raw sensor data during magnetmeter cali…

…bration
sugikazu75 · Nov 3, 2024 · 32db881 · 32db881
1 parent 47e6c27
commit 32db881
Show file tree

Hide file tree

Showing 6 changed files with 35 additions and 2 deletions.
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_basic.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_basic.cpp
@@ -26,6 +26,7 @@ void IMU::init()
   calib_gyro_ = true;
   calib_acc_ = false;
   calib_mag_ = false;
+  mag_lsm_calib_ = false;
   gyro_calib_duration_ = GYRO_DEFAULT_CALIB_DURATION * 1000;
   acc_calib_duration_ = ACC_DEFAULT_CALIB_DURATION * 1000;
   mag_calib_duration_ = MAG_DEFAULT_CALIB_DURATION * 1000;

diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_basic.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_basic.h
@@ -48,6 +48,7 @@ class IMU
   Vector3f raw_gyro_adc_, raw_acc_adc_, raw_mag_adc_;
 
   bool calib_acc_, calib_gyro_, calib_mag_;
+  bool mag_lsm_calib_;
 
 public:
   IMU();
@@ -86,6 +87,7 @@ class IMU
     if(!calib_acc_ && !calib_gyro_ && !calib_mag_) return true;
     else return false;
   }
+  bool getMagLsmCalib() {return mag_lsm_calib_;}
   void update();
   inline Vector3f getGyroBias() {return gyro_bias_;}
   inline Vector3f getAccBias() {return acc_bias_;}
@@ -95,6 +97,7 @@ class IMU
   inline void setAccBias(Vector3f data) { acc_bias_ = data;}
   inline void setMagBias(Vector3f data) { mag_bias_ = data;}
   inline void setMagScale(Vector3f data) { mag_scale_ = data;}
+  inline void setMagLsmCalib(bool flag) {mag_lsm_calib_ = flag;}
   void gyroCalib(bool flag, float duration);
   void accCalib(bool flag, float duration);
   void magCalib(bool flag, float duration);

diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_ros_cmd.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/imu_ros_cmd.cpp
@@ -154,6 +154,17 @@ namespace IMU_ROS_CMD
           res.data_length = 0;
           break;
         }
+      case spinal::ImuCalib::Request::MAG_LSM_CALIB:
+        {
+          bool calib_flag = req.data[0]; // implicit casting from float to bool
+
+          for(unsigned int i = 0; i < imu_.size(); i++)
+            imu_.at(i)->setMagLsmCalib(calib_flag);
+
+          res.success = true;
+          res.data_length = 0;
+          break;
+        }
       default:
         {
           nh_->logwarn("unsupported command");

diff --git a/...al_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/attitude/attitude_estimate.h b/...al_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/attitude/attitude_estimate.h
@@ -192,7 +192,14 @@ class AttitudeEstimate
 #endif
         for(int i = 0; i < 3 ; i ++)
           {
+#ifdef SIMULATION
             imu_msg_.mag_data[i] = estimator_->getEstM(Frame::BODY)[i];
+#else
+            if(imu_list_[0]->getMagLsmCalib())
+              imu_msg_.mag_data[i] = estimator_->getMag(Frame::BODY)[i];
+            else
+              imu_msg_.mag_data[i] = estimator_->getEstM(Frame::BODY)[i];
+#endif
             imu_msg_.acc_data[i] = estimator_->getAcc(Frame::BODY)[i];
             imu_msg_.gyro_data[i] = estimator_->getAngular(Frame::BODY)[i];
 #if ESTIMATE_TYPE == COMPLEMENTARY

diff --git a/aerial_robot_nerve/spinal/src/spinal/imu_calibration.py b/aerial_robot_nerve/spinal/src/spinal/imu_calibration.py
@@ -410,14 +410,13 @@ def mag_calib(self, flag):
 
     def mag_lsm_calib(self, flag):
         try:
-            # Rest Mag calib data first
+            # Reset Mag calib data first
             req = ImuCalibRequest()
             req.command = req.SEND_CALIB_DATA
             req.data = []
             req.data.append(0) # hard-coding: imu0
             req.data.append(req.CALIB_MAG)
             req.data.extend([0, 0, 0, 1, 1, 1]) # mag bias and scale
-            print(req.data)
             res = self.imu_calib_data_client(req)
         except rospy.ServiceException as e:
             rospy.logerr("/imu_calib service call failed: %s"%e)
@@ -426,6 +425,16 @@ def mag_lsm_calib(self, flag):
 
         rospy.sleep(1.0)
 
+        try:
+            # Set flag to get raw mag data during calibration
+            req = ImuCalibRequest()
+            req.command = req.MAG_LSM_CALIB
+            req.data = []
+            req.data.append(flag)
+            res = self.imu_calib_data_client(req)
+        except rospy.ServiceException as e:
+            rospy.logerr("/imu_calib service call failed: %s"%e)
+
         if flag:  # start calibration
             self.mag_view_clear_flag = True # clear the sample with certain delay
             self.update_imu_calib_data()
@@ -442,6 +451,7 @@ def mag_lsm_calib(self, flag):
                 req.data.append(req.CALIB_MAG)
                 req.data.extend(bias)
                 req.data.extend(scale) # mag bias and scale
+                print(req.data)
                 res = self.imu_calib_data_client(req)
             except rospy.ServiceException as e:
                 rospy.logerr("/imu_calib service call failed: %s"%e)

diff --git a/aerial_robot_nerve/spinal/srv/ImuCalib.srv b/aerial_robot_nerve/spinal/srv/ImuCalib.srv
@@ -6,6 +6,7 @@ uint8 CALIB_ACC = 3  #data: [start(1)/stop(0), int duration (t > 0: stop with t[
 uint8 CALIB_MAG = 4  #data: [start(1)/stop(0), int duration (t > 0: stop with t[sec])]
 uint8 SEND_CALIB_DATA = 5  #data: [imu_id, sensor_id (GYRO/ACC/MAG), calib data]
 uint8 SAVE_CALIB_DATA = 6  #data: []
+uint8 MAG_LSM_CALIB = 7  #data: [start(1)/stop(0)]
 
 uint8 command
 float32[] data