From 78fd21c202608050a98a66ae084763ff3d1cbcad Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 17 Feb 2023 10:59:23 +0100
Subject: [PATCH] Called the function to set the serial port in the low latency
 mode.

---
 src/dynamixelserver_serial.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/src/dynamixelserver_serial.cpp b/src/dynamixelserver_serial.cpp
index 03d0676..d24b407 100644
--- a/src/dynamixelserver_serial.cpp
+++ b/src/dynamixelserver_serial.cpp
@@ -9,6 +9,7 @@
 #ifdef _HAVE_XSD
 #include "xml/dyn_server_serial_cfg_file.hxx"
 #endif
+#include <iostream>
 
 CDynamixelServerSerial *CDynamixelServerSerial::pinstance=NULL;
 
@@ -41,6 +42,7 @@ void CDynamixelServerSerial::config_bus(const std::string &device, int baudrate)
 {
   TRS232_config serial_config;
 
+  this->dynamixel_access.enter();
   if(this->comm_dev!=NULL)
   {
     delete this->comm_dev;
@@ -54,6 +56,10 @@ void CDynamixelServerSerial::config_bus(const std::string &device, int baudrate)
     this->comm_dev=new CRS232(device);
     this->comm_dev->open((void *)&device);
     this->comm_dev->config(&serial_config);
+    try{
+      ((CRS232 *)this->comm_dev)->set_low_latency();
+    }catch(CException &e){
+    }
     this->bus_info.baud_rate=baudrate;
     this->bus_info.device=device;
   }catch(CException &e){
@@ -62,8 +68,10 @@ void CDynamixelServerSerial::config_bus(const std::string &device, int baudrate)
       delete this->comm_dev;
       this->comm_dev=NULL;
     }
+    this->dynamixel_access.exit();
     throw e;
   }
+  this->dynamixel_access.exit();
 }
 
 #ifdef _HAVE_XSD
-- 
GitLab