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; }