void LidarLiteI2C::cycle()
{
/* collection phase? */
if (_collect_phase) {
/* try a collection */
if (OK != collect()) {
DEVICE_DEBUG("collection error");
/* if we've been waiting more than 200ms then
send a new acquire */
if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT * 2) {
_collect_phase = false;
}
} else {
/* next phase is measurement */
_collect_phase = false;
/*
* Is there a collect->measure gap?
*/
if (getMeasureTicks() > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&LidarLiteI2C::cycle_trampoline,
this,
getMeasureTicks() - USEC2TICK(LL40LS_CONVERSION_INTERVAL));
return;
}
}
}
if (_collect_phase == false) {
/* measurement phase */
if (OK != measure()) {
DEVICE_DEBUG("measure error");
} else {
/* next phase is collection. Don't switch to
collection phase until we have a successful
acquire request I2C transfer */
_collect_phase = true;
}
}
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&LidarLiteI2C::cycle_trampoline,
this,
USEC2TICK(LL40LS_CONVERSION_INTERVAL));
}
void LidarLitePWM::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_read_errors);
perf_print_counter(_sensor_zero_resets);
warnx("poll interval: %u ticks", getMeasureTicks());
warnx("distance: %.3fm", (double)_range.current_distance);
}
ssize_t LidarLiteI2C::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct distance_sensor_s);
struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (getMeasureTicks() > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them.
*/
while (count--) {
if (_reports->get(rbuf)) {
ret += sizeof(*rbuf);
rbuf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement - run one conversion */
do {
_reports->flush();
/* trigger a measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
/* wait for it to complete */
usleep(LL40LS_CONVERSION_INTERVAL);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
break;
}
/* state machine will have generated a report, copy it out */
if (_reports->get(rbuf)) {
ret = sizeof(*rbuf);
}
} while (0);
return ret;
}
void LidarLiteI2C::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
perf_print_counter(_sensor_resets);
perf_print_counter(_sensor_zero_resets);
printf("poll interval: %u ticks\n", getMeasureTicks());
_reports->print_info("report queue");
printf("distance: %ucm (0x%04x)\n",
(unsigned)_last_distance, (unsigned)_last_distance);
}
void LidarLitePWM::cycle()
{
measure();
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&LidarLitePWM::cycle_trampoline,
this,
getMeasureTicks());
}
ssize_t LidarLitePWM::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct distance_sensor_s);
struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (getMeasureTicks() > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them.
*/
while (count--) {
if (_reports->get(rbuf)) {
ret += sizeof(*rbuf);
rbuf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
} else {
_reports->flush();
measure();
if (_reports->get(rbuf)) {
ret = sizeof(*rbuf);
}
}
return ret;
}
