Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 06:14:14 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

refactor srf02: use driver base class

Browse Source
This commit is contained in:
Beat Küng
2020-03-02 14:48:35 +01:00
committed by Daniel Agar
parent a6ddf0e4b9
commit bbe15aa61a
4 changed files with 62 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/srf02/SRF02.cpp
+7 -21
View File
@@ -33,9 +33,9 @@
#include "SRF02.hpp"
SRF02::SRF02(int bus, int address, uint8_t rotation) :
I2C("SRF02", nullptr, bus, address, 100000),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
SRF02::SRF02(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, int address) :
I2C("SRF02", 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_max_distance(SRF02_MAX_DISTANCE);
@@ -44,8 +44,6 @@ SRF02::SRF02(int bus, int address, uint8_t rotation) :
SRF02::~SRF02()
{
stop();
perf_free(_sample_perf);
perf_free(_comms_errors);
}
@@ -59,11 +57,6 @@ void SRF02::start()
ScheduleDelayed(5);
}
void SRF02::stop()
{
ScheduleClear();
}
int SRF02::init()
{
// I2C init (and probe) first.
@@ -74,15 +67,7 @@ int SRF02::init()
// XXX we should find out why we need to wait 200 ms here
px4_usleep(200000);
int ret = measure();
if (ret != PX4_OK) {
return ret;
}
start();
return PX4_OK;
return measure();
}
int SRF02::collect()
@@ -130,7 +115,7 @@ int SRF02::measure()
return PX4_OK;
}
void SRF02::Run()
void SRF02::RunImpl()
{
if (_collect_phase) {
// Perform collection.
@@ -157,8 +142,9 @@ void SRF02::Run()
ScheduleDelayed(_interval);
}
void SRF02::print_info()
void SRF02::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
src/drivers/distance_sensor/srf02/SRF02.hpp
+11 -6
View File
@@ -40,7 +40,7 @@
#pragma once
#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 <drivers/device/i2c.h>
#include <drivers/drv_hrt.h>
@@ -62,20 +62,25 @@
#define SRF02_CONVERSION_INTERVAL 100000 // 60ms for one sonar.
#define SRF02_INTERVAL_BETWEEN_SUCCESIVE_FIRES 100000 // 30ms between each sonar measurement (watch out for interference!).
class SRF02 : public device::I2C, public px4::ScheduledWorkItem
class SRF02 : public device::I2C, public I2CSPIDriver<SRF02>
{
public:
SRF02(int bus, int address = SRF02_BASEADDR, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
SRF02(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
int address = SRF02_BASEADDR);
~SRF02() override;
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
int init() override;
void print_info();
void print_status() override;
void RunImpl();
private:
void Run() override;
void start();
void stop();
int collect();
int measure();
src/drivers/distance_sensor/srf02/srf02_main.cpp
+43 -109
View File
@@ -36,139 +36,73 @@
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
namespace srf02
void
SRF02::print_usage()
{
SRF02 *g_dev{nullptr};
static int start_bus(uint8_t rotation, int i2c_bus)
{
if (g_dev != nullptr) {
PX4_ERR("already started");
return PX4_ERROR;
}
// Create the driver.
g_dev = new SRF02(rotation, i2c_bus);
if (g_dev == nullptr) {
PX4_ERR("failed to instantiate the device");
return PX4_ERROR;
}
if (OK != g_dev->init()) {
PX4_ERR("failed to initialize the device");
delete g_dev;
g_dev = nullptr;
return -1;
}
return 0;
PRINT_MODULE_USAGE_NAME("srf02", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
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_DEFAULT_COMMANDS();
}
static int start(uint8_t rotation)
I2CSPIDriverBase *SRF02::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
if (g_dev != nullptr) {
PX4_WARN("already started");
return -1;
SRF02 *instance = new SRF02(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency);
if (instance == nullptr) {
PX4_ERR("alloc failed");
return nullptr;
}
for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) {
if (start_bus(rotation, i2c_bus_options[i]) == PX4_OK) {
return PX4_OK;
}
if (instance->init() != PX4_OK) {
delete instance;
return nullptr;
}
return PX4_ERROR;
instance->start();
return instance;
}
static int stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
return -1;
}
return 0;
}
static int status()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return -1;
}
g_dev->print_info();
return 0;
}
static int usage()
{
PX4_INFO("usage: srf02 command [options]");
PX4_INFO("options:");
PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_EXPANSION);
PX4_INFO("\t-a --all");
PX4_INFO("\t-R --rotation (%d)", distance_sensor_s::ROTATION_DOWNWARD_FACING);
PX4_INFO("command:");
PX4_INFO("\tstart|stop|test|reset|info");
return PX4_OK;
}
} // namespace srf02
extern "C" __EXPORT int srf02_main(int argc, char *argv[])
{
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
int i2c_bus = PX4_I2C_BUS_EXPANSION;
bool start_all = false;
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
using ThisDriver = SRF02;
BusCLIArguments cli{true, false};
cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
cli.default_i2c_frequency = 100000;
while ((ch = px4_getopt(argc, argv, "R:ab:", &myoptind, &myoptarg)) != EOF) {
while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
switch (ch) {
case 'R':
rotation = (uint8_t)atoi(myoptarg);
cli.orientation = atoi(cli.optarg());
break;
case 'b':
i2c_bus = atoi(myoptarg);
break;
case 'a':
start_all = true;
break;
default:
srf02::usage();
return -1;
}
}
if (myoptind >= argc) {
srf02::usage();
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
if (!strcmp(argv[myoptind], "start")) {
if (start_all) {
return srf02::start(rotation);
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_SRF02);
} else {
return srf02::start_bus(rotation, i2c_bus);
}
} else if (!strcmp(argv[myoptind], "stop")) {
return srf02::stop();
} else if (!strcmp(argv[myoptind], "status")) {
return srf02::status();
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
return srf02::usage();
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ThisDriver::print_usage();
return -1;
}
src/drivers/drv_sensor.h
+1
View File
@@ -133,6 +133,7 @@
#define DRV_DIST_DEVTYPE_MAPPYDOT 0x71
#define DRV_DIST_DEVTYPE_MB12XX 0x72
#define DRV_DIST_DEVTYPE_SF1XX 0x73
#define DRV_DIST_DEVTYPE_SRF02 0x74
#define DRV_DEVTYPE_UNUSED 0xff