Permalink
Switch branches/tags
v2.4 pre-apm2-support cellular-modem-0.5 cellular-modem-0-5 Rudder_elevon Copter-3.5.1-rc1 Copter-3.5.0 Copter-3.5.0-rc11 Copter-3.5.0-rc10 Copter-3.5.0-rc9 Copter-3.5.0-rc8 Copter-3.5.0-rc7 Copter-3.5.0-rc2 Copter-3.5.0-rc1 Copter-3.4.6 Copter-3.4.5 Copter-3.4.4 Copter-3.4.3 Copter-3.4.3-rc1 Copter-3.4.2 Copter-3.4.2-rc2 Copter-3.4.2-rc1 Copter-3.4.1 Copter-3.4.0 Copter-3.3.2 Copter-3.3.1 Copter-3.3-rc7 Copter-3.3-rc5 CanberraUAV-OBC-2012 ArduSub-stable ArduSub-beta ArduPlane-stable ArduPlane-stable-AVR ArduPlane-beta ArduPlane-beta-AVR ArduPlane-3.7.1 ArduPlane-3.7.0 ArduPlane-3.7.0-beta1 ArduPlane-3.6.0 ArduPlane-3.5.3 ArduPlane-3.5.2 ArduPlane-3.5.1 ArduPlane-3.4.0 ArduPlane-3.3.0 ArduPlane-3.2.2 ArduPlane-3.2.1 ArduPlane-3.2.0 ArduPlane-3.1.0 ArduPlane-3.0.3 ArduPlane-3.0.2 ArduPlane-3.0.1 ArduPlane-3.0.0 ArduPlane-2.78 ArduPlane-2.78b ArduPlane-2.75 ArduPlane-2.74 ArduPlane-2.74b ArduPlane-2.73 ArduPlane-2.72 ArduPlane-2.70 ArduPlane-2.69 ArduPlane-2.68 ArduPlane-2.67 ArduPlane-2.66 ArduPlane-2.65 ArduPlane-2.64 ArduPlane-2.63 ArduPlane-2.62 ArduPlane-2.61 ArduPlane-2.60 ArduPlane-2.50 ArduPlane-2.40 ArduPlane-2.40-beta ArduPlane-2.34 ArduPlane-2.33 ArduPlane-2.32 ArduPlane-2.31 ArduPlane-2.30 ArduPlane-2.28 ArduPlane-2.27-Alpha ArduCopter-stable ArduCopter-stable-heli ArduCopter-stable-apm2 ArduCopter-stable-apm1 ArduCopter-beta ArduCopter-beta-heli ArduCopter-beta-apm2 ArduCopter-beta-apm1 ArduCopter-3.5.0-rc6 ArduCopter-3.5.0-rc5 ArduCopter-3.5.0-rc4 ArduCopter-3.5.0-rc3 ArduCopter-3.2.1-apm-px4 ArduCopter-3.2-apm-px4 ArduCopter-3.1.5 ArduCopter-3.1.5-apm ArduCopter-3.1.0 ArduCopter-3.1.0-rc8 ArduCopter-3.1.0-rc7 ArduCopter-3.1.0-rc6
Nothing to show
Find file
Fetching contributors…
Cannot retrieve contributors at this time
205 lines (175 sloc) 6.39 KB
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#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(uint8_t bus, RangeFinder &_ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state,
RangeFinder::RangeFinder_Type _rftype)
: AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_LASER)
, _dev(hal.i2c_mgr->get_device(bus, LL40LS_ADDR))
, rftype(_rftype)
{
}
/*
detect if a PulsedLight rangefinder is connected. We'll detect by
look for the version registers
*/
AP_RangeFinder_Backend *AP_RangeFinder_PulsedLightLRF::detect(uint8_t bus, RangeFinder &_ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state,
RangeFinder::RangeFinder_Type rftype)
{
AP_RangeFinder_PulsedLightLRF *sensor
= new AP_RangeFinder_PulsedLightLRF(bus, _ranger, instance, _state, rftype);
if (!sensor ||
!sensor->init()) {
delete sensor;
return nullptr;
}
return sensor;
}
/*
called at 50Hz
*/
void AP_RangeFinder_PulsedLightLRF::timer(void)
{
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);
}
}
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))) {
uint16_t distance_cm = be16toh(val);
// remove momentary spikes
if (abs(distance_cm - last_distance_cm) < 100) {
state.distance_cm = distance_cm;
update_status();
}
last_distance_cm = distance_cm;
} else {
set_status(RangeFinder::RangeFinder_NoData);
}
if (!v2_hardware) {
// for v2 hw we use continuous mode
phase = PHASE_MEASURE;
}
break;
}
}
}
/*
a table of settings for a lidar
*/
struct settings_table {
uint8_t reg;
uint8_t value;
};
/*
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 },
};
/*
initialise the sensor to required settings
*/
bool AP_RangeFinder_PulsedLightLRF::init(void)
{
if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
_dev->set_retries(3);
// LidarLite needs split transfers
_dev->set_split_transfers(true);
if (rftype == RangeFinder::RangeFinder_TYPE_PLI2CV3) {
v2_hardware = true;
} else {
// auto-detect v1 vs v2
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
goto failed;
}
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++) {
if (!_dev->write_register(table[i].reg, table[i].value, true)) {
goto failed;
}
}
printf("Found LidarLite device=0x%x v2=%d\n", _dev->get_bus_id(), (int)v2_hardware);
_dev->get_semaphore()->give();
_dev->register_periodic_callback(20000,
FUNCTOR_BIND_MEMBER(&AP_RangeFinder_PulsedLightLRF::timer, void));
return true;
failed:
_dev->get_semaphore()->give();
return false;
}