Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 11:59:17 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

refactor pmw3910: use driver base class

Browse Source
This commit is contained in:
Beat Küng
2020-02-26 14:35:14 +01:00
committed by Daniel Agar
parent 2f4080f47b
commit 58f386a81c
5 changed files with 61 additions and 164 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rc.sensors
+2 -2
View File
@@ -106,10 +106,10 @@ then
teraranger start -a
fi
# Possible pmw3901 optical flow sensor
# Possible external pmw3901 optical flow sensor
if param greater -s SENS_EN_PMW3901 0
then
pmw3901 start
pmw3901 -S start
fi
###############################################################################
boards/nxp/fmurt1062-v1/init/rc.board_sensors
+1 -1
View File
@@ -53,6 +53,6 @@ ms5611 -s start
rm3100 start
# Possible pmw3901 optical flow sensor
pmw3901 start
pmw3901 -S start
px4flow start &
src/drivers/optical_flow/pmw3901/PMW3901.cpp
+7 -8
View File
@@ -35,9 +35,10 @@
static constexpr uint32_t TIME_us_TSWW = 11; // - actually 10.5us
PMW3901::PMW3901(int bus, enum Rotation yaw_rotation) :
SPI("PMW3901", PMW3901_DEVICE_PATH, bus, PMW3901_SPIDEV, SPIDEV_MODE0, PMW3901_SPI_BUS_SPEED),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
PMW3901::PMW3901(I2CSPIBusOption bus_option, int bus, int devid, enum Rotation yaw_rotation, int bus_frequency,
spi_mode_e spi_mode) :
SPI("PMW3901", PMW3901_DEVICE_PATH, bus, devid, spi_mode, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
_sample_perf(perf_alloc(PC_ELAPSED, "pmw3901: read")),
_comms_errors(perf_alloc(PC_COUNT, "pmw3901: com err")),
_yaw_rotation(yaw_rotation)
@@ -46,9 +47,6 @@ PMW3901::PMW3901(int bus, enum Rotation yaw_rotation) :
PMW3901::~PMW3901()
{
// make sure we are truly inactive
stop();
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
@@ -296,7 +294,7 @@ PMW3901::writeRegister(unsigned reg, uint8_t data)
}
void
PMW3901::Run()
PMW3901::RunImpl()
{
perf_begin(_sample_perf);
@@ -414,8 +412,9 @@ PMW3901::stop()
}
void
PMW3901::print_info()
PMW3901::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
}
src/drivers/optical_flow/pmw3901/PMW3901.hpp
+11 -31
View File
@@ -49,29 +49,10 @@
#include <drivers/drv_hrt.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/optical_flow.h>
#include <px4_platform_common/i2c_spi_buses.h>
/* Configuration Constants */
#if defined PX4_SPI_BUS_EXPANSION // crazyflie
# define PMW3901_BUS PX4_SPI_BUS_EXPANSION
#elif defined PX4_SPI_BUS_EXTERNAL1 // fmu-v5
# define PMW3901_BUS PX4_SPI_BUS_EXTERNAL1
#elif defined PX4_SPI_BUS_EXTERNAL // fmu-v4 extspi
# define PMW3901_BUS PX4_SPI_BUS_EXTERNAL
#else
# error "add the required spi bus from board_config.h here"
#endif
#if defined PX4_SPIDEV_EXPANSION_2 // crazyflie flow deck
# define PMW3901_SPIDEV PX4_SPIDEV_EXPANSION_2
#elif defined PX4_SPIDEV_EXTERNAL1_1 // fmu-v5 ext CS1
# define PMW3901_SPIDEV PX4_SPIDEV_EXTERNAL1_1
#elif defined PX4_SPIDEV_EXTERNAL // fmu-v4 extspi
# define PMW3901_SPIDEV PX4_SPIDEV_EXTERNAL
#else
# error "add the required spi dev from board_config.h here"
#endif
#define PMW3901_SPI_BUS_SPEED (2000000L) // 2MHz
#define DIR_WRITE(a) ((a) | (1 << 7))
@@ -84,19 +65,23 @@
#define PMW3901_SAMPLE_INTERVAL 10000 /* 10 ms */
class PMW3901 : public device::SPI, public px4::ScheduledWorkItem
class PMW3901 : public device::SPI, public I2CSPIDriver<PMW3901>
{
public:
PMW3901(int bus = PMW3901_BUS, enum Rotation yaw_rotation = (enum Rotation)0);
PMW3901(I2CSPIBusOption bus_option, int bus, int devid, enum Rotation yaw_rotation, int bus_frequency,
spi_mode_e spi_mode);
virtual ~PMW3901();
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
virtual int init();
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
void print_status();
void RunImpl();
protected:
virtual int probe();
@@ -133,11 +118,6 @@ private:
*/
void stop();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void Run() override;
int readRegister(unsigned reg, uint8_t *data, unsigned count);
int writeRegister(unsigned reg, uint8_t data);
src/drivers/optical_flow/pmw3901/pmw3901_main.cpp
+40 -122
View File
@@ -32,159 +32,77 @@
****************************************************************************/
#include "PMW3901.hpp"
#include <px4_platform_common/module.h>
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int pmw3901_main(int argc, char *argv[]);
/**
* Local functions in support of the shell command.
*/
namespace pmw3901
{
PMW3901 *g_dev;
void start(int spi_bus);
void stop();
void test();
void reset();
void info();
void usage();
/**
* Start the driver.
*/
void
start(int spi_bus)
PMW3901::print_usage()
{
if (g_dev != nullptr) {
errx(1, "already started");
}
/* create the driver */
g_dev = new PMW3901(spi_bus, (enum Rotation)0);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
PRINT_MODULE_USAGE_NAME("pmw3901", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
/**
* Stop the driver
*/
void stop()
I2CSPIDriverBase *PMW3901::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
PMW3901 *instance = new PMW3901(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
cli.bus_frequency, cli.spi_mode);
} else {
errx(1, "driver not running");
if (!instance) {
PX4_ERR("alloc failed");
return nullptr;
}
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr) {
errx(1, "driver not running");
if (OK != instance->init()) {
delete instance;
return nullptr;
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
return instance;
}
/**
* Print a little info about how to start/stop/use the driver
*/
void usage()
{
PX4_INFO("usage: pmw3901 {start|test|reset|info'}");
PX4_INFO(" [-b SPI_BUS]");
}
} // namespace pmw3901
int
pmw3901_main(int argc, char *argv[])
{
if (argc < 2) {
pmw3901::usage();
return PX4_ERROR;
}
// don't exit from getopt loop to leave getopt global variables in consistent state,
// set error flag instead
bool err_flag = false;
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
int spi_bus = PMW3901_BUS;
using ThisDriver = PMW3901;
BusCLIArguments cli{false, true};
cli.spi_mode = SPIDEV_MODE0;
cli.default_spi_frequency = PMW3901_SPI_BUS_SPEED;
while ((ch = px4_getopt(argc, argv, "b:", &myoptind, &myoptarg)) != EOF) {
while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
switch (ch) {
case 'b':
spi_bus = (uint8_t)atoi(myoptarg);
break;
default:
err_flag = true;
case 'R':
cli.rotation = (enum Rotation)atoi(cli.optarg());
break;
}
}
if (err_flag) {
pmw3901::usage();
return PX4_ERROR;
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[myoptind], "start")) {
pmw3901::start(spi_bus);
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_FLOW_DEVTYPE_PMW3901);
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
pmw3901::stop();
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
/*
* Print driver information.
*/
if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
pmw3901::info();
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
pmw3901::usage();
return PX4_ERROR;
ThisDriver::print_usage();
return -1;
}