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.

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 GCC errors when building C library #616

Closed
wants to merge 1 commit into from
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
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
2 changes: 0 additions & 2 deletions c/include/dynamixel_sdk/packet_handler.h
Original file line number Diff line number Diff line change
Expand Up @@ -79,8 +79,6 @@ typedef struct
uint8_t *broadcast_ping_id_list;
}PacketData;

PacketData *packetData;

WINDECLSPEC void packetHandler ();

WINDECLSPEC const char *getTxRxResult (int protocol_version, int result);
Expand Down
5 changes: 0 additions & 5 deletions c/include/dynamixel_sdk/port_handler.h
Original file line number Diff line number Diff line change
Expand Up @@ -42,11 +42,6 @@

#include "robotis_def.h"

static const int DEFAULT_BAUDRATE = 57600;

int g_used_port_num;
uint8_t *g_is_using;

WINDECLSPEC int portHandler (const char *port_name);

WINDECLSPEC uint8_t openPort (int port_num);
Expand Down
2 changes: 2 additions & 0 deletions c/src/dynamixel_sdk/group_bulk_read.c
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@
#include "group_bulk_read.h"
#endif

extern PacketData *packetData; // defined in packet_handler.c

typedef struct
{
uint8_t id;
Expand Down
2 changes: 2 additions & 0 deletions c/src/dynamixel_sdk/group_bulk_write.c
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@
#include "group_bulk_write.h"
#endif

extern PacketData *packetData; // defined in packet_handler.c

typedef struct
{
uint8_t id;
Expand Down
2 changes: 2 additions & 0 deletions c/src/dynamixel_sdk/group_sync_read.c
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@
#include "group_sync_read.h"
#endif

extern PacketData *packetData; // defined in packet_handler.c

typedef struct
{
uint8_t id;
Expand Down
2 changes: 2 additions & 0 deletions c/src/dynamixel_sdk/group_sync_write.c
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@
#include "group_sync_write.h"
#endif

extern PacketData *packetData; // defined in packet_handler.c

typedef struct
{
uint8_t id;
Expand Down
3 changes: 3 additions & 0 deletions c/src/dynamixel_sdk/packet_handler.c
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,9 @@
#include "protocol2_packet_handler.h"
#endif

PacketData *packetData;
extern int g_used_port_num; // defined in port_handler.c

void packetHandler()
{
int port_num;
Expand Down
5 changes: 5 additions & 0 deletions c/src/dynamixel_sdk/port_handler.c
Original file line number Diff line number Diff line change
Expand Up @@ -91,3 +91,8 @@ void setPacketTimeoutMSec(int port_num, double msec) { setPacketTimeoutMSecWi
uint8_t isPacketTimeout (int port_num) { return isPacketTimeoutWindows(port_num); }

#endif

static const int DEFAULT_BAUDRATE = 57600;

int g_used_port_num;
uint8_t *g_is_using;
3 changes: 3 additions & 0 deletions c/src/dynamixel_sdk/port_handler_linux.c
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,9 @@ typedef struct
}PortData;

static PortData *portData;
extern int DEFAULT_BAUDRATE; // defined in port_handler.c
extern int g_used_port_num; // defined in port_handler.c
extern uint8_t *g_is_using; // defined in port_handler.c

int portHandlerLinux(const char *port_name)
{
Expand Down
3 changes: 3 additions & 0 deletions c/src/dynamixel_sdk/port_handler_mac.c
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,9 @@ typedef struct
}PortData;

static PortData *portData;
extern int DEFAULT_BAUDRATE; // defined in port_handler.c
extern int g_used_port_num; // defined in port_handler.c
extern uint8_t *g_is_using; // defined in port_handler.c

int portHandlerMac(const char *port_name)
{
Expand Down
3 changes: 3 additions & 0 deletions c/src/dynamixel_sdk/port_handler_windows.c
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,9 @@ typedef struct
}PortData;

static PortData *portData;
extern int DEFAULT_BAUDRATE; // defined in port_handler.c
extern int g_used_port_num; // defined in port_handler.c
extern uint8_t *g_is_using; // defined in port_handler.c

int portHandlerWindows(const char *port_name)
{
Expand Down
5 changes: 4 additions & 1 deletion c/src/dynamixel_sdk/protocol1_packet_handler.c
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,9 @@
#define ERRBIT_OVERLOAD 32 // The current load cannot be controlled by the set torque.
#define ERRBIT_INSTRUCTION 64 // Undefined instruction or delivering the action command without the reg_write command.

extern PacketData *packetData; // defined in packet_handler.c
extern uint8_t *g_is_using; // defined in port_handler.c

const char *getTxRxResult1(int result)
{
switch (result)
Expand Down Expand Up @@ -701,7 +704,7 @@ void regWriteTxOnly1(int port_num, uint8_t id, uint16_t address, uint16_t length
}

txPacket1(port_num);

g_is_using[port_num] = False;
}

Expand Down
27 changes: 15 additions & 12 deletions c/src/dynamixel_sdk/protocol2_packet_handler.c
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,9 @@

#define ERRBIT_ALERT 128 //When the device has a problem, this bit is set to 1. Check "Device Status Check" value.

extern PacketData *packetData; // defined in packet_handler.c
extern uint8_t *g_is_using; // defined in port_handler.c

const char *getTxRxResult2(int result)
{
switch (result)
Expand Down Expand Up @@ -251,24 +254,24 @@ void addStuffing(uint8_t *packet)
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;

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++)
{
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;

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)
Expand Down Expand Up @@ -548,7 +551,7 @@ uint16_t pingGetModelNum2(int port_num, uint8_t id)
printf("[PingGetModeNum] memory allocation failed.. \n");
return COMM_TX_FAIL;
}

if (id >= BROADCAST_ID)
{
packetData[port_num].communication_result = COMM_NOT_AVAILABLE;
Expand Down Expand Up @@ -749,7 +752,7 @@ void clearMultiTurn2(int port_num, uint8_t id)
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;
Expand All @@ -772,7 +775,7 @@ void factoryReset2(int port_num, uint8_t id, uint8_t option)
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;
Expand All @@ -792,7 +795,7 @@ void readTx2(int port_num, uint8_t id, uint16_t address, uint16_t length)
printf("[ReadTx] memory allocation failed..\n");
return;
}

if (id >= BROADCAST_ID)
{
packetData[port_num].communication_result = COMM_NOT_AVAILABLE;
Expand Down Expand Up @@ -828,7 +831,7 @@ void readRx2(int port_num, uint16_t length)
printf("[ReadRx] memory allocation failed..\n");
return;
}

rxPacket2(port_num);
if (packetData[port_num].communication_result == COMM_SUCCESS)
{
Expand All @@ -854,7 +857,7 @@ void readTxRx2(int port_num, uint8_t id, uint16_t address, uint16_t length)
printf("[ReadTxRx] memory allocation failed..\n");
return;
}

if (id >= BROADCAST_ID)
{
packetData[port_num].communication_result = COMM_NOT_AVAILABLE;
Expand Down Expand Up @@ -1032,7 +1035,7 @@ void writeTxRx2(int port_num, uint8_t id, uint16_t address, uint16_t length)
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);
Expand Down