Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fixed memory issue and ETC. #268

Merged
merged 1 commit into from
Dec 6, 2018
Merged

fixed memory issue and ETC. #268

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
141 changes: 90 additions & 51 deletions c++/src/dynamixel_sdk/protocol2_packet_handler.cpp
100644 → 100755
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,11 +17,14 @@
/* Author: zerom, Ryu Woon Jung (Leon) */

#if defined(__linux__)
#include <unistd.h>
#include "protocol2_packet_handler.h"
#elif defined(__APPLE__)
#include <unistd.h>
#include "protocol2_packet_handler.h"
#elif defined(_WIN32) || defined(_WIN64)
#define WINDLLEXPORT
#include <Windows.h>
#include "protocol2_packet_handler.h"
#elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__)
#include "../../include/dynamixel_sdk/protocol2_packet_handler.h"
Expand All @@ -31,8 +34,8 @@
#include <string.h>
#include <stdlib.h>

#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
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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);
Expand All @@ -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;

Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand Down
2 changes: 1 addition & 1 deletion c/example/dxl_monitor/dxl_monitor.c
100644 → 100755
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down
Loading