fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor teraranger: use driver base class

Browse Source
This commit is contained in:
Beat Küng 2020-03-02 15:03:57 +01:00 committed by Daniel Agar
parent 5a26f06f8a
commit 83b367f000
5 changed files with 65 additions and 148 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

@ -103,7 +103,7 @@ fi
# Teraranger one tof sensor
if param greater -s SENS_EN_TRANGER 0
then
teraranger start -a
teraranger start -X
fi
# Possible external pmw3901 optical flow sensor

20
src/drivers/distance_sensor/teraranger/TERARANGER.cpp
View File

@ -73,9 +73,9 @@ static uint8_t crc8(uint8_t *p, uint8_t len)
return crc & 0xFF;
}
TERARANGER::TERARANGER(const int bus, const int address, const uint8_t rotation) :
I2C("TERARANGER", nullptr, bus, address, 100000),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
TERARANGER::TERARANGER(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency) :
I2C("TERARANGER", nullptr, bus, TERARANGER_ONE_BASEADDR, 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)
{
// up the retries since the device misses the first measure attempts
@ -84,8 +84,6 @@ TERARANGER::TERARANGER(const int bus, const int address, const uint8_t rotation)
TERARANGER::~TERARANGER()
{
stop();
perf_free(_sample_perf);
perf_free(_comms_errors);
}
@ -211,8 +209,6 @@ int TERARANGER::init()
return PX4_ERROR;
}
start();
return PX4_OK;
}
@ -253,7 +249,7 @@ int TERARANGER::probe()
return -EIO;
}
void TERARANGER::Run()
void TERARANGER::RunImpl()
{
// Perform data collection.
collect();
@ -267,13 +263,9 @@ void TERARANGER::start()
ScheduleOnInterval(TERARANGER_MEASUREMENT_INTERVAL);
}
void TERARANGER::stop()
{
ScheduleClear();
}
void TERARANGER::print_info()
void TERARANGER::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);

19
src/drivers/distance_sensor/teraranger/TERARANGER.hpp
View File

@ -42,7 +42,7 @@
#include <drivers/device/i2c.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
#include <lib/perf/perf_counter.h>
@ -72,24 +72,26 @@ using namespace time_literals;
#define TERARANGER_MEASUREMENT_INTERVAL 10_ms
class TERARANGER : public device::I2C, public px4::ScheduledWorkItem
class TERARANGER : public device::I2C, public I2CSPIDriver<TERARANGER>
{
public:
TERARANGER(const int bus, const int address = TERARANGER_ONE_BASEADDR,
const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
TERARANGER(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency);
~TERARANGER() override;
virtual int init() override;
void print_info();
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
virtual int init() override;
void print_status() override;
void RunImpl();
protected:
virtual int probe() override;
private:
void start();
void stop();
int collect();
int measure();
@ -102,7 +104,6 @@ private:
*/
int probe_address(const uint8_t address);
void Run() override;
PX4Rangefinder _px4_rangefinder;

171
src/drivers/distance_sensor/teraranger/teraranger_main.cpp
View File

@ -36,77 +36,8 @@
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
namespace teraranger
{
TERARANGER *g_dev{nullptr};
static int start_bus(const int i2c_bus, const uint8_t orientation)
{
if (g_dev != nullptr) {
PX4_ERR("already started");
return PX4_OK;
}
// Instantiate the driver.
g_dev = new TERARANGER(i2c_bus, TERARANGER_ONE_BASEADDR, orientation);
if (g_dev == nullptr) {
delete g_dev;
return PX4_ERROR;
}
// Initialize the sensor.
if (g_dev->init() != PX4_OK) {
delete g_dev;
g_dev = nullptr;
return PX4_ERROR;
}
return PX4_OK;
}
static int start(const uint8_t orientation)
{
if (g_dev != 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], orientation) == PX4_OK) {
return PX4_OK;
}
}
return PX4_ERROR;
}
static int stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
PX4_INFO("driver stopped");
return PX4_OK;
}
static int status()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return PX4_ERROR;
}
g_dev->print_info();
return PX4_OK;
}
static int usage()
void
TERARANGER::print_usage()
{
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
@ -117,79 +48,71 @@ I2C bus driver for TeraRanger rangefinders.
The sensor/driver must be enabled using the parameter SENS_EN_TRANGER.
Setup/usage information: https://docs.px4.io/en/sensor/rangefinders.html#teraranger-rangefinders
### Examples
Start driver on any bus (start on bus where first sensor found).
$ teraranger start -a
Start driver on specified bus
$ teraranger start -b 1
Stop driver
$ teraranger stop
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("teraranger", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
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_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
PRINT_MODULE_USAGE_PARAM_INT('R', 25, 1, 25, "Sensor rotation - downward facing by default", true);
PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver information");
return PX4_OK;
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
} // namespace
I2CSPIDriverBase *TERARANGER::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
TERARANGER *instance = new TERARANGER(iterator.configuredBusOption(), iterator.bus(), cli.orientation, 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;
}
extern "C" __EXPORT int teraranger_main(int argc, char *argv[])
{
const char *myoptarg = nullptr;
int ch;
int i2c_bus = PX4_I2C_BUS_EXPANSION;
int myoptind = 1;
using ThisDriver = TERARANGER;
BusCLIArguments cli{true, false};
cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
cli.default_i2c_frequency = 100000;
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
bool start_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_all = true;
break;
case 'b':
i2c_bus = atoi(myoptarg);
break;
case 'R':
rotation = (uint8_t)atoi(myoptarg);
cli.orientation = atoi(cli.optarg());
break;
default:
PX4_WARN("Unknown option!");
return teraranger::usage();
}
}
if (myoptind >= argc) {
return teraranger::usage();
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
if (!strcmp(argv[myoptind], "start")) {
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_TERARANGER);
if (start_all) {
return teraranger::start(rotation);
} else {
return teraranger::start_bus(i2c_bus, rotation);
}
} else if (!strcmp(argv[myoptind], "status")) {
return teraranger::status();
} else if (!strcmp(argv[myoptind], "stop")) {
return teraranger::stop();
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
return teraranger::usage();
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ThisDriver::print_usage();
return -1;
}

1
src/drivers/drv_sensor.h
View File

@ -134,6 +134,7 @@
#define DRV_DIST_DEVTYPE_MB12XX 0x72
#define DRV_DIST_DEVTYPE_SF1XX 0x73
#define DRV_DIST_DEVTYPE_SRF02 0x74
#define DRV_DIST_DEVTYPE_TERARANGER 0x75
#define DRV_DEVTYPE_UNUSED 0xff