From 85bdd6588b1be964a5fbaabf59c8ff90c2fbdcd0 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 10 Jul 2024 18:03:56 +0200
Subject: [PATCH] Solved a bug in the slave input state machine to process the
incomming data.
---
src/dynamixel_slave.cpp | 13 ++++++++-----
src/dynamixelserver.cpp | 6 +++---
2 files changed, 11 insertions(+), 8 deletions(-)
diff --git a/src/dynamixel_slave.cpp b/src/dynamixel_slave.cpp
index 59fa467..e9bc724 100644
--- a/src/dynamixel_slave.cpp
+++ b/src/dynamixel_slave.cpp
@@ -88,9 +88,9 @@ void *CDynamixelSlave::process_packets_thread(void *params)
else
slave->num_bytes--;
break;
- case 2: if(data[i]==0xFD)// version 2 header
+ case 2: if(slave->dyn_ver==dyn_version2)// the module is configured for version 2
{
- if(slave->dyn_ver==dyn_version2)// the module is configured for version 2
+ if(data[i]==0xFD)// version 2 header
{
slave->packet[slave->num_bytes]=data[i];
slave->num_bytes++;
@@ -98,10 +98,13 @@ void *CDynamixelSlave::process_packets_thread(void *params)
else
slave->num_bytes=0;// ignore packet and restart synchronization
}
- else if(data[i]!=0xFF)
+ else
{
- slave->packet[slave->num_bytes]=data[i];
- slave->num_bytes++;
+ if(data[i]!=0xFF)
+ {
+ slave->packet[slave->num_bytes]=data[i];
+ slave->num_bytes++;
+ }
}
break;
case 3: slave->packet[slave->num_bytes]=data[i];
diff --git a/src/dynamixelserver.cpp b/src/dynamixelserver.cpp
index f8ea023..91a0de4 100644
--- a/src/dynamixelserver.cpp
+++ b/src/dynamixelserver.cpp
@@ -467,9 +467,9 @@ void CDynamixelServer::receive_sync_bulk_status_packets_v2(std::vector<unsigned
}
else
need_data=true;
- }
- if(length!=-1 && num>=length)
- {
+ }
+ if(length!=-1 && num>=length)
+ {
// check the checksum
crc=this->compute_checksum_v2(&data_int[read],length-2);
if((crc%256)!=data_int[read+length-2] || (crc/256)!=data_int[read+length-1])
--
GitLab