dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Refactor the ll40ls namespace driver methods to more closely match the cm8jl65, mappydot, leddar_one, and other distance sensor driver implementations.

This commit is contained in:
mcsauder 2019-08-06 11:06:48 -06:00 committed by Nuno Marques
parent 8ce4a15778
commit 7b16c3482d
13 changed files with 364 additions and 324 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d/rc.sensors
View File

@ -64,7 +64,7 @@ fi
# Lidar-Lite on I2C
if param compare SENS_EN_LL40LS 2
then
ll40ls start i2c
ll40ls start i2c -a
fi
# mappydot lidar sensor

2
boards/intel/aerofc-v1/init/rc.board_sensors
View File

@ -13,7 +13,7 @@ hmc5883 -X start
ist8310 -C -b 1 -R 4 start
aerofc_adc start
ll40ls start i2c
ll40ls start i2c -a
# Internal SPI (auto detect ms5611 or ms5607)
ms5611 -T 0 -s start

2
src/drivers/distance_sensor/ll40ls/LidarLite.cpp
View File

@ -41,7 +41,7 @@
#include "LidarLite.h"
LidarLite::LidarLite(uint8_t rotation) :
LidarLite::LidarLite(const uint8_t rotation) :
_px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation)
{
_px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE);

20
src/drivers/distance_sensor/ll40ls/LidarLite.h
View File

@ -44,22 +44,24 @@
#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
#include <perf/perf_counter.h>
using namespace time_literals;
/* Device limits */
#define LL40LS_MIN_DISTANCE (0.05f)
#define LL40LS_MAX_DISTANCE (25.00f)
#define LL40LS_MIN_DISTANCE (0.05f)
#define LL40LS_MAX_DISTANCE (25.00f)
#define LL40LS_MAX_DISTANCE_V2 (35.00f)
// normal conversion wait time
#define LL40LS_CONVERSION_INTERVAL 50*1000UL /* 50ms */
// Normal conversion wait time.
#define LL40LS_CONVERSION_INTERVAL 50_ms
// maximum time to wait for a conversion to complete.
#define LL40LS_CONVERSION_TIMEOUT 100*1000UL /* 100ms */
// Maximum time to wait for a conversion to complete.
#define LL40LS_CONVERSION_TIMEOUT 100_ms
class LidarLite
{
public:
LidarLite(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
LidarLite(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
virtual ~LidarLite();
virtual int init() = 0;
@ -72,13 +74,13 @@ public:
void print_info();
/**
* @brief print registers to console
* @brief print registers to console.
*/
virtual void print_registers() {};
protected:
uint32_t get_measure_interval() const { return _measure_interval; }
uint32_t get_measure_interval() const { return _measure_interval; };
virtual int collect() = 0;

77
src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp
View File

@ -41,11 +41,7 @@
#include "LidarLiteI2C.h"
#include <px4_defines.h>
#include <mathlib/mathlib.h>
#include <drivers/drv_hrt.h>
LidarLiteI2C::LidarLiteI2C(int bus, uint8_t rotation, int address) :
LidarLiteI2C::LidarLiteI2C(const int bus, const uint8_t rotation, const int address) :
LidarLite(rotation),
I2C("LL40LS", nullptr, bus, address, 100000),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id()))
@ -62,45 +58,34 @@ LidarLiteI2C::~LidarLiteI2C()
int
LidarLiteI2C::init()
{
int ret = PX4_ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
return ret;
// Perform I2C init (and probe) first.
if (I2C::init() != PX4_OK) {
return PX4_ERROR;
}
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
start();
return OK;
return PX4_OK;
}
int
LidarLiteI2C::read_reg(uint8_t reg, uint8_t &val)
LidarLiteI2C::read_reg(const uint8_t reg, uint8_t &val)
{
return lidar_transfer(&reg, 1, &val, 1);
}
int
LidarLiteI2C::write_reg(uint8_t reg, uint8_t val)
LidarLiteI2C::write_reg(const uint8_t reg, const uint8_t &val)
{
const uint8_t cmd[2] = { reg, val };
return transfer(&cmd[0], 2, nullptr, 0);
}
/*
LidarLite specific transfer() function that avoids a stop condition
with SCL low
*/
int
LidarLiteI2C::lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
LidarLiteI2C::lidar_transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len)
{
if (send != nullptr && send_len > 0) {
int ret = transfer(send, send_len, nullptr, 0);
if (ret != OK) {
if (ret != PX4_OK) {
return ret;
}
}
@ -109,7 +94,7 @@ LidarLiteI2C::lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *re
return transfer(nullptr, 0, recv, recv_len);
}
return OK;
return PX4_ERROR;
}
int
@ -135,11 +120,6 @@ LidarLiteI2C::probe()
* v1 and v3 don't have the unit id register while v2 has both.
* It would be better if we had a proper WHOAMI register.
*/
/**
* check for hw and sw versions for v1, v2 and v3
*/
if ((read_reg(LL40LS_HW_VERSION, _hw_version) == OK) &&
(read_reg(LL40LS_SW_VERSION, _sw_version) == OK)) {
@ -164,7 +144,7 @@ LidarLiteI2C::probe()
if (_unit_id > 0) {
// v3hp
_is_V3hp = true;
_is_v3hp = true;
PX4_INFO("probe success - id: %u", _unit_id);
}
}
@ -194,12 +174,10 @@ LidarLiteI2C::measure()
return OK;
}
/*
* Send the command to begin a measurement.
*/
// Send the command to begin a measurement.
int ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE);
if (OK != ret) {
if (ret != PX4_OK) {
perf_count(_comms_errors);
PX4_DEBUG("i2c::transfer returned %d", ret);
@ -220,9 +198,6 @@ LidarLiteI2C::measure()
return OK;
}
/*
reset the sensor to power on defaults plus additional configurations
*/
int
LidarLiteI2C::reset_sensor()
{
@ -267,10 +242,8 @@ LidarLiteI2C::reset_sensor()
return OK;
}
/*
dump sensor registers for debugging
*/
void LidarLiteI2C::print_registers()
void
LidarLiteI2C::print_registers()
{
_pause_measurements = true;
PX4_INFO("registers");
@ -297,9 +270,10 @@ void LidarLiteI2C::print_registers()
_pause_measurements = false;
}
int LidarLiteI2C::collect()
int
LidarLiteI2C::collect()
{
/* read from the sensor */
// read from the sensor
uint8_t val[2] {};
perf_begin(_sample_perf);
@ -359,8 +333,8 @@ int LidarLiteI2C::collect()
// this should be fairly close to the end of the measurement, so the best approximation of the time
const hrt_abstime timestamp_sample = hrt_absolute_time();
/* Relative signal strength measurement, i.e. the strength of
* the main signal peak compared to the general noise level*/
// Relative signal strength measurement, i.e. the strength of
// the main signal peak compared to the general noise level.
uint8_t signal_strength_reg = LL40LS_SIGNAL_STRENGTH_REG;
ret = lidar_transfer(&signal_strength_reg, 1, &val[0], 1);
@ -396,13 +370,13 @@ int LidarLiteI2C::collect()
uint8_t signal_value = 0;
// We detect if V3HP is being used
if (_is_V3hp) {
if (_is_v3hp) {
signal_min = LL40LS_SIGNAL_STRENGTH_MIN_V3HP;
signal_max = LL40LS_SIGNAL_STRENGTH_MAX_V3HP;
signal_value = ll40ls_signal_strength;
} else {
/* Absolute peak strength measurement, i.e. absolute strength of main signal peak*/
// Absolute peak strength measurement, i.e. absolute strength of main signal peak.
uint8_t peak_strength_reg = LL40LS_PEAK_STRENGTH_REG;
ret = lidar_transfer(&peak_strength_reg, 1, &val[0], 1);
@ -442,10 +416,9 @@ int LidarLiteI2C::collect()
} else {
signal_value = ll40ls_peak_strength;
}
}
/* Final data quality evaluation. This is based on the datasheet and simple heuristics retrieved from experiments*/
// Final data quality evaluation. This is based on the datasheet and simple heuristics retrieved from experiments
// Step 1: Normalize signal strength to 0...100 percent using the absolute signal peak strength.
uint8_t signal_quality = 100 * math::max(signal_value - signal_min, 0) / (signal_max - signal_min);
@ -462,10 +435,10 @@ int LidarLiteI2C::collect()
void LidarLiteI2C::start()
{
/* reset the report ring and state machine */
// reset the report ring and state machine
_collect_phase = false;
/* schedule a cycle to start things */
// schedule a cycle to start things
ScheduleNow();
}

163
src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h
View File

@ -42,114 +42,127 @@
#include "LidarLite.h"
#include <drivers/device/i2c.h>
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>
#include <px4_defines.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <drivers/device/i2c.h>
/* Configuration Constants */
static constexpr uint8_t LL40LS_BASEADDR = 0x62; /* 7-bit address */
static constexpr uint8_t LL40LS_BASEADDR_OLD = 0x42; /* previous 7-bit address */
static constexpr uint8_t LL40LS_SIG_COUNT_VAL_DEFAULT = 0x80; /* Default maximum acquisition count */
static constexpr uint8_t LL40LS_BASEADDR = 0x62; /* 7-bit address */
static constexpr uint8_t LL40LS_BASEADDR_OLD = 0x42; /* previous 7-bit address */
static constexpr uint8_t LL40LS_SIG_COUNT_VAL_DEFAULT = 0x80; /* Default maximum acquisition count */
/* LL40LS Registers addresses */
static constexpr uint8_t LL40LS_MEASURE_REG = 0x00; /* Measure range register */
static constexpr uint8_t LL40LS_MSRREG_RESET = 0x00; /* reset to power on defaults */
static constexpr uint8_t LL40LS_MSRREG_ACQUIRE =
0x04; /* Value to initiate a measurement, varies based on sensor revision */
static constexpr uint8_t LL40LS_DISTHIGH_REG = 0x0F; /* High byte of distance register, auto increment */
static constexpr uint8_t LL40LS_AUTO_INCREMENT = 0x80;
static constexpr uint8_t LL40LS_HW_VERSION = 0x41;
static constexpr uint8_t LL40LS_SW_VERSION = 0x4f;
static constexpr uint8_t LL40LS_SIGNAL_STRENGTH_REG = 0x0e;
static constexpr uint8_t LL40LS_PEAK_STRENGTH_REG = 0x0c;
static constexpr uint8_t LL40LS_UNIT_ID_HIGH = 0x16;
static constexpr uint8_t LL40LS_UNIT_ID_LOW = 0x17;
static constexpr uint8_t LL40LS_MEASURE_REG = 0x00; /* Measure range register */
static constexpr uint8_t LL40LS_MSRREG_RESET = 0x00; /* reset to power on defaults */
static constexpr uint8_t LL40LS_MSRREG_ACQUIRE =
0x04; /* Value to initiate a measurement, varies based on sensor revision */
static constexpr uint8_t LL40LS_DISTHIGH_REG = 0x0F; /* High byte of distance register, auto increment */
static constexpr uint8_t LL40LS_AUTO_INCREMENT = 0x80;
static constexpr uint8_t LL40LS_HW_VERSION = 0x41;
static constexpr uint8_t LL40LS_SW_VERSION = 0x4f;
static constexpr uint8_t LL40LS_SIGNAL_STRENGTH_REG = 0x0e;
static constexpr uint8_t LL40LS_PEAK_STRENGTH_REG = 0x0c;
static constexpr uint8_t LL40LS_UNIT_ID_HIGH = 0x16;
static constexpr uint8_t LL40LS_UNIT_ID_LOW = 0x17;
static constexpr uint8_t LL40LS_SIG_COUNT_VAL_REG = 0x02; /* Maximum acquisition count register */
static constexpr uint8_t LL40LS_SIG_COUNT_VAL_MAX = 0xFF; /* Maximum acquisition count max value */
static constexpr int LL40LS_SIGNAL_STRENGTH_MIN_V3HP = 70; // Min signal strength for V3HP
static constexpr int LL40LS_SIGNAL_STRENGTH_MAX_V3HP = 255; // Max signal strength for V3HP
static constexpr uint8_t LL40LS_SIG_COUNT_VAL_REG = 0x02; /* Maximum acquisition count register */
static constexpr uint8_t LL40LS_SIG_COUNT_VAL_MAX = 0xFF; /* Maximum acquisition count max value */
static constexpr int LL40LS_SIGNAL_STRENGTH_LOW =
24; // Minimum (relative) signal strength value for accepting a measurement
static constexpr int LL40LS_PEAK_STRENGTH_LOW = 135; // Minimum peak strength raw value for accepting a measurement
static constexpr int LL40LS_PEAK_STRENGTH_HIGH = 234; // Max peak strength raw value
static constexpr int LL40LS_SIGNAL_STRENGTH_MIN_V3HP = 70; /* Min signal strength for V3HP */
static constexpr int LL40LS_SIGNAL_STRENGTH_MAX_V3HP = 255; /* Max signal strength for V3HP */
static constexpr int LL40LS_SIGNAL_STRENGTH_LOW =
24; /* Minimum (relative) signal strength value for accepting a measurement */
static constexpr int LL40LS_PEAK_STRENGTH_LOW =
135; /* Minimum peak strength raw value for accepting a measurement */
static constexpr int LL40LS_PEAK_STRENGTH_HIGH = 234; /* Max peak strength raw value */
class LidarLiteI2C : public LidarLite, public device::I2C, public px4::ScheduledWorkItem
{
public:
LidarLiteI2C(int bus, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int address = LL40LS_BASEADDR);
LidarLiteI2C(const int bus, const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
const int address = LL40LS_BASEADDR);
virtual ~LidarLiteI2C();
int init() override;
int init() override;
void print_registers() override;
/**
* Print sensor registers to console for debugging.
*/
void print_registers() override;
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start() override;
/**
* Stop the automatic measurement state machine.
*/
void stop() override;
protected:
int probe() override;
int read_reg(uint8_t reg, uint8_t &val);
int write_reg(uint8_t reg, uint8_t val);
int measure() override;
int reset_sensor() override;
int measure() override;
/**
* Reset the sensor to power on defaults plus additional configurations.
*/
int reset_sensor() override;
int probe() override;
int read_reg(const uint8_t reg, uint8_t &val);
int write_reg(const uint8_t reg, const uint8_t &val);
private:
int collect() override;
/**
* LidarLite specific transfer function. This is needed
* to avoid a stop transition with SCL high
*
* @param send Pointer to bytes to send.
* @param send_len Number of bytes to send.
* @param recv Pointer to buffer for bytes received.
* @param recv_len Number of bytes to receive.
* @return OK if the transfer was successful, -errno
* otherwise.
* @param send Pointer to bytes to send.
* @param send_len Number of bytes to send.
* @param recv Pointer to buffer for bytes received.
* @param recv_len Number of bytes to receive.
* @return OK if the transfer was successful, -errno
* otherwise.
*/
int lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len);
int lidar_transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len);
/**
* Test whether the device supported by the driver is present at a
* specific address.
*
* @param address The I2C bus address to probe.
* @return True if the device is present.
*/
int probe_address(uint8_t address);
* Test whether the device supported by the driver is present at a
* specific address.
*
* @param address The I2C bus address to probe.
* @return True if the device is present.
*/
int probe_address(const uint8_t address);
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start() override;
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void Run() override;
/**
* Stop the automatic measurement state machine.
*/
void stop() override;
bool _collect_phase{false};
bool _is_v3hp{false};
bool _pause_measurements{false};
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void Run() override;
int collect() override;
uint8_t _hw_version{0};
uint8_t _sw_version{0};
uint16_t _unit_id{0};
uint16_t _zero_counter{0};
bool _sensor_ok{false};
bool _collect_phase{false};
uint16_t _zero_counter{0};
uint64_t _acquire_time_usec{0};
bool _pause_measurements{false};
uint8_t _hw_version{0};
uint8_t _sw_version{0};
uint16_t _unit_id{0};
bool _is_V3hp{false};
uint64_t _acquire_time_usec{0};
};

7
src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp
View File

@ -43,13 +43,8 @@
*/
#include "LidarLitePWM.h"
#include <stdio.h>
#include <string.h>
#include <px4_defines.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_input.h>
LidarLitePWM::LidarLitePWM(uint8_t rotation) :
LidarLitePWM::LidarLitePWM(const uint8_t rotation) :
LidarLite(rotation),
ScheduledWorkItem(px4::wq_configurations::hp_default)
{

12
src/drivers/distance_sensor/ll40ls/LidarLitePWM.h
View File

@ -45,13 +45,19 @@
#include "LidarLite.h"
#include <stdio.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_input.h>
#include <px4_defines.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/topics/pwm_input.h>
class LidarLitePWM : public LidarLite, public px4::ScheduledWorkItem
{
public:
LidarLitePWM(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
LidarLitePWM(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
virtual ~LidarLitePWM();
int init() override;
@ -62,12 +68,12 @@ public:
protected:
int measure() override;
int collect() override;
int measure() override;
private:
int _pwmSub{-1};
pwm_input_s _pwm{};
pwm_input_s _pwm{};
};

387
src/drivers/distance_sensor/ll40ls/ll40ls.cpp
View File

@ -41,45 +41,19 @@
* Interface for the PulsedLight Lidar-Lite range finders.
*/
#include "LidarLiteI2C.h"
#include "LidarLitePWM.h"
#include <board_config.h>
#include <systemlib/err.h>
#include <fcntl.h>
#include <cstdlib>
#include <fcntl.h>
#include <string.h>
#include <stdio.h>
#include <systemlib/err.h>
#include <board_config.h>
#include <drivers/device/i2c.h>
#include <px4_getopt.h>
#include <px4_module.h>
enum LL40LS_BUS {
LL40LS_BUS_I2C_ALL = 0,
LL40LS_BUS_I2C_INTERNAL,
LL40LS_BUS_I2C_EXTERNAL,
LL40LS_BUS_PWM
};
static constexpr struct ll40ls_bus_option {
enum LL40LS_BUS busid;
uint8_t busnum;
} bus_options[] = {
#ifdef PX4_I2C_BUS_EXPANSION
{ LL40LS_BUS_I2C_EXTERNAL, PX4_I2C_BUS_EXPANSION },
#endif
#ifdef PX4_I2C_BUS_EXPANSION1
{ LL40LS_BUS_I2C_EXTERNAL, PX4_I2C_BUS_EXPANSION1 },
#endif
#ifdef PX4_I2C_BUS_EXPANSION2
{ LL40LS_BUS_I2C_EXTERNAL, PX4_I2C_BUS_EXPANSION2 },
#endif
#ifdef PX4_I2C_BUS_ONBOARD
{ LL40LS_BUS_I2C_INTERNAL, PX4_I2C_BUS_ONBOARD },
#endif
};
/**
* @brief Driver 'main' command.
*/
extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]);
#include "LidarLiteI2C.h"
#include "LidarLitePWM.h"
/**
@ -89,159 +63,227 @@ namespace ll40ls
{
LidarLite *instance = nullptr;
int start_bus(const struct ll40ls_bus_option &i2c_bus, uint8_t rotation);
void start(enum LL40LS_BUS busid, uint8_t rotation);
void stop();
void info();
void regdump();
void usage();
int print_regs();
int start(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
int start_bus(const int bus = PX4_I2C_BUS_EXPANSION,
const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
int start_pwm(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
int status();
int stop();
int usage();
/**
* @brief Starts the driver.
* @brief Prints register information to the console.
*/
void start(enum LL40LS_BUS busid, uint8_t rotation)
int
print_regs()
{
if (instance) {
PX4_INFO("driver already started");
return;
if (!instance) {
PX4_ERR("No ll40ls driver running");
return PX4_ERROR;
}
if (busid == LL40LS_BUS_PWM) {
instance = new LidarLitePWM(rotation);
instance->print_registers();
return PX4_OK;
}
if (!instance) {
PX4_ERR("Failed to instantiate LidarLitePWM");
return;
}
if (instance->init() != PX4_OK) {
PX4_ERR("failed to initialize LidarLitePWM");
stop();
return;
}
} else {
for (uint8_t i = 0; i < (sizeof(bus_options) / sizeof(bus_options[0])); i++) {
if (busid != LL40LS_BUS_I2C_ALL && busid != bus_options[i].busid) {
continue;
}
if (start_bus(bus_options[i], rotation) == PX4_OK) {
return;
}
/**
* Attempt to start driver on all available I2C busses.
*
* This function will return as soon as the first sensor
* is detected on one of the available busses or if no
* sensors are detected.
*/
int
start(const uint8_t rotation)
{
if (instance != nullptr) {
PX4_ERR("already started");
return PX4_ERROR;
}
for (size_t i = 0; i < NUM_I2C_BUS_OPTIONS; i++) {
if (start_bus(i2c_bus_options[i], rotation) == PX4_OK) {
return PX4_OK;
}
}
if (instance == nullptr) {
PX4_WARN("No LidarLite found");
return;
}
return PX4_ERROR;
}
/**
* Start the driver on a specific bus.
*
* This function call only returns once the driver is up and running
* or failed to detect the sensor.
* This function only returns if the sensor is up and running
* or could not be detected successfully.
*/
int
start_bus(const struct ll40ls_bus_option &i2c_bus, uint8_t rotation)
start_bus(const int bus, const uint8_t rotation)
{
instance = new LidarLiteI2C(i2c_bus.busnum, rotation);
if (instance != nullptr) {
PX4_ERR("already started");
return PX4_OK;
}
if (instance->init() != OK) {
stop();
PX4_INFO("LidarLiteI2C - no device on bus %u", (unsigned)i2c_bus.busid);
// Instantiate the driver.
instance = new LidarLiteI2C(bus, rotation);
if (instance == nullptr) {
PX4_ERR("Failed to instantiate the driver");
delete instance;
return PX4_ERROR;
}
// Initialize the sensor.
if (instance->init() != PX4_OK) {
PX4_ERR("Failed to initialize LidarLite on bus = %u", bus);
delete instance;
instance = nullptr;
return PX4_ERROR;
}
// Start the driver.
instance->start();
PX4_INFO("driver started");
return PX4_OK;
}
/**
* Start the pwm driver.
*
* This function only returns if the sensor is up and running
* or could not be detected successfully.
*/
int
start_pwm(const uint8_t rotation)
{
if (instance != nullptr) {
PX4_ERR("already started");
return PX4_OK;
}
instance = new LidarLitePWM(rotation);
if (instance == nullptr) {
PX4_ERR("Failed to instantiate the driver");
delete instance;
return PX4_ERROR;
}
// Initialize the sensor.
if (instance->init() != PX4_OK) {
PX4_ERR("Failed to initialize LidarLite pwm.");
delete instance;
instance = nullptr;
return PX4_ERROR;
}
// Start the driver.
instance->start();
PX4_INFO("driver started");
return PX4_OK;
}
/**
* @brief Prints status info about the driver.
*/
int
status()
{
if (instance == nullptr) {
PX4_ERR("driver not running");
return PX4_ERROR;
}
instance->print_info();
return PX4_OK;
}
/**
* @brief Stops the driver
*/
void stop()
int
stop()
{
if (instance != nullptr) {
delete instance;
instance = nullptr;
} else {
PX4_ERR("driver not running");
}
}
/**
* @brief Prints status info about the driver.
*/
void
info()
{
if (!instance) {
warnx("No ll40ls driver running");
return;
}
printf("state @ %p\n", instance);
instance->print_info();
}
/**
* @brief Dumps the register information.
*/
void
regdump()
{
if (!instance) {
warnx("No ll40ls driver running");
return;
}
printf("regdump @ %p\n", instance);
instance->print_registers();
PX4_INFO("driver stopped");
return PX4_OK;
}
/**
* @brief Displays driver usage at the console.
*/
void
int
usage()
{
PX4_INFO("missing command: try 'start', 'stop', 'info', 'info' or 'regdump' [i2c|pwm]");
PX4_INFO("options for I2C:");
PX4_INFO(" -X only external bus");
#ifdef PX4_I2C_BUS_ONBOARD
PX4_INFO(" -I only internal bus");
#endif
PX4_INFO("E.g. ll40ls start i2c -R 0");
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
I2C bus driver for LidarLite rangefinders.
The sensor/driver must be enabled using the parameter SENS_EN_LL40LS.
Setup/usage information: https://docs.px4.io/v1.9.0/en/sensor/lidar_lite.htmls
### Examples
Start driver on any bus (start on bus where first sensor found).
$ ll40ls start i2c -a
Start driver on specified bus
$ ll40ls start i2c -b 1
Stop driver
$ ll40ls stop
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("ll40ls", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
PRINT_MODULE_USAGE_COMMAND_DESCR("print_regs","Print the register values");
PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("pwm", "PWM device");
PRINT_MODULE_USAGE_COMMAND_DESCR("i2c", "I2C device");
PRINT_MODULE_USAGE_PARAM_FLAG('a', "Attempt to start driver on all I2C buses (first one found)", true);
PRINT_MODULE_USAGE_PARAM_INT('b', 1, 1, 2000, "Start driver on specific I2C bus", true);
PRINT_MODULE_USAGE_PARAM_INT('R', 25, 1, 25, "Sensor rotation - downward facing by default", true);
PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver status information");
PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
return PX4_OK;
}
} // namespace ll40ls
int
ll40ls_main(int argc, char *argv[])
/**
* @brief Driver 'main' command.
*/
extern "C" __EXPORT int ll40ls_main(int argc, char *argv[])
{
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
enum LL40LS_BUS busid = LL40LS_BUS_I2C_ALL;
int bus = PX4_I2C_BUS_EXPANSION;
int ch = 0;
int myoptind = 1;
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
while ((ch = px4_getopt(argc, argv, "IXR:", &myoptind, &myoptarg)) != EOF) {
bool start_i2c_all = false;
bool start_pwm = false;
while ((ch = px4_getopt(argc, argv, "ab:R", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
#ifdef PX4_I2C_BUS_ONBOARD
case 'I':
busid = LL40LS_BUS_I2C_INTERNAL;
case 'a':
start_i2c_all = true;
break;
#endif
case 'X':
busid = LL40LS_BUS_I2C_EXTERNAL;
case 'b':
bus = atoi(myoptarg);
break;
case 'R':
@ -250,52 +292,61 @@ ll40ls_main(int argc, char *argv[])
break;
default:
ll40ls::usage();
return 0;
return ll40ls::usage();
}
}
/* Determine protocol first because it's needed next. */
// Determine protocol.
if (argc > myoptind + 1) {
const char *protocol = argv[myoptind + 1];
if (!strcmp(protocol, "pwm")) {
busid = LL40LS_BUS_PWM;;
if (!strcmp(protocol, "i2c")) {
PX4_INFO("protocol %s", protocol);
} else if (!strcmp(protocol, "i2c")) {
// Do nothing
} else if (!strcmp(protocol, "pwm")) {
PX4_INFO("protocol %s", protocol);
start_pwm = true;
} else {
warnx("unknown protocol, choose pwm or i2c");
ll40ls::usage();
return 0;
PX4_INFO("unknown protocol, choose pwm or i2c");
return ll40ls::usage();
}
}
/* Now determine action. */
if (argc > myoptind) {
const char *verb = argv[myoptind];
if (!strcmp(verb, "start")) {
ll40ls::start(busid, rotation);
} else if (!strcmp(verb, "stop")) {
ll40ls::stop();
} else if (!strcmp(verb, "regdump")) {
ll40ls::regdump();
} else if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
ll40ls::info();
} else {
ll40ls::usage();
}
return 0;
if (myoptind >= argc) {
return ll40ls::usage();
}
warnx("unrecognized command, try 'start', 'info' or 'regdump'");
ll40ls::usage();
return 0;
// Print the sensor register values.
if (!strcmp(argv[myoptind], "print_regs")) {
return ll40ls::print_regs();
}
// Start the driver.
if (!strcmp(argv[myoptind], "start")) {
if (start_i2c_all) {
PX4_INFO("starting all i2c busses");
return ll40ls::start(rotation);
} else if (start_pwm) {
PX4_INFO("starting pwm");
return ll40ls::start_pwm(rotation);
} else {
return ll40ls::start_bus(bus, rotation);
}
}
// Print the driver status.
if (!strcmp(argv[myoptind], "status")) {
return ll40ls::status();
}
// Stop the driver
if (!strcmp(argv[myoptind], "stop")) {
return ll40ls::stop();
}
// Print driver usage information.
return ll40ls::usage();
}

4
src/lib/drivers/device/nuttx/I2C.cpp
View File

@ -51,7 +51,7 @@ namespace device
unsigned int I2C::_bus_clocks[BOARD_NUMBER_I2C_BUSES] = BOARD_I2C_BUS_CLOCK_INIT;
I2C::I2C(const char *name, const char *devname, int bus, uint16_t address, uint32_t frequency) :
I2C::I2C(const char *name, const char *devname, const int bus, const uint16_t address, const uint32_t frequency) :
CDev(name, devname),
_frequency(frequency)
{
@ -167,7 +167,7 @@ out:
}
int
I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len)
{
px4_i2c_msg_t msgv[2];
unsigned msgs;

4
src/lib/drivers/device/nuttx/I2C.hpp
View File

@ -81,7 +81,7 @@ protected:
* @param address I2C bus address, or zero if set_address will be used
* @param frequency I2C bus frequency for the device (currently not used)
*/
I2C(const char *name, const char *devname, int bus, uint16_t address, uint32_t frequency);
I2C(const char *name, const char *devname, const int bus, const uint16_t address, const uint32_t frequency);
virtual ~I2C();
/**
@ -101,7 +101,7 @@ protected:
* @return OK if the transfer was successful, -errno
* otherwise.
*/
int transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len);
int transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len);
bool external() { return px4_i2c_bus_external(_device_id.devid_s.bus); }

4
src/lib/drivers/device/posix/I2C.cpp
View File

@ -59,7 +59,7 @@ static constexpr const int simulate = PX4_SIMULATE_I2C;
namespace device
{
I2C::I2C(const char *name, const char *devname, int bus, uint16_t address, uint32_t frequency) :
I2C::I2C(const char *name, const char *devname, const int bus, const uint16_t address, const uint32_t frequency) :
CDev(name, devname)
{
DEVICE_DEBUG("I2C::I2C name = %s devname = %s", name, devname);
@ -119,7 +119,7 @@ I2C::init()
}
int
I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len)
{
#ifndef __PX4_LINUX
return PX4_ERROR;

4
src/lib/drivers/device/posix/I2C.hpp
View File

@ -78,7 +78,7 @@ protected:
* @param address I2C bus address, or zero if set_address will be used
* @param frequency I2C bus frequency for the device (currently not used)
*/
I2C(const char *name, const char *devname, int bus, uint16_t address, uint32_t frequency = 0);
I2C(const char *name, const char *devname, const int bus, const uint16_t address, const uint32_t frequency = 0);
virtual ~I2C();
/**
@ -98,7 +98,7 @@ protected:
* @return OK if the transfer was successful, -errno
* otherwise.
*/
int transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len);
int transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len);
bool external() { return px4_i2c_bus_external(_device_id.devid_s.bus); }