Merge pull request #36 from ichiro-its/enhancement/refactor-kansei

[Enhancement] - Refactor Kansei
ichiro-its · Jun 4, 2024 · 27f7508 · 27f7508
2 parents 6e3e6b9 + c52750e
commit 27f7508
Show file tree

Hide file tree

Showing 5 changed files with 27 additions and 24 deletions.
diff --git a/data/kansei.json b/data/kansei.json
diff --git a/include/kansei/measurement/sensor/mpu.hpp b/include/kansei/measurement/sensor/mpu.hpp
@@ -39,16 +39,19 @@ class MPU : public MeasurementUnit
   bool connect();
 
   void update_rpy() override;
+  static void *start(void *object);
 
   void reset_orientation() override;
   void set_orientation_to(const keisan::Angle<double> & target_orientation) override;
 
 private:
   int socket_fd;
   std::string port_name;
+  pthread_t thread;
 
-  keisan::Angle<double> oreintation_compensation;
-  keisan::Angle<double> oreintation_error;
+  keisan::Angle<double> raw_orientation;
+  keisan::Angle<double> orientation_compensation;
+  keisan::Angle<double> orientation_error;
 };
 
 }  // namespace kansei::measurement

diff --git a/src/check_rpy_main.cpp b/src/check_rpy_main.cpp
@@ -27,6 +27,8 @@
 #include "kansei/measurement/measurement.hpp"
 #include "keisan/keisan.hpp"
 
+using namespace std::chrono_literals;
+
 int main(int argc, char * argv[])
 {
   std::string port_name = "/dev/ttyUSB1";
@@ -45,15 +47,17 @@ int main(int argc, char * argv[])
     return 0;
   }
 
+  rclcpp::Rate loop_rate(8ms);
   while (true) {
-    mpu.update_rpy();
 
     keisan::Euler<double> rpy = mpu.get_orientation();
 
     std::cout << "Roll: " << rpy.roll.degree() << std::endl;
     std::cout << "Pitch: " << rpy.pitch.degree() << std::endl;
     std::cout << "Yaw: " << rpy.yaw.degree() << std::endl;
     std::cout << "\033c";
+
+    loop_rate.sleep();
   }
 
   return 0;

diff --git a/src/kansei/measurement/node/measurement_node.cpp b/src/kansei/measurement/node/measurement_node.cpp
@@ -75,7 +75,6 @@ MeasurementNode::MeasurementNode(
 void MeasurementNode::update(double seconds)
 {
   if (std::dynamic_pointer_cast<MPU>(measurement_unit)) {
-    measurement_unit->update_rpy();
 
     publish_status();
   } else if (std::dynamic_pointer_cast<Filter>(measurement_unit)) {

diff --git a/src/kansei/measurement/sensor/mpu.cpp b/src/kansei/measurement/sensor/mpu.cpp
@@ -41,7 +41,7 @@ namespace kansei::measurement
 {
 
 MPU::MPU(const std::string & port_name)
-: socket_fd(-1), oreintation_error(0_deg), oreintation_compensation(0_deg)
+: socket_fd(-1), orientation_error(0_deg), orientation_compensation(0_deg), raw_orientation(0_deg)
 {
   set_port_name(port_name);
 }
@@ -78,6 +78,8 @@ bool MPU::connect()
     tcflush(socket_fd, TCIFLUSH);
     tcsetattr(socket_fd, TCSANOW, &newtio);
 
+    pthread_create(&thread, NULL, &start, this);
+
     return true;
   } else {
     fprintf(stderr, "Can not connect MPU on %s\n", port_name.c_str());
@@ -168,29 +170,38 @@ void MPU::update_rpy()
 
       rpy.roll = keisan::make_degree(roll);
       rpy.pitch = keisan::make_degree(pitch);
-      rpy.yaw = keisan::make_degree(yaw) + oreintation_error + oreintation_compensation;
 
-      break;
+      raw_orientation = keisan::make_degree(yaw);
+      rpy.yaw = raw_orientation + orientation_error + orientation_compensation;
+
     } else {
       usart_status = 0;
     }
   }
 }
 
+void *MPU::start(void *object)
+{
+  reinterpret_cast<MPU *>(object)->update_rpy();
+
+  return 0;
+}
+
 void MPU::set_port_name(const std::string & port_name)
 {
   this->port_name = port_name;
 }
 
 void MPU::reset_orientation()
 {
-  oreintation_error = -rpy.yaw;
-  oreintation_compensation = 0_deg;
+  orientation_error = -raw_orientation;
+  orientation_compensation = 0_deg;
 }
 
 void MPU::set_orientation_to(const keisan::Angle<double> & target_orientation)
 {
-  oreintation_compensation = target_orientation;
+  orientation_error = -raw_orientation;
+  orientation_compensation = target_orientation;
 }
 
 }  // namespace kansei::measurement