1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45
| int servoSendRequestAndCheckResponse(unsigned char *tx, size_t len_tx,
unsigned char *rx, size_t len_rx,
unsigned char startOfFrame,
unsigned char srcFrame) {
int rc = 0;
unsigned char crc = 0;
// send request
if (serial_write(tx, len_tx) < 0) {
rc = SERVO_TX_ERROR;
log_error("TX error: Frame %s", FRAME_LABEL[srcFrame]);
goto __error;
}
// receive response
if (serial_read(rx, len_rx) < 0) {
rc = SERVO_RX_ERROR;
log_error("RX error: Frame %s", FRAME_LABEL[srcFrame]);
goto __error;
}
// check start of frame
if (rx[0] != startOfFrame) {
rc = SERVO_SYNC_FRAME_ERROR;
printf("Receive: ");
for (int i = 0; i < len_rx; i++) {
printf("[%02X]", rx[i]);
}
printf("\n");
log_error("Start of frame error: Frame %s, received: %02X", FRAME_LABEL[srcFrame], rx[0]);
goto __error;
}
// check CRC
if (rx[len_rx - 1] != (crc = crc8(rx, len_rx - 1))) {
rc = SERVO_CRC_ERROR;
log_error("CRC error: Frame %s, crc8: %02X, received: %02X", FRAME_LABEL[srcFrame], crc, rx[len_rx - 1]);
goto __error;
}
__error:
return rc;
} |
Partager