int LidarLiteI2C::probe()
{
// cope with both old and new I2C bus address
const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD};
// more retries for detection
_retries = 10;
for (uint8_t i = 0; i < sizeof(addresses); i++) {
/*
check for hw and sw versions. It would be better if
we had a proper WHOAMI register
*/
if (read_reg(LL40LS_HW_VERSION, _hw_version) == OK && _hw_version > 0 &&
read_reg(LL40LS_SW_VERSION, _sw_version) == OK && _sw_version > 0) {
goto ok;
}
DEVICE_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x\n",
(unsigned)_hw_version,
(unsigned)_sw_version);
}
// not found on any address
return -EIO;
ok:
_retries = 3;
return reset_sensor();
}
int
LL40LS::probe()
{
// cope with both old and new I2C bus address
const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD};
// more retries for detection
_retries = 10;
for (uint8_t i=0; i<sizeof(addresses); i++) {
uint8_t who_am_i=0, max_acq_count=0;
// set the I2C bus address
set_address(addresses[i]);
/* register 2 defaults to 0x80. If this matches it is
almost certainly a ll40ls */
if (read_reg(LL40LS_MAX_ACQ_COUNT_REG, max_acq_count) == OK && max_acq_count == 0x80) {
// very likely to be a ll40ls. This is the
// default max acquisition counter
goto ok;
}
if (read_reg(LL40LS_WHO_AM_I_REG, who_am_i) == OK && who_am_i == LL40LS_WHO_AM_I_REG_VAL) {
// it is responding correctly to a
// WHO_AM_I. This works with older sensors (pre-production)
goto ok;
}
debug("probe failed reg11=0x%02x reg2=0x%02x\n",
(unsigned)who_am_i,
(unsigned)max_acq_count);
}
// not found on any address
return -EIO;
ok:
_retries = 3;
// reset the sensor to ensure it is in a known state with
// correct settings
return reset_sensor();
}
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;
}
int LidarLiteI2C::measure()
{
int ret;
if (_pause_measurements) {
// we are in print_registers() and need to avoid
// acquisition to keep the I2C peripheral on the
// sensor active
return OK;
}
/*
* Send the command to begin a measurement.
*/
const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE };
ret = lidar_transfer(cmd, sizeof(cmd), nullptr, 0);
if (OK != ret) {
perf_count(_comms_errors);
DEVICE_DEBUG("i2c::transfer returned %d", ret);
// if we are getting lots of I2C transfer errors try
// resetting the sensor
if (perf_event_count(_comms_errors) % 10 == 0) {
perf_count(_sensor_resets);
reset_sensor();
}
return ret;
}
// remember when we sent the acquire so we can know when the
// acquisition has timed out
_acquire_time_usec = hrt_absolute_time();
ret = OK;
return ret;
}
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;
}
int
LL40LS::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(LL40LS_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
reset_sensor();
return OK;
case RANGEFINDERIOCSETMINIUMDISTANCE: {
set_minimum_distance(*(float *)arg);
return 0;
}
break;
case RANGEFINDERIOCSETMAXIUMDISTANCE: {
set_maximum_distance(*(float *)arg);
return 0;
}
break;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
}
}
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;
}
__interrupt void TIMER0_A0_ISR(void)
{
// Disable IE
TA0CCTL0 &= ~CCIE;
// Reset IRQ flag
TA0CCTL0 &= ~CCIFG;
// Add 1 sec to TACCR0 register (IRQ will be asserted at 0x7FFF and 0xFFFF = 1 sec intervals)
TA0CCR0 += 32768;
// Enable IE
TA0CCTL0 |= CCIE;
// Add 1 second to global time
clock_tick();
// Critical measurements must be done in real time
do_speed_measurement();
do_distance_measurement();
reset_sensor();
//push_speed();
if(sRFsmpl.mode == SIMPLICITI_OFF)
{
// if it is time to connect
if ( sTime.system_time == bike_try_to_connect)
{
simpliciti_bike_flag = SIMPLICITI_BIKE_CONNECT;
bike_communication_timeout = sTime.system_time + 1;
bike_sync_attempt++;
// Increases the time between two attempts. We try to fit inside the listen from the watch
bike_try_to_connect = sTime.system_time + 4 + bike_sync_attempt;
bike_sync_attempt = bike_sync_attempt%3;
}
}
if(last_sent_message_index==2)
{
last_sent_message_index=0;
// the bike didn't receive any message in 20 seconds -> reconnect
sRFsmpl.mode = SIMPLICITI_OFF;
// tries to reconnect in 5 seconds
bike_try_to_connect = sTime.system_time+5;
}
if(sRFsmpl.mode == SIMPLICITI_IDLE)
{
// if we are ready to send
if ( (sTime.system_time - rf_send_time)%SIMPLICITI_BIKE_SEND_INTERVAL == 0)
{
bike_communication_timeout = sTime.system_time +1;
simpliciti_bike_flag = SIMPLICITI_BIKE_TRIGGER_SEND_DATA;
}
}
/*if ( sTime.system_time%10 == 0)
{
bike_communication_timeout = sTime.system_time +1;
if(sRFsmpl.mode == SIMPLICITI_IDLE)
{
simpliciti_bike_flag = SIMPLICITI_BIKE_TRIGGER_SEND_DATA;
}
else if ((sRFsmpl.mode == SIMPLICITI_OFF) && (bike_sync_attempt==0))
{
bike_try_to_connect = sTime.system_time + 1;
}
}*/
if(bike_communication_timeout==sTime.system_time)
{
simpliciti_bike_flag = SIMPLICITI_BIKE_TRIGGER_STOP;
}
// -------------------------------------------------------------------
// Service active modules that require 1/s processing
// To change the menu automatically
if( change_menu >= CHANGE_MENU_PERIOD )
{
display.flag.line2_change = 1;
change_menu=0;
}
else
{
//change_menu++;
}
// Exit from LPM3 on RETI
_BIC_SR_IRQ(LPM3_bits);
}
