Skip to content

Commit

Permalink
Maximum baudrate auto-sensing #649
Browse files Browse the repository at this point in the history
  • Loading branch information
matlo committed Jan 21, 2020
1 parent 10b7037 commit 30e4711
Show file tree
Hide file tree
Showing 2 changed files with 179 additions and 34 deletions.
6 changes: 5 additions & 1 deletion core/connectors/protocol.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,11 +28,15 @@ typedef struct GIMX_PACKED

#define HEADER_SIZE sizeof(s_header)

#define BYTE_NO_PACKET 0x00
#define BYTE_TYPE 0x11
#define BYTE_STATUS 0x22
#define BYTE_STATUS 0x22 // no more used
#define BYTE_START 0x33
#define BYTE_CONTROL_DATA 0x44
#define BYTE_RESET 0x55
#define BYTE_IDS 0x66
#define BYTE_VERSION 0x77
#define BYTE_BAUDRATE 0x88
#define BYTE_DEBUG 0x99
#define BYTE_OUT_REPORT 0xee
#define BYTE_IN_REPORT 0xff
Expand Down
207 changes: 174 additions & 33 deletions core/controller.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@

#include <haptic/haptic_core.h>

static const int baudrates[] = { 2000000, 1000000, 500000 }; //bps
#define BAUDRATE 500000 //bps
#define SERIAL_TIMEOUT 1000 //millisecond
/*
Expand All @@ -38,6 +39,10 @@
* The number of times the adapter is queried for its type, before it is assumed as unreachable.
*/
#define ADAPTER_INIT_RETRIES 10
/*
* The number of times the adapter is queried for its baudrate before assuming baudrate is not supported.
*/
#define ADAPTER_BAUDRATE_RETRIES 3

static s_adapter adapters[MAX_CONTROLLERS] = {};

Expand Down Expand Up @@ -517,6 +522,46 @@ static int adapter_start_serialasync(int adapter)
return 0;
}

int adapter_read_reply(int adapter, s_packet * packet, int permissive)
{
uint8_t type = packet->header.type;

/*
* The adapter may send a packet before it processes the command,
* so it is possible to receive a packet that is not the command response.
*/
while(1)
{
int ret = gserial_read_timeout(adapters[adapter].serial.device, &packet->header, sizeof(packet->header), SERIAL_TIMEOUT);
if(ret < 0 || (size_t)ret < sizeof(packet->header))
{
if (!permissive)
{
gerror("failed to read packet header from the GIMX adapter\n");
}
return -1;
}

ret = gserial_read_timeout(adapters[adapter].serial.device, &packet->value, packet->header.length, SERIAL_TIMEOUT);
if(ret < 0 || (size_t)ret < packet->header.length)
{
if (!permissive)
{
gerror("failed to read packet data from the GIMX adapter\n");
}
return -1;
}

//Check this packet is the command response.
if(packet->header.type == type)
{
return 0;
}
}

return -1;
}

/*
* This function should only be used in the initialization stages, i.e. before the mainloop.
*/
Expand All @@ -532,43 +577,94 @@ static int adapter_send_short_command(int adapter, unsigned char type)
};

int ret = gserial_write_timeout(adapters[adapter].serial.device, &packet.header, sizeof(packet.header), SERIAL_TIMEOUT);
if(ret < 0 || (unsigned int)ret < sizeof(packet.header))
if(ret < 0 || (size_t)ret < sizeof(packet.header))
{
gerror("failed to send data to the GIMX adapter\n");
return -1;
}

/*
* The adapter may send a packet before it processes the command,
* so it is possible to receive a packet that is not the command response.
*/
while(1)
ret = adapter_read_reply(adapter, &packet, 0);

if(ret == 0)
{
ret = gserial_read_timeout(adapters[adapter].serial.device, &packet.header, sizeof(packet.header), SERIAL_TIMEOUT);
if(ret < 0 || (unsigned int)ret < sizeof(packet.header))
if(packet.header.length != BYTE_LEN_1_BYTE)
{
gerror("failed to read packet header from the GIMX adapter\n");
gerror("bad response from the GIMX adapter (invalid length)\n");
return -1;
}
return packet.value[0];
}

ret = gserial_read_timeout(adapters[adapter].serial.device, &packet.value, packet.header.length, SERIAL_TIMEOUT);
if(ret < 0 || (unsigned int)ret < packet.header.length)
{
gerror("failed to read packet data from the GIMX adapter\n");
return -1;
}
return -1;
}

//Check this packet is the command response.
if(packet.header.type == type)
{
if(packet.header.length != BYTE_LEN_1_BYTE)
{
gerror("bad response from the GIMX adapter (invalid length)\n");
return -1;
}
static int adapter_get_version(int adapter, int *major, int *minor)
{
s_packet packet = { .header = { .type = BYTE_VERSION, .length = 0 }, .value = {} };

return packet.value[0];
}
int ret = gserial_write_timeout(adapters[adapter].serial.device, &packet, sizeof(packet.header), SERIAL_TIMEOUT);
if (ret < 0 || (size_t)ret != sizeof(packet.header))
{
gerror("failed to send data to the GIMX adapter\n");
return -1;
}

ret = adapter_read_reply(adapter, &packet, 1);

if(ret == 0 && packet.header.length == 2)
{
*major = packet.value[0];
*minor = packet.value[1];
}
else
{
*major = 5;
*minor = 8;
}

ginfo(_("Firmware version: %d.%d\n"), *major, *minor);

return 0;
}

static int adapter_get_baudrate(int adapter)
{
s_packet packet = { .header = { .type = BYTE_BAUDRATE, .length = 0 }, .value = {} };

int ret = gserial_write_timeout(adapters[adapter].serial.device, &packet, sizeof(packet.header), SERIAL_TIMEOUT);
if (ret < 0 || (size_t)ret != sizeof(packet.header))
{
gerror("failed to send data to the GIMX adapter\n");
return -1;
}

ret = adapter_read_reply(adapter, &packet, 1);

int baudrate = (ret != -1) ? packet.value[0] * 100000 : -1;

return baudrate;
}

static int adapter_get_baudrate_retry(int adapter, int retries)
{
int baudrate = -1;
int i;
for (i = 0; i < retries && baudrate == -1 && get_done() == 0; ++i)
{
baudrate = adapter_get_baudrate(adapter);
}
return baudrate;
}

static int adapter_set_baudrate(int adapter, int baudrate)
{
s_packet packet = { .header = { .type = BYTE_BAUDRATE, .length = 1 }, .value = { baudrate / 100000 } };

int ret = gserial_write_timeout(adapters[adapter].serial.device, &packet, sizeof(packet.header) + packet.header.length, SERIAL_TIMEOUT);
if (ret < 0 || (size_t)ret != sizeof(packet.header) + packet.header.length)
{
gerror("failed to send data to the GIMX adapter\n");
return -1;
}

return 0;
Expand All @@ -582,8 +678,10 @@ static int adapter_send_reset(int adapter)
.length = BYTE_LEN_0_BYTE
};

if(gserial_write_timeout(adapters[adapter].serial.device, &header, sizeof(header), SERIAL_TIMEOUT) != sizeof(header))
int ret = gserial_write_timeout(adapters[adapter].serial.device, &header, sizeof(header), SERIAL_TIMEOUT);
if(ret < 0 || (size_t)ret != sizeof(header))
{
gerror("failed to send data to the GIMX adapter\n");
return -1;
}

Expand Down Expand Up @@ -622,6 +720,7 @@ e_gimx_status adapter_detect()
{
if(adapter->serial.portname)
{
// open serial port at default baudrate, for backward compatibility
adapter->serial.device = gserial_open(adapter->serial.portname, BAUDRATE);
if(adapter->serial.device == NULL)
{
Expand Down Expand Up @@ -651,15 +750,16 @@ e_gimx_status adapter_detect()
ret = E_GIMX_STATUS_GENERIC_ERROR;
}

int status = adapter_send_short_command(i, BYTE_STATUS);

if(status < 0)
int major, minor;
if (ret == E_GIMX_STATUS_SUCCESS)
{
gerror(_("failed to get the GIMX adapter status.\n"));
ret = E_GIMX_STATUS_ADAPTER_NOT_DETECTED;
if (adapter_get_version(i, &major, &minor) < 0)
{
ret = E_GIMX_STATUS_GENERIC_ERROR;
}
}

if(ret != -1)
if(ret == E_GIMX_STATUS_SUCCESS)
{
if(adapter_send_reset(i) < 0)
{
Expand All @@ -674,6 +774,47 @@ e_gimx_status adapter_detect()
}
}

if (ret == E_GIMX_STATUS_SUCCESS && major >= 8)
{
int baudrate = adapter_get_baudrate_retry(i, ADAPTER_BAUDRATE_RETRIES);

if (baudrate > 0) {

unsigned int b;
for (b = 0; b < sizeof(baudrates) / sizeof(*baudrates); ++b)
{
if (baudrate == baudrates[b]) {
break;
}
adapter_set_baudrate(i, baudrates[b]);

ginfo(_("Trying baudrate: %d bps.\n"), baudrates[b]);

gserial_close(adapter->serial.device);

adapter->serial.device = gserial_open(adapter->serial.portname, baudrates[b]);
if(adapter->serial.device == NULL)
{
ret = E_GIMX_STATUS_GENERIC_ERROR;
continue;
}
baudrate = adapter_get_baudrate_retry(i, ADAPTER_BAUDRATE_RETRIES);
if (baudrate == baudrates[b]) {
break;
}
}

if (b == sizeof(baudrates) / sizeof(*baudrates))
{
ret = E_GIMX_STATUS_GENERIC_ERROR;
}

if (ret == E_GIMX_STATUS_SUCCESS){
ginfo(_("Using baudrate: %d bps.\n"), baudrate);
}
}
}

if(ret == E_GIMX_STATUS_SUCCESS)
{
int usb_res = usb_init(i, adapter->ctype);
Expand Down Expand Up @@ -1007,7 +1148,7 @@ int adapter_send()

unsigned int index = controller_build_report(adapter->ctype, adapter->axis, adapter->report);

s_report_packet* report = adapter->report+index;
s_report_packet* report = adapter->report + index;

switch(adapter->ctype)
{
Expand Down

0 comments on commit 30e4711

Please sign in to comment.