Exemple #1
0
/**
 * @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;
}
Exemple #3
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;
}
Exemple #4
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;
}