/**
* @brief Waits for the specified reply for timeo ms
*
* @param dev pointer to communication structure
* @param rqst identifier of the response to wait for
* @param timeo < 0 -> block
* timeo = 0 -> no wait
* timeo > 0 wait..
*
* @return 0 if message arrived; <0 (-errno) otherwise;
*/
int
sickldmrs_wait_reply(struct sickldmrs_device *dev, int16_t rqst, int timeo)
{
struct timespec d, ts;
int rc;
sickldmrs_lock(dev);
if (timeo == 0) {
if (dev->priv->pending == rqst) {
dev->priv->pending = -1;
sickldmrs_unlock(dev);
return 0;
}
}
if (timeo > 0) {
d.tv_sec = timeo / 1000;
d.tv_nsec = (timeo % 1000) * 1000000;
clock_gettime(CLOCK_REALTIME, &ts);
timespec_add(&ts, &d);
rc = 0;
/* wait for condition variable with timeout */
while (dev->priv->pending != rqst && rc == 0)
rc = pthread_cond_timedwait(&dev->priv->cond,
&dev->priv->lock, &ts);
dev->priv->pending = -1;
sickldmrs_unlock(dev);
return -rc;
}
/* timeo < 0 -> blocking wait for condition variable */
while ((dev->priv->pending & 0x1fff) != rqst) {
rc = pthread_cond_wait(&dev->priv->cond, &dev->priv->lock);
if (rc != 0) {
warnx("%s: %s\n", __func__, strerror(rc));
sickldmrs_unlock(dev);
return -rc;
}
}
/* check for error bit in the command id */
if ((dev->priv->pending & 0x8000) != 0)
rc = -EIO;
else
rc = 0;
dev->priv->pending = -1;
sickldmrs_unlock(dev);
return rc;
}
int
main(int argc, char *argv[])
{
struct sickldmrs_device *dev;
char *address, *port;
int rc;
address = argv[1];
port = argv[2];
signal(SIGINT, interrupt);
signal(SIGTERM, interrupt);
dev = sickldmrs_init(address, port, true);
if (dev == NULL)
exit(2);
dev->debug = 1;
if ((rc = sickldmrs_get_status(dev, -1)) < 0)
errx(2, "sickldmrs_get_status: %s\n", strerror(-rc));
if ((rc = sickldmrs_config_output(dev, 0x00ee, -1)) < 0)
errx(2, "sickldmrs_config_output: %s\n", strerror(rc));
/* scan frequency -> 25 Hz */
if ((rc = sickldmrs_set_parameter(dev,
SLDMRS_PARAM_SCAN_FREQUENCY, 6400, -1)) < 0)
errx(2, "sickldmrs_set_parameter: %s", strerror(rc));
if ((rc = sickldmrs_get_parameter(dev,
SLDMRS_PARAM_SCAN_FREQUENCY, -1)) < 0)
errx(2, "sickldmrs_get_parameter: %s", strerror(rc));
while (!done) {
usleep(40000); /* 25Hz */
sickldmrs_lock(dev);
if (dev->scan != NULL && dev->scan->scan_number != last_frame) {
sickldmrs_print_scan(dev->scan);
last_frame = dev->scan->scan_number;
}
sickldmrs_unlock(dev);
}
sickldmrs_end(dev);
printf("bye\n");
return 0;
}
/**
* receive and decode error and warning messages (section 6 pp 27-30)
*/
static int
sickldmrs_recv_error(struct sickldmrs_device *dev, struct sickldmrs_header *h)
{
struct sickldmrs_error e;
ssize_t len;
len = recv(dev->priv->fd, &e, sizeof(e), MSG_WAITALL);
if (len <= 0) {
warn("%s: recv error", __func__);
return -1;
}
/* store errors */
sickldmrs_lock(dev);
dev->errors.error_register_1 = le16toh(e.error_register_1);
dev->errors.error_register_2 = le16toh(e.error_register_2);
dev->errors.warning_register_1 = le16toh(e.warning_register_1);
dev->errors.warning_register_2 = le16toh(e.warning_register_2);
sickldmrs_unlock(dev);
return 0;
}
/**
* receive and decode a command reply (section 5 pp 13-26)
*/
static int
sickldmrs_recv_reply(struct sickldmrs_device *dev, struct sickldmrs_header *h)
{
uint8_t buf[64];
ssize_t len;
struct sickldmrs_reply *r;
struct sickldmrs_status *s;
struct sickldmrs_param *p;
uint16_t rid;
len = h->msg_size;
len = recv(dev->priv->fd, buf, len, MSG_WAITALL);
if (len <= 0) {
warn("%s: recv error", __func__);
return -1;
}
r = (struct sickldmrs_reply *)&buf[0];
rid = le16toh(r->reply_id);
sickldmrs_lock(dev);
if (dev->debug)
printf("-- %s 0x%04x\n", __func__, rid);
switch (rid) {
case SLDMRS_CMD_GET_STATUS:
s = (struct sickldmrs_status *)&buf[0];
if (len != sizeof(struct sickldmrs_status)) {
warnx("%s: size mismatch %ld, %lu\n", __func__,
len, sizeof(struct sickldmrs_status));
sickldmrs_unlock(dev);
return -1;
}
snprintf(dev->firmware_version, sizeof(dev->firmware_version),
"%x.%02x.%x",
(s->firmware_version & 0xf000) >> 12,
(s->firmware_version & 0x0ff0) >> 4,
(s->firmware_version & 0x000f));
snprintf(dev->fpga_version, sizeof(dev->fpga_version),
"%x.%02x.%x",
(s->fpga_version & 0xf000) >> 12,
(s->fpga_version & 0x0ff0) >> 4,
(s->fpga_version & 0x000f));
snprintf(dev->serial_number, sizeof(dev->serial_number),
"%04x.%04x.%04x", s->serial_number_0,
s->serial_number_1, s->serial_number_2);
dev->status = s->scanner_status;
if (s->temperature < 0x7fff)
dev->temperature = -(s->temperature - 579.2364)/3.63;
else
dev->temperature = -273; /* XXX invalid */
if (dev->debug) {
printf("Device status: 0x%04x\n", dev->status);
printf(" temperature: %02f\n", dev->temperature);
printf(" dsp %s fpga %s serial %s\n", dev->firmware_version,
dev->fpga_version, dev->serial_number);
}
break;
case SLDMRS_CMD_GET_PARAM:
p = (struct sickldmrs_param *)&buf[0];
if (len != sizeof(struct sickldmrs_param)) {
warnx("%s: size mismatch %ld, %lu\n", __func__,
len, sizeof(struct sickldmrs_param));
sickldmrs_unlock(dev);
return -1;
}
if (dev->debug)
printf("-- get param: %04x value %08x\n",
le16toh(p->param_index), le32toh(p->param_value));
break;
case SLDMRS_CMD_RESET:
case SLDMRS_CMD_SAVE_CONFIG:
case SLDMRS_CMD_SET_PARAM:
case SLDMRS_CMD_RESET_FACTORY_DEFAULTS:
case SLDMRS_CMD_START_MEASURE:
case SLDMRS_CMD_STOP_MEASURE:
case SLDMRS_CMD_SET_NTP_SEC:
case SLDMRS_CMD_SET_NTP_FRAC_SEC:
if (len != sizeof(struct sickldmrs_reply)) {
warnx("%s: size mismatch %ld, %lu\n", __func__,
len, sizeof(struct sickldmrs_reply));
sickldmrs_unlock(dev);
return -1;
}
break;
default:
if ((rid & 0x8000) != 0) /* command error */
break;
warnx("%s: unknown command reply %x\n", __func__,
r->reply_id);
sickldmrs_unlock(dev);
return -1;
}
dev->priv->pending = rid;
pthread_cond_broadcast(&dev->priv->cond);
sickldmrs_unlock(dev);
return 0;
}
