Exemple #1
0
int
MB12XX::collect()
{
	int	ret = -EIO;

	/* read from the sensor */
	uint8_t val[2] = {0, 0};

	perf_begin(_sample_perf);

	ret = transfer(nullptr, 0, &val[0], 2);

	if (ret < 0) {
		log("error reading from sensor: %d", ret);
		perf_count(_comms_errors);
		perf_end(_sample_perf);
		return ret;
	}

	uint16_t distance = val[0] << 8 | val[1];
	float si_units = (distance * 1.0f) / 100.0f; /* cm to m */
	struct range_finder_report report;

	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
	report.timestamp = hrt_absolute_time();
	report.error_count = perf_event_count(_comms_errors);
	report.distance = si_units;
	report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;

	/* publish it, if we are the primary */
	if (_range_finder_topic >= 0) {
		orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
	}

	if (_reports->force(&report)) {
		perf_count(_buffer_overflows);
	}

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	ret = OK;

	perf_end(_sample_perf);
	return ret;
}
int LidarLitePWM::measure()
{
	perf_begin(_sample_perf);

	if (OK != collect()) {
		debug("collection error");
		perf_count(_read_errors);
		perf_end(_sample_perf);
		return ERROR;
	}

	_range.timestamp = hrt_absolute_time();
	_range.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
	_range.max_distance = get_maximum_distance();
	_range.min_distance = get_minimum_distance();
	_range.current_distance = float(_pwm.pulse_width) * 1e-3f;   /* 10 usec = 1 cm distance for LIDAR-Lite */
	_range.covariance = 0.0f;
	_range.orientation = 8;
	/* TODO: set proper ID */
	_range.id = 0;

	/* Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) */
	if (_range.current_distance <= 0.0f) {
		perf_count(_sensor_zero_resets);
		perf_end(_sample_perf);
		return reset_sensor();
	}

	if (_distance_sensor_topic != nullptr) {
		orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &_range);
	}

	if (_reports->force(&_range)) {
		perf_count(_buffer_overflows);
	}

	poll_notify(POLLIN);
	perf_end(_sample_perf);
	return OK;
}
Exemple #3
0
int
LL40LS::collect()
{
    int	ret = -EIO;

    /* read from the sensor */
    uint8_t val[2] = {0, 0};

    perf_begin(_sample_perf);

    // read the high and low byte distance registers
    uint8_t distance_reg = LL40LS_DISTHIGH_REG;
    ret = transfer(&distance_reg, 1, &val[0], sizeof(val));

    if (ret < 0) {
        if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) {
            /*
              NACKs from the sensor are expected when we
              read before it is ready, so only consider it
              an error if more than 100ms has elapsed.
             */
            debug("error reading from sensor: %d", ret);
            perf_count(_comms_errors);
            if (perf_event_count(_comms_errors) % 10 == 0) {
                perf_count(_sensor_resets);
                reset_sensor();
            }
        }
        perf_end(_sample_perf);
        // if we are getting lots of I2C transfer errors try
        // resetting the sensor
        return ret;
    }

    uint16_t distance = (val[0] << 8) | val[1];
    float si_units = distance * 0.01f; /* cm to m */
    struct range_finder_report report;

    if (distance == 0) {
        _zero_counter++;
        if (_zero_counter == 20) {
            /* we have had 20 zeros in a row - reset the
               sensor. This is a known bad state of the
               sensor where it returns 16 bits of zero for
               the distance with a trailing NACK, and
               keeps doing that even when the target comes
               into a valid range.
            */
            _zero_counter = 0;
            perf_end(_sample_perf);
            perf_count(_sensor_zero_resets);
            return reset_sensor();
        }
    } else {
        _zero_counter = 0;
    }

    _last_distance = distance;

    /* this should be fairly close to the end of the measurement, so the best approximation of the time */
    report.timestamp = hrt_absolute_time();
    report.error_count = perf_event_count(_comms_errors);
    report.distance = si_units;
    report.minimum_distance = get_minimum_distance();
    report.maximum_distance = get_maximum_distance();
    if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) {
        report.valid = 1;
    }
    else {
        report.valid = 0;
    }

    /* publish it, if we are the primary */
    if (_range_finder_topic >= 0) {
        orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
    }

    if (_reports->force(&report)) {
        perf_count(_buffer_overflows);
    }

    /* notify anyone waiting for data */
    poll_notify(POLLIN);

    ret = OK;

    perf_end(_sample_perf);
    return ret;
}
Exemple #4
0
int
SF0X::collect()
{
	int	ret;

	perf_begin(_sample_perf);

	/* clear buffer if last read was too long ago */
	uint64_t read_elapsed = hrt_elapsed_time(&_last_read);

	if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
		_linebuf_index = 0;
	} else if (_linebuf_index > 0) {
		/* increment to next read position */
		_linebuf_index++;
	}

	/* the buffer for read chars is buflen minus null termination */
	unsigned readlen = sizeof(_linebuf) - 1;

	/* read from the sensor (uart buffer) */
	ret = ::read(_fd, &_linebuf[_linebuf_index], readlen - _linebuf_index);

	if (ret < 0) {
		_linebuf[sizeof(_linebuf) - 1] = '\0';
		debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf);
		perf_count(_comms_errors);
		perf_end(_sample_perf);

		/* only throw an error if we time out */
		if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
			return ret;

		} else {
			return -EAGAIN;
		}
	} else if (ret == 0) {
		return -EAGAIN;
	}

	/* we did increment the index to the next position already, so just add the additional fields */
	_linebuf_index += (ret - 1);

	_last_read = hrt_absolute_time();

	if (_linebuf_index < 1) {
		/* we need at least the two end bytes to make sense of this string */
		return -EAGAIN;

	} else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') {

		if (_linebuf_index >= readlen - 1) {
			/* we have a full buffer, but no line ending - abort */
			_linebuf_index = 0;
			perf_count(_comms_errors);
			return -ENOMEM;
		} else {
			/* incomplete read, reschedule ourselves */
			return -EAGAIN;
		}
	}

	char *end;
	float si_units;
	bool valid;

	/* enforce line ending */
	unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1);

	_linebuf[lend] = '\0';

	if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
		si_units = -1.0f;
		valid = false;

	} else {

		/* we need to find a dot in the string, as we're missing the meters part else */
		valid = false;

		/* wipe out partially read content from last cycle(s), check for dot */
		for (int i = 0; i < (lend - 2); i++) {
			if (_linebuf[i] == '\n') {
				char buf[sizeof(_linebuf)];
				memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1));
				memcpy(_linebuf, buf, (lend + 1) - (i + 1));
			}

			if (_linebuf[i] == '.') {
				valid = true;
			}
		}

		if (valid) {
			si_units = strtod(_linebuf, &end);

			/* we require at least 3 characters for a valid number */
			if (end > _linebuf + 3) {
				valid = true;
			} else {
				si_units = -1.0f;
				valid = false;
			}
		}
	}

	debug("val (float): %8.4f, raw: %s, valid: %s\n", si_units, _linebuf, ((valid) ? "OK" : "NO"));

	/* done with this chunk, resetting - even if invalid */
	_linebuf_index = 0;

	/* if its invalid, there is no reason to forward the value */
	if (!valid) {
		perf_count(_comms_errors);
		return -EINVAL;
	}

	struct range_finder_report report;

	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
	report.timestamp = hrt_absolute_time();
	report.error_count = perf_event_count(_comms_errors);
	report.distance = si_units;
	report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0);

	/* publish it */
	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);

	if (_reports->force(&report)) {
		perf_count(_buffer_overflows);
	}

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	ret = OK;

	perf_end(_sample_perf);
	return ret;
}
Exemple #5
0
int LidarLiteI2C::collect()
{
	int ret = -EIO;

	/* read from the sensor */
	uint8_t val[2] = {0, 0};

	perf_begin(_sample_perf);

	// read the high and low byte distance registers
	uint8_t distance_reg = LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT;
	ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val));

	// if the transfer failed or if the high bit of distance is
	// set then the distance is invalid
	if (ret < 0 || (val[0] & 0x80)) {
		if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) {
			/*
			  NACKs from the sensor are expected when we
			  read before it is ready, so only consider it
			  an error if more than 100ms has elapsed.
			 */
			DEVICE_DEBUG("error reading from sensor: %d", ret);
			perf_count(_comms_errors);

			if (perf_event_count(_comms_errors) % 10 == 0) {
				perf_count(_sensor_resets);
				reset_sensor();
			}
		}

		perf_end(_sample_perf);
		// if we are getting lots of I2C transfer errors try
		// resetting the sensor
		return ret;
	}

	uint16_t distance_cm = (val[0] << 8) | val[1];
	float distance_m = float(distance_cm) * 1e-2f;
	struct distance_sensor_s report;

	if (distance_cm == 0) {
		_zero_counter++;

		if (_zero_counter == 20) {
			/* we have had 20 zeros in a row - reset the
			   sensor. This is a known bad state of the
			   sensor where it returns 16 bits of zero for
			   the distance with a trailing NACK, and
			   keeps doing that even when the target comes
			   into a valid range.
			*/
			_zero_counter = 0;
			perf_end(_sample_perf);
			perf_count(_sensor_zero_resets);
			return reset_sensor();
		}

	} else {
		_zero_counter = 0;
	}

	_last_distance = distance_cm;


	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
	report.timestamp = hrt_absolute_time();
	report.current_distance = distance_m;
	report.min_distance = get_minimum_distance();
	report.max_distance = get_maximum_distance();
	report.covariance = 0.0f;
	/* the sensor is in fact a laser + sonar but there is no enum for this */
	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
	report.orientation = _rotation;
	/* TODO: set proper ID */
	report.id = 0;

	/* publish it, if we are the primary */
	if (_distance_sensor_topic != nullptr) {
		orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
	}

	_reports->force(&report);

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	ret = OK;

	perf_end(_sample_perf);
	return ret;
}
Exemple #6
0
int
SF0X::collect()
{
	int	ret;

	perf_begin(_sample_perf);

	/* clear buffer if last read was too long ago */
	uint64_t read_elapsed = hrt_elapsed_time(&_last_read);

	/* the buffer for read chars is buflen minus null termination */
	char readbuf[sizeof(_linebuf)];
	unsigned readlen = sizeof(readbuf) - 1;

	/* read from the sensor (uart buffer) */
	ret = ::read(_fd, &readbuf[0], readlen);

	if (ret < 0) {
		DEVICE_DEBUG("read err: %d", ret);
		perf_count(_comms_errors);
		perf_end(_sample_perf);

		/* only throw an error if we time out */
		if (read_elapsed > (_conversion_interval * 2)) {
			return ret;

		} else {
			return -EAGAIN;
		}

	} else if (ret == 0) {
		return -EAGAIN;
	}

	_last_read = hrt_absolute_time();

	float distance_m = -1.0f;
	bool valid = false;

	for (int i = 0; i < ret; i++) {
		if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) {
			valid = true;
		}
	}

	if (!valid) {
		return -EAGAIN;
	}

	DEVICE_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO"));

	struct distance_sensor_s report;

	report.timestamp = hrt_absolute_time();
	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
	report.orientation = _rotation;
	report.current_distance = distance_m;
	report.min_distance = get_minimum_distance();
	report.max_distance = get_maximum_distance();
	report.covariance = 0.0f;
	/* TODO: set proper ID */
	report.id = 0;

	/* publish it */
	orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);

	_reports->force(&report);

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	ret = OK;

	perf_end(_sample_perf);
	return ret;
}
Exemple #7
0
int

/*
 * Method: collect()
 *
 * This method reads data from serial UART and places it into a buffer
*/
CM8JL65::collect()
{
	int bytes_read = 0;
	int bytes_processed = 0;
	int i = 0;
	bool crc_valid = false;


	perf_begin(_sample_perf);


	/* read from the sensor (uart buffer) */
	bytes_read = ::read(_fd, &_linebuf[0], sizeof(_linebuf));


	if (bytes_read < 0) {
		PX4_DEBUG("read err: %d \n", bytes_read);
		perf_count(_comms_errors);
		perf_end(_sample_perf);

	} else if (bytes_read > 0) {
		//     printf("Bytes read: %d \n",bytes_read);
		i = bytes_read - 6 ;

		while ((i >= 0) && (!crc_valid)) {
			if (_linebuf[i] == START_FRAME_DIGIT1) {
				bytes_processed = i;

				while ((bytes_processed < bytes_read) && (!crc_valid)) {
					//    printf("In the cycle, processing  byte %d, 0x%02X \n",bytes_processed, _linebuf[bytes_processed]);
					if (OK == cm8jl65_parser(_linebuf[bytes_processed], _frame_data, _parse_state, _crc16, _distance_mm)) {
						crc_valid = true;
					}

					bytes_processed++;
				}

				_parse_state = STATE0_WAITING_FRAME;

			}

			i--;
		}

	}

	if (!crc_valid) {
		return -EAGAIN;
	}


	//printf("val (int): %d, raw: 0x%08X, valid: %s \n", _distance_mm, _frame_data, ((crc_valid) ? "OK" : "NO"));

	struct distance_sensor_s report;

	report.timestamp = hrt_absolute_time();
	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
	report.orientation = _rotation;
	report.current_distance = _distance_mm / 1000.0f;
	report.min_distance = get_minimum_distance();
	report.max_distance = get_maximum_distance();
	report.variance = 0.0f;
	report.signal_quality = -1;
	/* TODO: set proper ID */
	report.id = 0;

	/* publish it */
	orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);

	_reports->force(&report);

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	bytes_read = OK;

	perf_end(_sample_perf);

	return OK;

}
Exemple #8
0
int
MB12XX::collect()
{
	int	ret = -EIO;

	/* read from the sensor */
	uint8_t val[2] = {0, 0};

	perf_begin(_sample_perf);

	ret = transfer(nullptr, 0, &val[0], 2);

	if (ret < 0) {
		debug("error reading from sensor: %d", ret);
		perf_count(_comms_errors);
		perf_end(_sample_perf);
		return ret;
	}

	uint16_t distance = val[0] << 8 | val[1];
	float si_units = (distance * 1.0f) / 100.0f; /* cm to m */
	struct range_finder_report report;

	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
	report.timestamp = hrt_absolute_time();
	report.error_count = perf_event_count(_comms_errors);

	/* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */
	if (addr_ind.size() == 1) {
		report.distance = si_units;

		for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS); i++) {
			report.distance_vector[i] = 0;
		}

		report.just_updated = 0;

	} else {
		/* for multiple sonars connected */

		/* don't use the orginial single sonar variable */
		report.distance = 0;

		/* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values 			of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest 			value for each connected sonar */
		_latest_sonar_measurements[_cycle_counter] = si_units;

		for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) {
			report.distance_vector[i] = _latest_sonar_measurements[i];
		}

		/* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been 		collected as this could be of use for Kalman filters */
		report.just_updated = _index_counter;

		/* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */
		for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS - addr_ind.size()); i++) {
			report.distance_vector[addr_ind.size() + i] = 0;
		}
	}

	report.minimum_distance = get_minimum_distance();
	report.maximum_distance = get_maximum_distance();
	report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;

	/* publish it, if we are the primary */
	if (_range_finder_topic >= 0) {
		orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
	}

	if (_reports->force(&report)) {
		perf_count(_buffer_overflows);
	}

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	ret = OK;

	perf_end(_sample_perf);
	return ret;
}
Exemple #9
0
int
SF0X::collect()
{
    int	ret;

    perf_begin(_sample_perf);

    /* clear buffer if last read was too long ago */
    uint64_t read_elapsed = hrt_elapsed_time(&_last_read);

    /* the buffer for read chars is buflen minus null termination */
    char readbuf[sizeof(_linebuf)];
    unsigned readlen = sizeof(readbuf) - 1;

    /* read from the sensor (uart buffer) */
    ret = ::read(_fd, &readbuf[0], readlen);

    if (ret < 0) {
        debug("read err: %d", ret);
        perf_count(_comms_errors);
        perf_end(_sample_perf);

        /* only throw an error if we time out */
        if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
            return ret;

        } else {
            return -EAGAIN;
        }
    } else if (ret == 0) {
        return -EAGAIN;
    }

    _last_read = hrt_absolute_time();

    float si_units;
    bool valid = false;

    for (int i = 0; i < ret; i++) {
        if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) {
            valid = true;
        }
    }

    if (!valid) {
        return -EAGAIN;
    }

    debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));

    struct range_finder_report report;

    /* this should be fairly close to the end of the measurement, so the best approximation of the time */
    report.timestamp = hrt_absolute_time();
    report.error_count = perf_event_count(_comms_errors);
    report.distance = si_units;
    report.minimum_distance = get_minimum_distance();
    report.maximum_distance = get_maximum_distance();
    report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0);

    /* publish it */
    orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);

    if (_reports->force(&report)) {
        perf_count(_buffer_overflows);
    }

    /* notify anyone waiting for data */
    poll_notify(POLLIN);

    ret = OK;

    perf_end(_sample_perf);
    return ret;
}
Exemple #10
0
int
TFMINI::collect()
{
	perf_begin(_sample_perf);

	/* clear buffer if last read was too long ago */
	int64_t read_elapsed = hrt_elapsed_time(&_last_read);

	/* the buffer for read chars is buflen minus null termination */
	char readbuf[sizeof(_linebuf)];
	unsigned readlen = sizeof(readbuf) - 1;

	int ret = 0;
	float distance_m = -1.0f;

	/* Check the number of bytes available in the buffer*/
	int bytes_available = 0;
	::ioctl(_fd, FIONREAD, (unsigned long)&bytes_available);

	if (!bytes_available) {
		return -EAGAIN;
	}

	/* parse entire buffer */
	do {
		/* read from the sensor (uart buffer) */
		ret = ::read(_fd, &readbuf[0], readlen);

		if (ret < 0) {
			PX4_ERR("read err: %d", ret);
			perf_count(_comms_errors);
			perf_end(_sample_perf);

			/* only throw an error if we time out */
			if (read_elapsed > (_conversion_interval * 2)) {
				/* flush anything in RX buffer */
				tcflush(_fd, TCIFLUSH);
				return ret;

			} else {
				return -EAGAIN;
			}
		}

		_last_read = hrt_absolute_time();

		/* parse buffer */
		for (int i = 0; i < ret; i++) {
			tfmini_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m);
		}

		/* bytes left to parse */
		bytes_available -= ret;

	} while (bytes_available > 0);

	/* no valid measurement after parsing buffer */
	if (distance_m < 0.0f) {
		return -EAGAIN;
	}

	/* publish most recent valid measurement from buffer */
	distance_sensor_s report{};

	report.timestamp = hrt_absolute_time();
	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
	report.orientation = _rotation;
	report.current_distance = distance_m;
	report.min_distance = get_minimum_distance();
	report.max_distance = get_maximum_distance();
	report.covariance = 0.0f;
	report.signal_quality = -1;
	/* TODO: set proper ID */
	report.id = 0;

	/* publish it */
	orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);

	_reports->force(&report);

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	ret = OK;

	perf_end(_sample_perf);
	return ret;
}