// === 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; }