mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-21 09:22:18 +00:00
refactor teraranger: use driver base class
This commit is contained in:
Beat Küng
Daniel Agar
@@ -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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user