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