From da6eab74dd10a4025d2030bec4aaa034a56e8243 Mon Sep 17 00:00:00 2001
From: Leon Ryuwoon Jung <rwjung@robotis.com>
Date: Fri, 23 Jun 2017 16:24:11 +0900
Subject: [PATCH] hotfix - now protocol1.0 read/write 4Byte available
---
.../protocol1_packet_handler.cpp | 20 ++++++++++++++-----
ros/src/protocol1_packet_handler.cpp | 20 ++++++++++++++-----
2 files changed, 30 insertions(+), 10 deletions(-)
diff --git a/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp b/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp
index 68d22db0..5469e3a1 100644
--- a/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp
+++ b/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp
@@ -517,15 +517,23 @@ int Protocol1PacketHandler::read2ByteTxRx(PortHandler *port, uint8_t id, uint16_
int Protocol1PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, uint16_t address)
{
- return COMM_NOT_AVAILABLE;
+ return readTx(port, id, address, 4);
}
int Protocol1PacketHandler::read4ByteRx(PortHandler *port, uint32_t *data, uint8_t *error)
{
- return COMM_NOT_AVAILABLE;
+ uint8_t data_read[4] = {0};
+ int result = readRx(port, 4, data_read, error);
+ if (result == COMM_SUCCESS)
+ *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3]));
+ return result;
}
int Protocol1PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error)
{
- return COMM_NOT_AVAILABLE;
+ uint8_t data_read[4] = {0};
+ int result = readTxRx(port, id, address, 4, data_read, error);
+ if (result == COMM_SUCCESS)
+ *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3]));
+ return result;
}
int Protocol1PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
@@ -600,11 +608,13 @@ int Protocol1PacketHandler::write2ByteTxRx(PortHandler *port, uint8_t id, uint16
int Protocol1PacketHandler::write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data)
{
- return COMM_NOT_AVAILABLE;
+ uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) };
+ return writeTxOnly(port, id, address, 4, data_write);
}
int Protocol1PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error)
{
- return COMM_NOT_AVAILABLE;
+ uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) };
+ return writeTxRx(port, id, address, 4, data_write, error);
}
int Protocol1PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
diff --git a/ros/src/protocol1_packet_handler.cpp b/ros/src/protocol1_packet_handler.cpp
index e8a7d362..f8610772 100644
--- a/ros/src/protocol1_packet_handler.cpp
+++ b/ros/src/protocol1_packet_handler.cpp
@@ -523,15 +523,23 @@ int Protocol1PacketHandler::read2ByteTxRx(PortHandler *port, uint8_t id, uint16_
int Protocol1PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, uint16_t address)
{
- return COMM_NOT_AVAILABLE;
+ return readTx(port, id, address, 4);
}
int Protocol1PacketHandler::read4ByteRx(PortHandler *port, uint8_t id, uint32_t *data, uint8_t *error)
{
- return COMM_NOT_AVAILABLE;
+ uint8_t data_read[4] = {0};
+ int result = readRx(port, 4, data_read, error);
+ if (result == COMM_SUCCESS)
+ *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3]));
+ return result;
}
int Protocol1PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error)
{
- return COMM_NOT_AVAILABLE;
+ uint8_t data_read[4] = {0};
+ int result = readTxRx(port, id, address, 4, data_read, error);
+ if (result == COMM_SUCCESS)
+ *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3]));
+ return result;
}
int Protocol1PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
@@ -606,11 +614,13 @@ int Protocol1PacketHandler::write2ByteTxRx(PortHandler *port, uint8_t id, uint16
int Protocol1PacketHandler::write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data)
{
- return COMM_NOT_AVAILABLE;
+ uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) };
+ return writeTxOnly(port, id, address, 4, data_write);
}
int Protocol1PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error)
{
- return COMM_NOT_AVAILABLE;
+ uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) };
+ return writeTxRx(port, id, address, 4, data_write, error);
}
int Protocol1PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
--
GitLab