// === Bus specific implementation ===================
// Send an instruction with the specified parameters. The error code,
// body, and checksum of the response are returned (while the initial 4
// bytes of the header are removed)
static dynamixel_msg_t *
send_command_raw(dynamixel_bus_t *bus,
uint8_t id,
int instruction,
dynamixel_msg_t *params)
{
dynamixel_serial_bus_impl_t *impl = (bus->impl);
// Missing synchronization
int parameterlen = (params == NULL) ? 0 : params->len;
uint8_t *cmd = malloc((6+parameterlen)*sizeof(uint8_t));
cmd[0] = 255; // MAGIC
cmd[1] = 255; // MAGIC
cmd[2] = id; // servo id
cmd[3] = (uint8_t)(parameterlen+2) & 0xff; // Length
cmd[4] = (uint8_t)(instruction & 0xff);
if (params != NULL) {
for (int i = 0; i < params->len; i++)
cmd[5+i] = params->buf[i];
}
int checksum = 0;
for (int i = 2; i < parameterlen+6-1; i++) {
checksum += (cmd[i] & 0xff);
}
cmd[5+parameterlen] = (uint8_t)((checksum ^ 0xff) & 0xff);
int res = write(impl->fd, cmd, 6+parameterlen);
if (VERBOSE) {
// XXX Dump cmd...which isn't a msg
}
free(cmd);
// Read response. The header is really 5 bytes, but we put the
// error code in the body so that the caller knows what went wrong
// if something bad happens. Synchronize on the first two 0xffff
// characters.
dynamixel_msg_t *header = dynamixel_msg_create(4);
int header_have = 0;
while (header_have < 4) {
res = read_fully_timeout(impl->fd,
(header->buf)+header_have,
4 - header_have,
TIMEOUT_MS);
if (VERBOSE) {
printf("READ: res = %d : ", res);
dynamixel_msg_dump(header);
}
if (res < 1)
return NULL;
assert (res <= (4 - header_have));
//assert (res + header_have == 4);
// If the first two bytes are the sync bytes, we're done
if ((header->buf[0] & 0xff) == 0xff && (header->buf[1] & 0xff) == 0xff)
break;
// Shift buffer, read one more character
header_have = 3;
for (int i = 0; i < 3; i++)
header->buf[i] = header->buf[i+1];
}
if ((header->buf[2] & 0xff) != id) {
printf("serial_bus: Received response for wrong servo %d\n",
header->buf[2] & 0xff);
return NULL;
}
//int thisid = header->buf[2] & 0xff;
int length = header->buf[3] & 0xff;
if (length < 2)
return NULL;
dynamixel_msg_t *body = dynamixel_msg_create(length);
res = read_fully_timeout(impl->fd,
body->buf,
body->len,
TIMEOUT_MS);
if (VERBOSE) {
printf("READ: res = %d : ", res);
dynamixel_msg_dump(body);
}
if (1) {
int checksum = 0;
for (int i = 2; i < header->len; i++)
checksum += (header->buf[i] & 0xff);
for (int i = 0; i < body->len-1; i++)
checksum += (body->buf[i] & 0xff);
checksum = (checksum & 0xff) ^ 0xff;
if ((body->buf[body->len - 1] & 0xff) != checksum) {
printf("serial_bus: Bad checksum %02x %02x\n",
body->buf[body->len - 1] & 0xff,
checksum);
return NULL;
}
}
dynamixel_msg_destroy(header);
return body;
}
int sick_read_packet(sick_t *s, uint8_t *data)
{
int res;
int datalen = 0; // how many bytes of data[] are valid?
int chk, chk2;
int want;
readmore:
// printf("%i\n", rxlen);
want = 4 - datalen;
if (want > 0) {
// we're willing to wait forever for these bytes
// (this prevents us from spinning in a poll loop
res = read_fully_timeout(s->serialfd, &data[datalen], want, -1);
rxlen += res;
if (res <= 0)
return -1;
datalen += want;
}
// two cases: either the 4 bytes consistute a good header, or
// we skip along to the next occurence of an STX and try
// again.
// is this header good?
int payloadlen = payload_length(data);
if (data[0] != 0x02 || data[1] != 0x80 || payloadlen >= (SICK_MAX_MSG_LENGTH - 6)) {
goto resync;
}
// this header is good. read the message (plus checksum bytes)
want = payloadlen + 6 - datalen;
if (want > 0) {
res = read_fully_timeout(s->serialfd, &data[datalen], want, SICK_RX_TIMEOUT_MS);
rxlen+=res;
if (res <= 0)
return -2;
datalen += want;
}
// is the checksum good?
chk = data[4+payloadlen] + (data[5+payloadlen]<<8);
chk2 = sick_compute_checksum(data);
if (chk != chk2) {
printf("bad chk: %04X != %04X\n", chk, chk2);
goto resync;
}
// good packet received!
if (s->log_packets_file) {
for (int i = 0; i < datalen; i++) {
if (i%16 == 0)
fprintf(s->log_packets_file, "RX %04x : ", i);
fprintf(s->log_packets_file, "%02x ", data[i]);
if ((i%16) == 15 || i+1 == datalen)
fprintf(s->log_packets_file, "\n");
}
}
// is this the response to a request?
pthread_mutex_lock(&s->writelock);
if (s->writedata != NULL && data[4] == s->writereqid) {
memcpy(s->writedata, data, packet_length(data));
s->writevalid = 1;
pthread_cond_signal(&s->writecond);
}
pthread_mutex_unlock(&s->writelock);
// is it a laser scan?
if (data[4] == 0xb0) {
sick_handle_scan_b0(s, data);
} else if (data[4] == 0xf5) {
sick_handle_scan_f5(s, data);
}
return 0;
resync:
for (int i = 1; i < datalen; i++) {
if (data[i] == 0x02) {
memmove(data, &data[i], datalen - i);
datalen = datalen - i;
goto readmore;
}
}
// no STX found, start from scratch
datalen = 0;
goto readmore;
}
