fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 16:27:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor ll40ls: use driver base class

Browse Source
This commit is contained in:
Beat Küng
2020-03-02 11:50:25 +01:00
committed by Daniel Agar
parent df99555132
commit 1b1e1ba31f
6 changed files with 78 additions and 228 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rc.sensors
+1 -1
View File
@@ -67,7 +67,7 @@ fi
# Lidar-Lite on I2C
if param compare -s SENS_EN_LL40LS 2
then
ll40ls start i2c -a
ll40ls start -X
fi
# mappydot lidar sensor
boards/intel/aerofc-v1/init/rc.board_sensors
+1 -1
View File
@@ -16,7 +16,7 @@ hmc5883 -X start
ist8310 -I -R 4 start
ll40ls start i2c -a
ll40ls start -X
# Internal SPI (auto detect ms5611 or ms5607)
if ! ms5611 -T 5607 -s start
src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp
+7 -11
View File
@@ -41,9 +41,10 @@
#include "LidarLiteI2C.h"
LidarLiteI2C::LidarLiteI2C(const int bus, const uint8_t rotation, const int address) :
I2C("LL40LS", nullptr, bus, address, 100000),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
LidarLiteI2C::LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
const int address) :
I2C("LL40LS", nullptr, bus, address, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
_px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation)
{
_px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE);
@@ -55,7 +56,6 @@ LidarLiteI2C::LidarLiteI2C(const int bus, const uint8_t rotation, const int addr
LidarLiteI2C::~LidarLiteI2C()
{
stop();
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_sensor_resets);
@@ -74,8 +74,9 @@ LidarLiteI2C::init()
}
void
LidarLiteI2C::print_info()
LidarLiteI2C::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
perf_print_counter(_sensor_resets);
@@ -457,12 +458,7 @@ void LidarLiteI2C::start()
ScheduleNow();
}
void LidarLiteI2C::stop()
{
ScheduleClear();
}
void LidarLiteI2C::Run()
void LidarLiteI2C::RunImpl()
{
/* collection phase? */
if (_collect_phase) {
src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h
+15 -12
View File
@@ -45,6 +45,7 @@
#include <mathlib/mathlib.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
#include <drivers/device/device.h>
#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
#include <perf/perf_counter.h>
@@ -90,13 +91,18 @@ static constexpr uint32_t LL40LS_CONVERSION_INTERVAL{50_ms};
static constexpr uint32_t LL40LS_CONVERSION_TIMEOUT{100_ms};
class LidarLiteI2C : public device::I2C, public px4::ScheduledWorkItem
class LidarLiteI2C : public device::I2C, public I2CSPIDriver<LidarLiteI2C>
{
public:
LidarLiteI2C(const int bus, const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
const int address = LL40LS_BASEADDR);
virtual ~LidarLiteI2C();
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
int init();
/**
@@ -112,14 +118,17 @@ public:
*/
void start();
void print_info();
/**
* Stop the automatic measurement state machine.
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void stop();
void RunImpl();
protected:
void custom_method(const BusCLIArguments &cli) override;
void print_status() override;
uint32_t get_measure_interval() const { return LL40LS_CONVERSION_INTERVAL; };
int measure();
@@ -161,12 +170,6 @@ private:
*/
int probe_address(const uint8_t address);
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void Run() override;
bool _collect_phase{false};
bool _is_v3hp{false};
bool _pause_measurements{false};
src/drivers/distance_sensor/ll40ls/ll40ls.cpp
+53 -203
View File
@@ -38,151 +38,16 @@
* @author Ban Siesta <bansiesta@gmail.com>
* @author James Goppert <james.goppert@gmail.com>
*
* Interface for the PulsedLight Lidar-Lite range finders.
*/
#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_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include "LidarLiteI2C.h"
/**
* @brief Local functions in support of the shell command.
*/
namespace ll40ls
{
LidarLiteI2C *instance = nullptr;
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 status();
int stop();
int usage();
/**
* @brief Prints register information to the console.
*/
int
print_regs()
{
if (!instance) {
PX4_ERR("No ll40ls driver running");
return PX4_ERROR;
}
instance->print_registers();
return PX4_OK;
}
/**
* 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;
}
}
return PX4_ERROR;
}
/**
* Start the driver on a specific bus.
*
* This function only returns if the sensor is up and running
* or could not be detected successfully.
*/
int
start_bus(const int bus, const uint8_t rotation)
{
if (instance != nullptr) {
PX4_ERR("already started");
return PX4_OK;
}
// 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;
}
/**
* @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
*/
int
stop()
{
if (instance != nullptr) {
delete instance;
instance = nullptr;
}
PX4_INFO("driver stopped");
return PX4_OK;
}
/**
* @brief Displays driver usage at the console.
*/
int
usage()
void
LidarLiteI2C::print_usage()
{
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
@@ -193,97 +58,82 @@ 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/en/sensor/lidar_lite.html
### 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_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;
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_COMMAND("regdump");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
} // namespace ll40ls
I2CSPIDriverBase *LidarLiteI2C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
LidarLiteI2C* instance = new LidarLiteI2C(iterator.configuredBusOption(), iterator.bus(), cli.rotation, cli.bus_frequency);
if (instance == nullptr) {
PX4_ERR("alloc failed");
return nullptr;
}
if (instance->init() != PX4_OK) {
delete instance;
return nullptr;
}
instance->start();
return instance;
}
void
LidarLiteI2C::custom_method(const BusCLIArguments &cli)
{
print_registers();
}
/**
* @brief Driver 'main' command.
*/
extern "C" __EXPORT int ll40ls_main(int argc, char *argv[])
{
const char *myoptarg = nullptr;
int ch;
using ThisDriver = LidarLiteI2C;
BusCLIArguments cli{true, false};
cli.default_i2c_frequency = 100000;
int bus = PX4_I2C_BUS_EXPANSION;
int ch = 0;
int myoptind = 1;
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
bool start_i2c_all = false;
while ((ch = px4_getopt(argc, argv, "ab:R:", &myoptind, &myoptarg)) != EOF) {
while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
switch (ch) {
case 'a':
start_i2c_all = true;
break;
case 'b':
bus = atoi(myoptarg);
break;
case 'R':
rotation = (uint8_t)atoi(myoptarg);
PX4_INFO("Setting Lidar orientation to %d", (int)rotation);
cli.rotation = (enum Rotation)atoi(cli.optarg());
break;
default:
return ll40ls::usage();
}
}
if (myoptind >= argc) {
return ll40ls::usage();
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
// Print the sensor register values.
if (!strcmp(argv[myoptind], "print_regs")) {
return ll40ls::print_regs();
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_LL40LS);
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
// Start the driver.
if (!strcmp(argv[myoptind], "start")) {
if (start_i2c_all) {
PX4_INFO("starting all i2c busses");
return ll40ls::start(rotation);
} else {
return ll40ls::start_bus(bus, rotation);
}
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
// Print the driver status.
if (!strcmp(argv[myoptind], "status")) {
return ll40ls::status();
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
// Stop the driver
if (!strcmp(argv[myoptind], "stop")) {
return ll40ls::stop();
if (!strcmp(verb, "regdump")) {
return ThisDriver::module_custom_method(cli, iterator);
}
// Print driver usage information.
return ll40ls::usage();
ThisDriver::print_usage();
return -1;
}
src/drivers/drv_sensor.h
+1
View File
@@ -129,6 +129,7 @@
#define DRV_OSD_DEVTYPE_ATXXXX 0x6b
#define DRV_FLOW_DEVTYPE_PMW3901 0x6c
#define DRV_DIST_DEVTYPE_LL40LS 0x70
#define DRV_DEVTYPE_UNUSED 0xff