long urg_currentTimestamp(urg_t *urg)
{
char buffer[ScipLineWidth];
long timestamp = -1;
int ret = 0;
int n;
/* Send TM1 */
int expected_ret[] = { 0, -1 };
int send_n = scip_send(&urg->serial_, "TM1\n");
if (send_n != 4) {
return SerialSendFail;
}
ret = scip_recv(&urg->serial_, "TM", NULL, expected_ret, ScipTimeout);
if (ret != 0) {
return ret;
}
/* Decode the timestamp and return */
n = serial_getLine(&urg->serial_, buffer, ScipLineWidth, ScipTimeout);
if (n == 5) {
timestamp = decode(buffer, 4);
}
/* Read and throw the last response */
n = serial_recv(&urg->serial_, buffer, 1, ScipTimeout);
if (! serial_isLF(buffer[0])) {
serial_ungetc(&urg->serial_, buffer[0]);
}
return timestamp;
}
int serial_readline(urg_serial_t *serial, char *data, int max_size, int timeout)
{
/* \~japanese １文字ずつ読み出して評価する */
/* \~english Reads and evaluates 1 character at a time */
int filled = 0;
int is_timeout = 0;
while (filled < max_size) {
char recv_ch;
int n = serial_read(serial, &recv_ch, 1, timeout);
if (n <= 0) {
is_timeout = 1;
break;
} else if (is_linefeed(recv_ch)) {
break;
}
data[filled++] = recv_ch;
}
if (filled >= max_size) {
--filled;
serial_ungetc(serial, data[filled]);
}
data[filled] = '\0';
if ((filled == 0) && is_timeout) {
return -1;
} else {
//fprintf(stderr, "%s\n", data);
return filled;
}
}
