From 223ae677326eb84b6fe36fa0e4c5c015c7103f5d Mon Sep 17 00:00:00 2001 From: zerom Date: Wed, 5 Dec 2018 20:35:43 +0900 Subject: [PATCH] - reduced TXPACKET_MAX_LEN & RXPACKET_MAX_LEN (4K -> 1K) because the size of most Dynamixel receive buffers is 1K. - checked whether memory allocation was successful. - marked the crc table as static const - changed addStuffing() function (reduced stack memory usage) - removed busy waiting for rxPacket() - fixed the broadcastping bug in dxl_monitor --- .../protocol2_packet_handler.cpp | 141 ++++++---- c/example/dxl_monitor/dxl_monitor.c | 2 +- .../dynamixel_sdk/protocol2_packet_handler.c | 250 +++++++++++++++--- .../protocol2_packet_handler.cpp | 141 ++++++---- .../dynamixel_sdk/protocol2_packet_handler.py | 4 +- 5 files changed, 394 insertions(+), 144 deletions(-) mode change 100644 => 100755 c++/src/dynamixel_sdk/protocol2_packet_handler.cpp mode change 100644 => 100755 c/example/dxl_monitor/dxl_monitor.c mode change 100644 => 100755 c/src/dynamixel_sdk/protocol2_packet_handler.c mode change 100644 => 100755 ros/src/dynamixel_sdk/protocol2_packet_handler.cpp mode change 100644 => 100755 ros/src/dynamixel_sdk/protocol2_packet_handler.py diff --git a/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp b/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp old mode 100644 new mode 100755 index 3c836031..f9a96baa --- a/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ b/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp @@ -17,11 +17,14 @@ /* Author: zerom, Ryu Woon Jung (Leon) */ #if defined(__linux__) +#include #include "protocol2_packet_handler.h" #elif defined(__APPLE__) +#include #include "protocol2_packet_handler.h" #elif defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT +#include #include "protocol2_packet_handler.h" #elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) #include "../../include/dynamixel_sdk/protocol2_packet_handler.h" @@ -31,8 +34,8 @@ #include #include -#define TXPACKET_MAX_LEN (4*1024) -#define RXPACKET_MAX_LEN (4*1024) +#define TXPACKET_MAX_LEN (1*1024) +#define RXPACKET_MAX_LEN (1*1024) ///////////////// for Protocol 2.0 Packet ///////////////// #define PKT_HEADER0 0 @@ -140,7 +143,7 @@ const char *Protocol2PacketHandler::getRxPacketError(uint8_t error) unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size) { uint16_t i; - uint16_t crc_table[256] = {0x0000, + static const uint16_t crc_table[256] = {0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, @@ -190,39 +193,48 @@ unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t *da void Protocol2PacketHandler::addStuffing(uint8_t *packet) { - int i = 0, index = 0; int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); int packet_length_out = packet_length_in; - uint8_t temp[TXPACKET_MAX_LEN] = {0}; + + if (packet_length_in < 8) // INSTRUCTION, ADDR_L, ADDR_H, CRC16_L, CRC16_H + FF FF FD + return; - for (uint16_t s = PKT_HEADER0; s <= PKT_LENGTH_H; s++) - temp[s] = packet[s]; // FF FF FD XX ID LEN_L LEN_H - //memcpy(temp, packet, PKT_LENGTH_H+1); - index = PKT_INSTRUCTION; - for (i = 0; i < packet_length_in - 2; i++) // except CRC + uint8_t *packet_ptr; + uint16_t packet_length_before_crc = packet_length_in - 2; + for (uint16_t i = 3; i < packet_length_before_crc; i++) { - temp[index++] = packet[i+PKT_INSTRUCTION]; - if (packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) - { // FF FF FD - temp[index++] = 0xFD; + packet_ptr = &packet[i+PKT_INSTRUCTION-2]; + if (packet_ptr[0] == 0xFF && packet_ptr[1] == 0xFF && packet_ptr[2] == 0xFD) packet_length_out++; + } + + if (packet_length_in == packet_length_out) // no stuffing required + return; + + uint16_t out_index = packet_length_out + 6 - 2; // last index before crc + uint16_t in_index = packet_length_in + 6 - 2; // last index before crc + while (out_index != in_index) + { + if (packet[in_index] == 0xFD && packet[in_index-1] == 0xFF && packet[in_index-2] == 0xFF) + { + packet[out_index--] = 0xFD; // byte stuffing + if (out_index != in_index) + { + packet[out_index--] = packet[in_index--]; // FD + packet[out_index--] = packet[in_index--]; // FF + packet[out_index--] = packet[in_index--]; // FF + } + } + else + { + packet[out_index--] = packet[in_index--]; } } - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; - - - ////////////////////////// - if (packet_length_in != packet_length_out) - packet = (uint8_t *)realloc(packet, index * sizeof(uint8_t)); - /////////////////////////// - - for (uint16_t s = 0; s < index; s++) - packet[s] = temp[s]; - //memcpy(packet, temp, index); packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); + + return; } void Protocol2PacketHandler::removeStuffing(uint8_t *packet) @@ -393,6 +405,11 @@ int Protocol2PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket) break; } } +#if defined(__linux__) || defined(__APPLE__) + usleep(0); +#elif defined(_WIN32) || defined(_WIN64) + Sleep(0); +#endif } port->is_using_ = false; @@ -657,10 +674,12 @@ int Protocol2PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t addre int Protocol2PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error) { int result = COMM_TX_FAIL; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); + uint8_t *rxpacket = (uint8_t *)malloc(length + 11 + (length / 3)); //(length + 11 + (length/3)); // (length/3): consider stuffing - //uint8_t *rxpacket = new uint8_t[length + 11 + (length/3)]; // (length/3): consider stuffing - + + if (rxpacket == NULL) + return result; + do { result = rxPacket(port, rxpacket); } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id); @@ -687,9 +706,12 @@ int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add int result = COMM_TX_FAIL; uint8_t txpacket[14] = {0}; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); + uint8_t *rxpacket = (uint8_t *)malloc(length + 11 + (length / 3)); //(length + 11 + (length/3)); // (length/3): consider stuffing + if (rxpacket == NULL) + return result; + if (id >= BROADCAST_ID) return COMM_NOT_AVAILABLE; @@ -788,8 +810,10 @@ int Protocol2PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length+12); - //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); + + if (txpacket == NULL) + return result; txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); @@ -814,10 +838,12 @@ int Protocol2PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t ad { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12); - //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); uint8_t rxpacket[11] = {0}; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); @@ -871,11 +897,13 @@ int Protocol2PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16 int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12); - //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); @@ -897,12 +925,14 @@ int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16 int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12); - //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); uint8_t rxpacket[11] = {0}; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); @@ -923,11 +953,14 @@ int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14); + uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -951,12 +984,14 @@ int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14); - //uint8_t *txpacket = new uint8_t[param_length + 14]; + uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -979,12 +1014,14 @@ int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_ad int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10); - //uint8_t *txpacket = new uint8_t[param_length + 10]; + uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H @@ -1010,12 +1047,14 @@ int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16 int Protocol2PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10); - //uint8_t *txpacket = new uint8_t[param_length + 10]; + uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H diff --git a/c/example/dxl_monitor/dxl_monitor.c b/c/example/dxl_monitor/dxl_monitor.c old mode 100644 new mode 100755 index d04db217..be01c3f6 --- a/c/example/dxl_monitor/dxl_monitor.c +++ b/c/example/dxl_monitor/dxl_monitor.c @@ -488,7 +488,7 @@ int main(int argc, char *argv[]) printf("Detected Dynamixel : \n"); for (id = 0; id < MAX_ID; id++) { - if (getBroadcastPingResult(port_num, id, PROTOCOL_VERSION2)) + if (getBroadcastPingResult(port_num, PROTOCOL_VERSION2, id)) printf("[ID:%03d]\n", id); } printf("\n"); diff --git a/c/src/dynamixel_sdk/protocol2_packet_handler.c b/c/src/dynamixel_sdk/protocol2_packet_handler.c old mode 100644 new mode 100755 index 759701eb..4a310ccd --- a/c/src/dynamixel_sdk/protocol2_packet_handler.c +++ b/c/src/dynamixel_sdk/protocol2_packet_handler.c @@ -17,11 +17,14 @@ /* Author: Ryu Woon Jung (Leon) */ #if defined(__linux__) +#include #include "protocol2_packet_handler.h" #elif defined(__APPLE__) +#include #include "protocol2_packet_handler.h" #elif defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT +#include #include "protocol2_packet_handler.h" #endif @@ -29,8 +32,8 @@ #include #include -#define TXPACKET_MAX_LEN (4*1024) -#define RXPACKET_MAX_LEN (4*1024) +#define TXPACKET_MAX_LEN (1*1024) +#define RXPACKET_MAX_LEN (1*1024) ///////////////// for Protocol 2.0 Packet ///////////////// #define PKT_HEADER0 0 @@ -142,6 +145,11 @@ uint8_t getLastRxPacketError2(int port_num) void setDataWrite2(int port_num, uint16_t data_length, uint16_t data_pos, uint32_t data) { packetData[port_num].data_write = (uint8_t *)realloc(packetData[port_num].data_write, (data_pos + data_length) * sizeof(uint8_t)); + if (packetData[port_num].data_write == NULL) + { + printf("[Set Data Write] memory allocation failed... \n"); + return; + } switch (data_length) { @@ -162,7 +170,7 @@ void setDataWrite2(int port_num, uint16_t data_length, uint16_t data_pos, uint32 break; default: - printf("[Set Data Write] failed... "); + printf("[Set Data Write] failed... \n"); break; } } @@ -181,7 +189,7 @@ uint32_t getDataRead2(int port_num, uint16_t data_length, uint16_t data_pos) , DXL_MAKEWORD(packetData[port_num].data_read[data_pos + 2], packetData[port_num].data_read[data_pos + 3])); default: - printf("[Set Data Read] failed... "); + printf("[Set Data Read] failed... \n"); return 0; } } @@ -189,7 +197,7 @@ uint32_t getDataRead2(int port_num, uint16_t data_length, uint16_t data_pos) unsigned short updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size) { uint16_t i, j; - uint16_t crc_table[256] = { 0x0000, + static const uint16_t crc_table[256] = { 0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, @@ -239,42 +247,52 @@ unsigned short updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t dat void addStuffing(uint8_t *packet) { - uint16_t s; - - uint16_t i = 0; - int index = 0; + uint8_t *packet_ptr; + uint16_t i; + uint16_t packet_length_before_crc; + uint16_t out_index, in_index; + int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); int packet_length_out = packet_length_in; - uint8_t temp[TXPACKET_MAX_LEN] = { 0 }; - - for (s = PKT_HEADER0; s <= PKT_LENGTH_H; s++) - { - temp[s] = packet[s]; // FF FF FD XX ID LEN_L LEN_H - } - - index = PKT_INSTRUCTION; - for (i = 0; i < packet_length_in - 2; i++) // except CRC + + if (packet_length_in < 8) // INSTRUCTION, ADDR_L, ADDR_H, CRC16_L, CRC16_H + FF FF FD + return; + + packet_length_before_crc = packet_length_in - 2; + for (i = 3; i < packet_length_before_crc; i++) { - temp[index++] = packet[i + PKT_INSTRUCTION]; - if (packet[i + PKT_INSTRUCTION] == 0xFD && packet[i + PKT_INSTRUCTION - 1] == 0xFF && packet[i + PKT_INSTRUCTION - 2] == 0xFF) - { // FF FF FD - temp[index++] = 0xFD; + packet_ptr = &packet[i+PKT_INSTRUCTION-2]; + if (packet_ptr[0] == 0xFF && packet_ptr[1] == 0xFF && packet_ptr[2] == 0xFD) packet_length_out++; - } } - temp[index++] = packet[PKT_INSTRUCTION + packet_length_in - 2]; - temp[index++] = packet[PKT_INSTRUCTION + packet_length_in - 1]; - - if (packet_length_in != packet_length_out) - packet = (uint8_t *)realloc(packet, index * sizeof(uint8_t)); - - for (s = 0; s < index; s++) + + if (packet_length_in == packet_length_out) // no stuffing required + return; + + out_index = packet_length_out + 6 - 2; // last index before crc + in_index = packet_length_in + 6 - 2; // last index before crc + while (out_index != in_index) { - packet[s] = temp[s]; + if (packet[in_index] == 0xFD && packet[in_index-1] == 0xFF && packet[in_index-2] == 0xFF) + { + packet[out_index--] = 0xFD; // byte stuffing + if (out_index != in_index) + { + packet[out_index--] = packet[in_index--]; // FD + packet[out_index--] = packet[in_index--]; // FF + packet[out_index--] = packet[in_index--]; // FF + } + } + else + { + packet[out_index--] = packet[in_index--]; + } } packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); + + return; } void removeStuffing(uint8_t *packet) @@ -460,6 +478,11 @@ void rxPacket2(int port_num) break; } } +#if defined(__linux__) || defined(__APPLE__) + usleep(0); +#elif defined(_WIN32) || defined(_WIN64) + Sleep(0); +#endif } g_is_using[port_num] = False; @@ -520,7 +543,12 @@ uint16_t pingGetModelNum2(int port_num, uint8_t id) packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, 10); packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, 14); - + if (packetData[port_num].tx_packet == NULL || packetData[port_num].rx_packet == NULL) + { + printf("[PingGetModeNum] memory allocation failed.. \n"); + return COMM_TX_FAIL; + } + if (id >= BROADCAST_ID) { packetData[port_num].communication_result = COMM_NOT_AVAILABLE; @@ -550,7 +578,11 @@ void broadcastPing2(int port_num) uint16_t wait_length = STATUS_LENGTH * MAX_ID; packetData[port_num].broadcast_ping_id_list = (uint8_t *)calloc(255, sizeof(uint8_t)); - + if (packetData[port_num].broadcast_ping_id_list == NULL) + { + printf("[BroadcastPing] memory allocation failed.. \n"); + return; + } for (id = 0; id < 255; id++) { @@ -559,6 +591,11 @@ void broadcastPing2(int port_num) packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, 10 * sizeof(uint8_t)); packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, STATUS_LENGTH * MAX_ID * sizeof(uint8_t)); + if (packetData[port_num].tx_packet == NULL || packetData[port_num].rx_packet == NULL) + { + printf("[BroadcastPing] memory allocation failed.. \n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = BROADCAST_ID; packetData[port_num].tx_packet[PKT_LENGTH_L] = 3; @@ -668,6 +705,11 @@ uint8_t getBroadcastPingResult2(int port_num, int id) void action2(int port_num, uint8_t id) { packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, 10); + if (packetData[port_num].tx_packet == NULL) + { + printf("[Action] memory allocation failed..\n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH_L] = 3; @@ -681,6 +723,11 @@ void reboot2(int port_num, uint8_t id) { packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, 10); packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, 11); + if (packetData[port_num].tx_packet == NULL || packetData[port_num].rx_packet == NULL) + { + printf("[Reboot] memory allocation failed..\n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH_L] = 3; @@ -694,7 +741,12 @@ void clearMultiTurn2(int port_num, uint8_t id) { packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, 15); packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, 11); - + if (packetData[port_num].tx_packet == NULL || packetData[port_num].rx_packet == NULL) + { + printf("[ClearMultiTurn] memory allocation failed..\n"); + return; + } + packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH_L] = 8; packetData[port_num].tx_packet[PKT_LENGTH_H] = 0; @@ -712,7 +764,12 @@ void factoryReset2(int port_num, uint8_t id, uint8_t option) { packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, 11); packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, 11); - + if (packetData[port_num].tx_packet == NULL || packetData[port_num].rx_packet == NULL) + { + printf("[FactoryReset] memory allocation failed..\n"); + return; + } + packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH_L] = 4; packetData[port_num].tx_packet[PKT_LENGTH_H] = 0; @@ -727,7 +784,12 @@ void readTx2(int port_num, uint8_t id, uint16_t address, uint16_t length) packetData[port_num].communication_result = COMM_TX_FAIL; packetData[port_num].tx_packet = (uint8_t *)malloc(14); - + if (packetData[port_num].tx_packet == NULL) + { + printf("[ReadTx] memory allocation failed..\n"); + return; + } + if (id >= BROADCAST_ID) { packetData[port_num].communication_result = COMM_NOT_AVAILABLE; @@ -756,7 +818,12 @@ void readRx2(int port_num, uint16_t length) packetData[port_num].communication_result = COMM_TX_FAIL; packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, RXPACKET_MAX_LEN); //(length + 11 + (length/3)); // (length/3): consider stuffing - + if (packetData[port_num].rx_packet == NULL) + { + printf("[ReadRx] memory allocation failed..\n"); + return; + } + rxPacket2(port_num); if (packetData[port_num].communication_result == COMM_SUCCESS) { @@ -777,7 +844,12 @@ void readTxRx2(int port_num, uint8_t id, uint16_t address, uint16_t length) packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, 14); packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, RXPACKET_MAX_LEN); //(length + 11 + (length/3)); // (length/3): consider stuffing - + if (packetData[port_num].tx_packet == NULL || packetData[port_num].rx_packet == NULL) + { + printf("[ReadTxRx] memory allocation failed..\n"); + return; + } + if (id >= BROADCAST_ID) { packetData[port_num].communication_result = COMM_NOT_AVAILABLE; @@ -812,6 +884,11 @@ void read1ByteTx2(int port_num, uint8_t id, uint16_t address) uint8_t read1ByteRx2(int port_num) { packetData[port_num].data_read = (uint8_t *)realloc(packetData[port_num].data_read, 1 * sizeof(uint8_t)); + if (packetData[port_num].data_read == NULL) + { + printf("[Read1ByteRx] memory allocation failed..\n"); + return 0; + } packetData[port_num].data_read[0] = 0; readRx2(port_num, 1); if (packetData[port_num].communication_result == COMM_SUCCESS) @@ -821,6 +898,11 @@ uint8_t read1ByteRx2(int port_num) uint8_t read1ByteTxRx2(int port_num, uint8_t id, uint16_t address) { packetData[port_num].data_read = (uint8_t *)realloc(packetData[port_num].data_read, 1 * sizeof(uint8_t)); + if (packetData[port_num].data_read == NULL) + { + printf("[Read1ByteTxRx] memory allocation failed..\n"); + return 0; + } packetData[port_num].data_read[0] = 0; readTxRx2(port_num, id, address, 1); if (packetData[port_num].communication_result == COMM_SUCCESS) @@ -835,6 +917,11 @@ void read2ByteTx2(int port_num, uint8_t id, uint16_t address) uint16_t read2ByteRx2(int port_num) { packetData[port_num].data_read = (uint8_t *)realloc(packetData[port_num].data_read, 2 * sizeof(uint8_t)); + if (packetData[port_num].data_read == NULL) + { + printf("[Read2ByteRx] memory allocation failed..\n"); + return 0; + } packetData[port_num].data_read[0] = 0; packetData[port_num].data_read[1] = 0; readRx2(port_num, 2); @@ -845,6 +932,11 @@ uint16_t read2ByteRx2(int port_num) uint16_t read2ByteTxRx2(int port_num, uint8_t id, uint16_t address) { packetData[port_num].data_read = (uint8_t *)realloc(packetData[port_num].data_read, 2 * sizeof(uint8_t)); + if (packetData[port_num].data_read == NULL) + { + printf("[Read2ByteTxRx] memory allocation failed..\n"); + return 0; + } packetData[port_num].data_read[0] = 0; packetData[port_num].data_read[1] = 0; readTxRx2(port_num, id, address, 2); @@ -860,6 +952,11 @@ void read4ByteTx2(int port_num, uint8_t id, uint16_t address) uint32_t read4ByteRx2(int port_num) { packetData[port_num].data_read = (uint8_t *)realloc(packetData[port_num].data_read, 4 * sizeof(uint8_t)); + if (packetData[port_num].data_read == NULL) + { + printf("[Read4ByteRx] memory allocation failed..\n"); + return 0; + } packetData[port_num].data_read[0] = 0; packetData[port_num].data_read[1] = 0; packetData[port_num].data_read[2] = 0; @@ -872,6 +969,11 @@ uint32_t read4ByteRx2(int port_num) uint32_t read4ByteTxRx2(int port_num, uint8_t id, uint16_t address) { packetData[port_num].data_read = (uint8_t *)realloc(packetData[port_num].data_read, 4 * sizeof(uint8_t)); + if (packetData[port_num].data_read == NULL) + { + printf("[Read2ByteTxRx] memory allocation failed..\n"); + return 0; + } packetData[port_num].data_read[0] = 0; packetData[port_num].data_read[1] = 0; packetData[port_num].data_read[2] = 0; @@ -890,6 +992,11 @@ void writeTxOnly2(int port_num, uint8_t id, uint16_t address, uint16_t length) packetData[port_num].communication_result = COMM_TX_FAIL; packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, length + 12); + if (packetData[port_num].tx_packet == NULL) + { + printf("[WriteTxOnly] memory allocation failed..\n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH_L] = DXL_LOBYTE(length + 5); @@ -915,7 +1022,12 @@ void writeTxRx2(int port_num, uint8_t id, uint16_t address, uint16_t length) packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, length + 12); packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, 11); - + if (packetData[port_num].tx_packet == NULL || packetData[port_num].rx_packet == NULL) + { + printf("[WriteTxRx] memory allocation failed..\n"); + return; + } + packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH_L] = DXL_LOBYTE(length + 5); packetData[port_num].tx_packet[PKT_LENGTH_H] = DXL_HIBYTE(length + 5); @@ -934,12 +1046,22 @@ void writeTxRx2(int port_num, uint8_t id, uint16_t address, uint16_t length) void write1ByteTxOnly2(int port_num, uint8_t id, uint16_t address, uint8_t data) { packetData[port_num].data_write = (uint8_t *)realloc(packetData[port_num].data_write, 1 * sizeof(uint8_t)); + if (packetData[port_num].data_write == NULL) + { + printf("[Write1ByteTxOnly] memory allocation failed..\n"); + return; + } packetData[port_num].data_write[0] = data; writeTxOnly2(port_num, id, address, 1); } void write1ByteTxRx2(int port_num, uint8_t id, uint16_t address, uint8_t data) { packetData[port_num].data_write = (uint8_t *)realloc(packetData[port_num].data_write, 1 * sizeof(uint8_t)); + if (packetData[port_num].data_write == NULL) + { + printf("[Write1ByteTxRx] memory allocation failed..\n"); + return; + } packetData[port_num].data_write[0] = data; writeTxRx2(port_num, id, address, 1); } @@ -947,6 +1069,11 @@ void write1ByteTxRx2(int port_num, uint8_t id, uint16_t address, uint8_t data) void write2ByteTxOnly2(int port_num, uint8_t id, uint16_t address, uint16_t data) { packetData[port_num].data_write = (uint8_t *)realloc(packetData[port_num].data_write, 2 * sizeof(uint8_t)); + if (packetData[port_num].data_write == NULL) + { + printf("[Write2ByteTxOnly] memory allocation failed..\n"); + return; + } packetData[port_num].data_write[0] = DXL_LOBYTE(data); packetData[port_num].data_write[1] = DXL_HIBYTE(data); writeTxOnly2(port_num, id, address, 2); @@ -954,6 +1081,11 @@ void write2ByteTxOnly2(int port_num, uint8_t id, uint16_t address, uint16_t data void write2ByteTxRx2(int port_num, uint8_t id, uint16_t address, uint16_t data) { packetData[port_num].data_write = (uint8_t *)realloc(packetData[port_num].data_write, 2 * sizeof(uint8_t)); + if (packetData[port_num].data_write == NULL) + { + printf("[Write2ByteTxRx] memory allocation failed..\n"); + return; + } packetData[port_num].data_write[0] = DXL_LOBYTE(data); packetData[port_num].data_write[1] = DXL_HIBYTE(data); writeTxRx2(port_num, id, address, 2); @@ -962,6 +1094,11 @@ void write2ByteTxRx2(int port_num, uint8_t id, uint16_t address, uint16_t data) void write4ByteTxOnly2(int port_num, uint8_t id, uint16_t address, uint32_t data) { packetData[port_num].data_write = (uint8_t *)realloc(packetData[port_num].data_write, 4 * sizeof(uint8_t)); + if (packetData[port_num].data_write == NULL) + { + printf("[Write4ByteTxOnly] memory allocation failed..\n"); + return; + } packetData[port_num].data_write[0] = DXL_LOBYTE(DXL_LOWORD(data)); packetData[port_num].data_write[1] = DXL_HIBYTE(DXL_LOWORD(data)); packetData[port_num].data_write[2] = DXL_LOBYTE(DXL_HIWORD(data)); @@ -971,6 +1108,11 @@ void write4ByteTxOnly2(int port_num, uint8_t id, uint16_t address, uint32_t data void write4ByteTxRx2(int port_num, uint8_t id, uint16_t address, uint32_t data) { packetData[port_num].data_write = (uint8_t *)realloc(packetData[port_num].data_write, 4 * sizeof(uint8_t)); + if (packetData[port_num].data_write == NULL) + { + printf("[Write4ByteTxRx] memory allocation failed..\n"); + return; + } packetData[port_num].data_write[0] = DXL_LOBYTE(DXL_LOWORD(data)); packetData[port_num].data_write[1] = DXL_HIBYTE(DXL_LOWORD(data)); packetData[port_num].data_write[2] = DXL_LOBYTE(DXL_HIWORD(data)); @@ -985,6 +1127,11 @@ void regWriteTxOnly2(int port_num, uint8_t id, uint16_t address, uint16_t length packetData[port_num].communication_result = COMM_TX_FAIL; packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, length + 12); + if (packetData[port_num].tx_packet == NULL) + { + printf("[RegWriteTxOnly] memory allocation failed..\n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH_L] = DXL_LOBYTE(length + 5); @@ -1010,6 +1157,11 @@ void regWriteTxRx2(int port_num, uint8_t id, uint16_t address, uint16_t length) packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, length + 12); packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, 11); + if (packetData[port_num].tx_packet == NULL || packetData[port_num].rx_packet == NULL) + { + printf("[RegWriteTxRx] memory allocation failed..\n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH_L] = DXL_LOBYTE(length + 5); @@ -1034,6 +1186,11 @@ void syncReadTx2(int port_num, uint16_t start_address, uint16_t data_length, uin packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, param_length + 14); // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + if (packetData[port_num].tx_packet == NULL) + { + printf("[SyncReadTx] memory allocation failed..\n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = BROADCAST_ID; packetData[port_num].tx_packet[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -1063,6 +1220,11 @@ void syncWriteTxOnly2(int port_num, uint16_t start_address, uint16_t data_length packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, param_length + 14); // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + if (packetData[port_num].tx_packet == NULL) + { + printf("[SyncWriteTxOnly] memory allocation failed..\n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = BROADCAST_ID; packetData[port_num].tx_packet[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -1090,6 +1252,11 @@ void bulkReadTx2(int port_num, uint16_t param_length) packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, param_length + 10); // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + if (packetData[port_num].tx_packet == NULL) + { + printf("[BulkReadTx] memory allocation failed..\n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = BROADCAST_ID; packetData[port_num].tx_packet[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H @@ -1121,6 +1288,11 @@ void bulkWriteTxOnly2(int port_num, uint16_t param_length) packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, param_length + 10); // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + if (packetData[port_num].tx_packet == NULL) + { + printf("[BulkWriteTxOnly] memory allocation failed..\n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = BROADCAST_ID; packetData[port_num].tx_packet[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H diff --git a/ros/src/dynamixel_sdk/protocol2_packet_handler.cpp b/ros/src/dynamixel_sdk/protocol2_packet_handler.cpp old mode 100644 new mode 100755 index c897e620..fb872f58 --- a/ros/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ b/ros/src/dynamixel_sdk/protocol2_packet_handler.cpp @@ -17,11 +17,14 @@ /* Author: zerom, Ryu Woon Jung (Leon) */ #if defined(__linux__) +#include #include "protocol2_packet_handler.h" #elif defined(__APPLE__) +#include #include "protocol2_packet_handler.h" #elif defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT +#include #include "protocol2_packet_handler.h" #elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) #include "../../include/dynamixel_sdk/protocol2_packet_handler.h" @@ -31,8 +34,8 @@ #include #include -#define TXPACKET_MAX_LEN (4*1024) -#define RXPACKET_MAX_LEN (4*1024) +#define TXPACKET_MAX_LEN (1*1024) +#define RXPACKET_MAX_LEN (1*1024) ///////////////// for Protocol 2.0 Packet ///////////////// #define PKT_HEADER0 0 @@ -140,7 +143,7 @@ const char *Protocol2PacketHandler::getRxPacketError(uint8_t error) unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size) { uint16_t i; - uint16_t crc_table[256] = {0x0000, + static const uint16_t crc_table[256] = {0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, @@ -190,39 +193,48 @@ unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t *da void Protocol2PacketHandler::addStuffing(uint8_t *packet) { - int i = 0, index = 0; int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); int packet_length_out = packet_length_in; - uint8_t temp[TXPACKET_MAX_LEN] = {0}; + + if (packet_length_in < 8) // INSTRUCTION, ADDR_L, ADDR_H, CRC16_L, CRC16_H + FF FF FD + return; - for (uint16_t s = PKT_HEADER0; s <= PKT_LENGTH_H; s++) - temp[s] = packet[s]; // FF FF FD XX ID LEN_L LEN_H - //memcpy(temp, packet, PKT_LENGTH_H+1); - index = PKT_INSTRUCTION; - for (i = 0; i < packet_length_in - 2; i++) // except CRC + uint8_t *packet_ptr; + uint16_t packet_length_before_crc = packet_length_in - 2; + for (uint16_t i = 3; i < packet_length_before_crc; i++) { - temp[index++] = packet[i+PKT_INSTRUCTION]; - if (packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) - { // FF FF FD - temp[index++] = 0xFD; + packet_ptr = &packet[i+PKT_INSTRUCTION-2]; + if (packet_ptr[0] == 0xFF && packet_ptr[1] == 0xFF && packet_ptr[2] == 0xFD) packet_length_out++; + } + + if (packet_length_in == packet_length_out) // no stuffing required + return; + + uint16_t out_index = packet_length_out + 6 - 2; // last index before crc + uint16_t in_index = packet_length_in + 6 - 2; // last index before crc + while (out_index != in_index) + { + if (packet[in_index] == 0xFD && packet[in_index-1] == 0xFF && packet[in_index-2] == 0xFF) + { + packet[out_index--] = 0xFD; // byte stuffing + if (out_index != in_index) + { + packet[out_index--] = packet[in_index--]; // FD + packet[out_index--] = packet[in_index--]; // FF + packet[out_index--] = packet[in_index--]; // FF + } + } + else + { + packet[out_index--] = packet[in_index--]; } } - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; - - - ////////////////////////// - if (packet_length_in != packet_length_out) - packet = (uint8_t *)realloc(packet, index * sizeof(uint8_t)); - /////////////////////////// - - for (uint16_t s = 0; s < index; s++) - packet[s] = temp[s]; - //memcpy(packet, temp, index); packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); + + return; } void Protocol2PacketHandler::removeStuffing(uint8_t *packet) @@ -393,6 +405,11 @@ int Protocol2PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket) break; } } +#if defined(__linux__) || defined(__APPLE__) + usleep(0); +#elif defined(_WIN32) || defined(_WIN64) + Sleep(0); +#endif } port->is_using_ = false; @@ -639,10 +656,12 @@ int Protocol2PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t addre int Protocol2PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error) { int result = COMM_TX_FAIL; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); + uint8_t *rxpacket = (uint8_t *)malloc(length + 11 + (length / 3)); //(length + 11 + (length/3)); // (length/3): consider stuffing - //uint8_t *rxpacket = new uint8_t[length + 11 + (length/3)]; // (length/3): consider stuffing - + + if (rxpacket == NULL) + return result; + do { result = rxPacket(port, rxpacket); } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id); @@ -669,9 +688,12 @@ int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add int result = COMM_TX_FAIL; uint8_t txpacket[14] = {0}; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); + uint8_t *rxpacket = (uint8_t *)malloc(length + 11 + (length / 3)); //(length + 11 + (length/3)); // (length/3): consider stuffing + if (rxpacket == NULL) + return result; + if (id >= BROADCAST_ID) return COMM_NOT_AVAILABLE; @@ -770,8 +792,10 @@ int Protocol2PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length+12); - //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); + + if (txpacket == NULL) + return result; txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); @@ -796,10 +820,12 @@ int Protocol2PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t ad { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12); - //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); uint8_t rxpacket[11] = {0}; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); @@ -853,11 +879,13 @@ int Protocol2PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16 int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12); - //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); @@ -879,12 +907,14 @@ int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16 int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12); - //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); uint8_t rxpacket[11] = {0}; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); @@ -905,11 +935,14 @@ int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14); + uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -933,12 +966,14 @@ int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14); - //uint8_t *txpacket = new uint8_t[param_length + 14]; + uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -961,12 +996,14 @@ int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_ad int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10); - //uint8_t *txpacket = new uint8_t[param_length + 10]; + uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H @@ -992,12 +1029,14 @@ int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16 int Protocol2PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10); - //uint8_t *txpacket = new uint8_t[param_length + 10]; + uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H diff --git a/ros/src/dynamixel_sdk/protocol2_packet_handler.py b/ros/src/dynamixel_sdk/protocol2_packet_handler.py old mode 100644 new mode 100755 index a3df5ea8..ef559d29 --- a/ros/src/dynamixel_sdk/protocol2_packet_handler.py +++ b/ros/src/dynamixel_sdk/protocol2_packet_handler.py @@ -21,8 +21,8 @@ from .robotis_def import * -TXPACKET_MAX_LEN = 4 * 1024 -RXPACKET_MAX_LEN = 4 * 1024 +TXPACKET_MAX_LEN = 1 * 1024 +RXPACKET_MAX_LEN = 1 * 1024 # for Protocol 2.0 Packet PKT_HEADER0 = 0