zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_RangeFinder: support LidarLite V2 using in-tree driver 

needs to run in continuous mode
mission-4.1.18
Andrew Tridgell 8 years ago
parent
52854aab3b
commit
0c7dd99d7e
4 changed files with 143 additions and 85 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 184
      libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp
  2. 33
      libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h
  3. 9
      libraries/AP_RangeFinder/RangeFinder.cpp
  4. 2
      libraries/AP_RangeFinder/RangeFinder.h

184
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp
Unescape View File

@ -15,108 +15,168 @@ @@ -15,108 +15,168 @@
#include "AP_RangeFinder_PulsedLightLRF.h"
#include <utility>
#include <stdio.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/sparse-endian.h>
extern const AP_HAL::HAL& hal;
/* LL40LS Registers addresses */
#define LL40LS_MEASURE_REG 0x00 /* Measure range register */
#define LL40LS_DISTHIGH_REG 0x0F /* High byte of distance register, auto increment */
#define LL40LS_COUNT 0x11
#define LL40LS_HW_VERSION 0x41
#define LL40LS_INTERVAL 0x45
#define LL40LS_SW_VERSION 0x4f
// bit values
#define LL40LS_MSRREG_RESET 0x00 /* reset to power on defaults */
#define LL40LS_AUTO_INCREMENT 0x80
#define LL40LS_COUNT_CONTINUOUS 0xff
#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */
// i2c address
#define LL40LS_ADDR 0x62
/*
The constructor also initializes the rangefinder. Note that this
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(RangeFinder &_ranger, uint8_t instance,
AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(uint8_t bus, RangeFinder &_ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state)
: AP_RangeFinder_Backend(_ranger, instance, _state)
, _dev(hal.i2c_mgr->get_device(bus, LL40LS_ADDR))
{
// try external bus first
_dev = hal.i2c_mgr->get_device(1, AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR);
if (!_dev) {
// then internal
_dev = hal.i2c_mgr->get_device(0, AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR);
}
}
/*
detect if a PulsedLight rangefinder is connected. We'll detect by
trying to take a reading on I2C. If we get a result the sensor is
there.
look for the version registers
*/
AP_RangeFinder_Backend *AP_RangeFinder_PulsedLightLRF::detect(RangeFinder &_ranger, uint8_t instance,
AP_RangeFinder_Backend *AP_RangeFinder_PulsedLightLRF::detect(uint8_t bus, RangeFinder &_ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state)
{
AP_RangeFinder_PulsedLightLRF *sensor
= new AP_RangeFinder_PulsedLightLRF(_ranger, instance, _state);
if (!sensor || !sensor->start_reading()) {
= new AP_RangeFinder_PulsedLightLRF(bus, _ranger, instance, _state);
if (!sensor ||
!sensor->init()) {
delete sensor;
return nullptr;
}
// give time for the sensor to process the request
hal.scheduler->delay(50);
uint16_t reading_cm;
if (!sensor->get_reading(reading_cm)) {
delete sensor;
return nullptr;
}
return sensor;
}
// start_reading() - ask sensor to make a range reading
bool AP_RangeFinder_PulsedLightLRF::start_reading()
/*
called at 50Hz
*/
bool AP_RangeFinder_PulsedLightLRF::timer(void)
{
if (!_dev || !_dev->get_semaphore()->take(1)) {
return false;
if (check_reg_counter++ == 10) {
check_reg_counter = 0;
if (!_dev->check_next_register()) {
// re-send the acquire. this handles the case of power
// cycling while running in continuous mode
_dev->write_register(LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE);
}
}
// send command to take reading
bool ret = _dev->write_register(AP_RANGEFINDER_PULSEDLIGHTLRF_MEASURE_REG,
AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE);
_dev->get_semaphore()->give();
return ret;
switch (phase) {
case PHASE_MEASURE:
if (_dev->write_register(LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE)) {
phase = PHASE_COLLECT;
}
break;
case PHASE_COLLECT: {
be16_t val;
// read the high and low byte distance registers
if (_dev->read_registers(LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT, (uint8_t*)&val, sizeof(val))) {
state.distance_cm = be16toh(val);
update_status();
} else {
set_status(RangeFinder::RangeFinder_NoData);
}
if (!v2_hardware) {
// for v2 hw we use continuous mode
phase = PHASE_MEASURE;
}
break;
}
}
return true;
}
// read - return last value measured by sensor
bool AP_RangeFinder_PulsedLightLRF::get_reading(uint16_t &reading_cm)
{
be16_t val;
if (!_dev->get_semaphore()->take(1)) {
return false;
}
/*
a table of settings for a lidar
*/
struct settings_table {
uint8_t reg;
uint8_t value;
};
// read the high and low byte distance registers
bool ret = _dev->read_registers(AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG,
(uint8_t *) &val, sizeof(val));
_dev->get_semaphore()->give();
/*
register setup table for V1 Lidar
*/
static const struct settings_table settings_v1[] = {
{ LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET },
};
/*
register setup table for V2 Lidar
*/
static const struct settings_table settings_v2[] = {
{ LL40LS_INTERVAL, 0x28 }, // 0x28 == 50Hz
{ LL40LS_COUNT, LL40LS_COUNT_CONTINUOUS },
{ LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE },
};
if (!ret) {
/*
initialise the sensor to required settings
*/
bool AP_RangeFinder_PulsedLightLRF::init(void)
{
if (!_dev || !_dev->get_semaphore()->take(0)) {
return false;
}
_dev->set_retries(3);
if (!(_dev->read_registers(LL40LS_HW_VERSION, &hw_version, 1) &&
hw_version > 0 &&
_dev->read_registers(LL40LS_SW_VERSION, &sw_version, 1) &&
sw_version > 0)) {
printf("PulsedLightI2C: bad version 0x%02x 0x%02x\n", (unsigned)hw_version, (unsigned)sw_version);
// invalid version information
_dev->get_semaphore()->give();
return false;
}
_dev->get_semaphore()->give();
// combine results into distance
reading_cm = be16toh(val);
// kick off another reading for next time
// To-Do: replace this with continuous mode
hal.scheduler->delay_microseconds(200);
start_reading();
v2_hardware = (hw_version >= 0x15);
const struct settings_table *table;
uint8_t num_settings;
if (v2_hardware) {
table = settings_v2;
num_settings = sizeof(settings_v2) / sizeof(settings_table);
phase = PHASE_COLLECT;
} else {
table = settings_v1;
num_settings = sizeof(settings_v1) / sizeof(settings_table);
phase = PHASE_MEASURE;
}
_dev->setup_checked_registers(num_settings);
for (uint8_t i = 0; i < num_settings; i++) {
_dev->write_register(table[i].reg, table[i].value, true);
}
_dev->register_periodic_callback(20000,
FUNCTOR_BIND_MEMBER(&AP_RangeFinder_PulsedLightLRF::timer, bool));
return true;
}
/*
update the state of the sensor
*/
void AP_RangeFinder_PulsedLightLRF::update(void)
{
if (get_reading(state.distance_cm)) {
// update range_valid state based on distance measured
update_status();
} else {
set_status(RangeFinder::RangeFinder_NoData);
}
}

33
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h
Unescape View File

@ -14,39 +14,34 @@ @@ -14,39 +14,34 @@
* ------------------------------------------------------------------------------------
*/
// i2c address
#define AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR 0x62
// min and max distances
#define AP_RANGEFINDER_PULSEDLIGHTLRF_MIN_DISTANCE 0
#define AP_RANGEFINDER_PULSEDLIGHTLRF_MAX_DISTANCE 1400
// registers
#define AP_RANGEFINDER_PULSEDLIGHTLRF_MEASURE_REG 0x00
#define AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG 0x8f // high byte of distance measurement
// command register values
#define AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE 0x04 // Varies based on sensor revision, 0x04 is newest, 0x61 is older
class AP_RangeFinder_PulsedLightLRF : public AP_RangeFinder_Backend
{
public:
// static detection function
static AP_RangeFinder_Backend *detect(RangeFinder &ranger, uint8_t instance,
static AP_RangeFinder_Backend *detect(uint8_t bus, RangeFinder &ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state);
// update state
void update(void);
void update(void) override {}
private:
// constructor
AP_RangeFinder_PulsedLightLRF(RangeFinder &ranger, uint8_t instance,
AP_RangeFinder_PulsedLightLRF(uint8_t bus, RangeFinder &ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state);
// start a reading
bool start_reading(void);
bool get_reading(uint16_t &reading_cm);
bool init(void);
bool timer(void);
bool lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len);
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
uint8_t sw_version;
uint8_t hw_version;
uint8_t check_reg_counter;
bool v2_hardware;
enum { PHASE_MEASURE, PHASE_COLLECT } phase;
};

9
libraries/AP_RangeFinder/RangeFinder.cpp
Unescape View File

@ -565,16 +565,17 @@ void RangeFinder::update(void) @@ -565,16 +565,17 @@ void RangeFinder::update(void)
}
}
void RangeFinder::_add_backend(AP_RangeFinder_Backend *backend)
bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend)
{
if (!backend) {
return;
return false;
}
if (num_instances == RANGEFINDER_MAX_INSTANCES) {
AP_HAL::panic("Too many RANGERS backends");
}
drivers[num_instances++] = backend;
return true;
}
/*
@ -584,7 +585,9 @@ void RangeFinder::detect_instance(uint8_t instance) @@ -584,7 +585,9 @@ void RangeFinder::detect_instance(uint8_t instance)
{
uint8_t type = _type[instance];
if (type == RangeFinder_TYPE_PLI2C) {
_add_backend(AP_RangeFinder_PulsedLightLRF::detect(*this, instance, state[instance]));
if (!_add_backend(AP_RangeFinder_PulsedLightLRF::detect(0, *this, instance, state[instance]))) {
_add_backend(AP_RangeFinder_PulsedLightLRF::detect(1, *this, instance, state[instance]));
}
}
if (type == RangeFinder_TYPE_MBI2C) {
_add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(*this, instance, state[instance]));

2
libraries/AP_RangeFinder/RangeFinder.h
Unescape View File

@ -206,5 +206,5 @@ private: @@ -206,5 +206,5 @@ private:
void update_instance(uint8_t instance);
void update_pre_arm_check(uint8_t instance);
void _add_backend(AP_RangeFinder_Backend *driver);
bool _add_backend(AP_RangeFinder_Backend *driver);
};

Write Preview
Loading…
Cancel
Save