dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

refactor irlock: use driver base class

This commit is contained in:
Beat Küng 2020-03-11 18:50:52 +01:00 committed by Daniel Agar
parent 1710cd9648
commit bb4ff04caf
2 changed files with 92 additions and 142 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/drivers/drv_sensor.h
View File

@ -144,6 +144,7 @@
#define DRV_LED_DEVTYPE_RGBLED 0x7a #define DRV_LED_DEVTYPE_RGBLED 0x7a
#define DRV_LED_DEVTYPE_RGBLED_NCP5623C 0x7b #define DRV_LED_DEVTYPE_RGBLED_NCP5623C 0x7b
#define DRV_BAT_DEVTYPE_SMBUS 0x7c #define DRV_BAT_DEVTYPE_SMBUS 0x7c
#define DRV_SENS_DEVTYPE_IRLOCK 0x7d
#define DRV_DEVTYPE_UNUSED 0xff #define DRV_DEVTYPE_UNUSED 0xff

233
src/drivers/irlock/irlock.cpp
View File

@ -46,14 +46,13 @@
#include <drivers/device/ringbuffer.h> #include <drivers/device/ringbuffer.h>
#include <px4_platform_common/getopt.h> #include <px4_platform_common/getopt.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> #include <px4_platform_common/module.h>
#include <systemlib/err.h> #include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/Publication.hpp> #include <uORB/Publication.hpp>
#include <uORB/topics/irlock_report.h> #include <uORB/topics/irlock_report.h>
/** Configuration Constants **/ /** Configuration Constants **/
#define IRLOCK_I2C_BUS PX4_I2C_BUS_EXPANSION
#define IRLOCK_I2C_ADDRESS 0x54 /** 7-bit address (non shifted) **/ #define IRLOCK_I2C_ADDRESS 0x54 /** 7-bit address (non shifted) **/
#define IRLOCK_CONVERSION_INTERVAL_US 20000U /** us = 20ms = 50Hz **/ #define IRLOCK_CONVERSION_INTERVAL_US 20000U /** us = 20ms = 50Hz **/
@ -91,35 +90,37 @@ struct irlock_target_s {
/** irlock_s structure returned from read calls **/ /** irlock_s structure returned from read calls **/
struct irlock_s { struct irlock_s {
uint64_t timestamp; /** microseconds since system start **/ hrt_abstime timestamp; /** microseconds since system start **/
uint8_t num_targets; uint8_t num_targets;
struct irlock_target_s targets[IRLOCK_OBJECTS_MAX]; struct irlock_target_s targets[IRLOCK_OBJECTS_MAX];
}; };
class IRLOCK : public device::I2C, public px4::ScheduledWorkItem class IRLOCK : public device::I2C, public I2CSPIDriver<IRLOCK>
{ {
public: public:
IRLOCK(int bus = IRLOCK_I2C_BUS, int address = IRLOCK_I2C_ADDRESS); IRLOCK(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address);
virtual ~IRLOCK(); virtual ~IRLOCK();
virtual int init(); static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
virtual int probe(); int runtime_instance);
virtual int info(); static void print_usage();
virtual int test();
void custom_method(const BusCLIArguments &cli) override;
int init() override;
int probe() override;
void print_status() override;
int test();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
/** read from device and schedule next read **/
void RunImpl();
private: private:
/** start periodic reads from sensor **/ /** start periodic reads from sensor **/
void start(); void start();
/** stop periodic reads from sensor **/
void stop();
/** read from device and schedule next read **/
void Run() override;
/** low level communication with sensor **/ /** low level communication with sensor **/
int read_device(); int read_device();
bool sync_device(); bool sync_device();
@ -135,20 +136,11 @@ private:
uORB::Publication<irlock_report_s> _irlock_report_topic{ORB_ID(irlock_report)}; uORB::Publication<irlock_report_s> _irlock_report_topic{ORB_ID(irlock_report)};
}; };
/** global pointer for single IRLOCK sensor **/
namespace
{
IRLOCK *g_irlock = nullptr;
}
void irlock_usage();
extern "C" __EXPORT int irlock_main(int argc, char *argv[]); extern "C" __EXPORT int irlock_main(int argc, char *argv[]);
/** constructor **/ IRLOCK::IRLOCK(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address) :
IRLOCK::IRLOCK(int bus, int address) : I2C("irlock", IRLOCK0_DEVICE_PATH, bus, address, bus_frequency),
I2C("irlock", IRLOCK0_DEVICE_PATH, bus, address, 400000), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
_reports(nullptr), _reports(nullptr),
_sensor_ok(false), _sensor_ok(false),
_read_failures(0), _read_failures(0),
@ -156,12 +148,8 @@ IRLOCK::IRLOCK(int bus, int address) :
{ {
} }
/** destructor **/
IRLOCK::~IRLOCK() IRLOCK::~IRLOCK()
{ {
stop();
/** clear reports queue **/
if (_reports != nullptr) { if (_reports != nullptr) {
delete _reports; delete _reports;
} }
@ -208,40 +196,29 @@ int IRLOCK::probe()
return OK; return OK;
} }
/** display driver info **/ void IRLOCK::print_status()
int IRLOCK::info()
{ {
if (g_irlock == nullptr) {
errx(1, "irlock device driver is not running");
}
/** display reports in queue **/ /** display reports in queue **/
if (_sensor_ok) { if (_sensor_ok) {
_reports->print_info("report queue: "); _reports->print_info("report queue: ");
warnx("read errors:%lu", (unsigned long)_read_failures); PX4_INFO("read errors:%lu", (unsigned long)_read_failures);
} else { } else {
warnx("sensor is not healthy"); PX4_WARN("sensor is not healthy");
} }
return OK;
} }
/** test driver **/ /** test driver **/
int IRLOCK::test() int IRLOCK::test()
{ {
/** exit immediately if driver not running **/
if (g_irlock == nullptr) {
errx(1, "irlock device driver is not running");
}
/** exit immediately if sensor is not healty **/ /** exit immediately if sensor is not healty **/
if (!_sensor_ok) { if (!_sensor_ok) {
errx(1, "sensor is not healthy"); PX4_ERR("sensor is not healthy");
return -1;
} }
/** instructions to user **/ /** instructions to user **/
warnx("searching for object for 10 seconds"); PX4_INFO("searching for object for 10 seconds");
/** read from sensor for 10 seconds **/ /** read from sensor for 10 seconds **/
struct irlock_s report; struct irlock_s report;
@ -251,12 +228,12 @@ int IRLOCK::test()
if (_reports->get(&report)) { if (_reports->get(&report)) {
/** output all objects found **/ /** output all objects found **/
for (uint8_t i = 0; i < report.num_targets; i++) { for (uint8_t i = 0; i < report.num_targets; i++) {
warnx("sig:%d x:%4.3f y:%4.3f width:%4.3f height:%4.3f", PX4_INFO("sig:%d x:%4.3f y:%4.3f width:%4.3f height:%4.3f",
(int)report.targets[i].signature, (int)report.targets[i].signature,
(double)report.targets[i].pos_x, (double)report.targets[i].pos_x,
(double)report.targets[i].pos_y, (double)report.targets[i].pos_y,
(double)report.targets[i].size_x, (double)report.targets[i].size_x,
(double)report.targets[i].size_y); (double)report.targets[i].size_y);
} }
} }
@ -277,13 +254,7 @@ void IRLOCK::start()
ScheduleNow(); ScheduleNow();
} }
/** stop periodic reads from sensor **/ void IRLOCK::RunImpl()
void IRLOCK::stop()
{
ScheduleClear();
}
void IRLOCK::Run()
{ {
/** ignoring failure, if we do, we will be back again right away... **/ /** ignoring failure, if we do, we will be back again right away... **/
read_device(); read_device();
@ -420,93 +391,71 @@ int IRLOCK::read_device_block(struct irlock_target_s *block)
return status; return status;
} }
void irlock_usage() void
IRLOCK::print_usage()
{ {
warnx("missing command: try 'start', 'stop', 'info', 'test'"); PRINT_MODULE_USAGE_NAME("irlock", "driver");
warnx("options:"); PRINT_MODULE_USAGE_COMMAND("start");
warnx(" -b i2cbus (%d)", IRLOCK_I2C_BUS); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x54);
PRINT_MODULE_USAGE_COMMAND("test");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
} }
int irlock_main(int argc, char *argv[]) I2CSPIDriverBase *IRLOCK::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{ {
int i2cdevice = IRLOCK_I2C_BUS; IRLOCK *instance = new IRLOCK(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address);
int ch; if (instance == nullptr) {
int myoptind = 1; PX4_ERR("alloc failed");
const char *myoptarg = nullptr; return nullptr;
while ((ch = px4_getopt(argc, argv, "b:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'b':
i2cdevice = (uint8_t)atoi(myoptarg);
break;
default:
PX4_WARN("Unknown option!");
return -1;
}
} }
if (myoptind >= argc) { if (instance->init() != PX4_OK) {
irlock_usage(); delete instance;
exit(1); return nullptr;
} }
const char *command = argv[myoptind]; return instance;
}
/** start driver **/ void
if (!strcmp(command, "start")) { IRLOCK::custom_method(const BusCLIArguments &cli)
if (g_irlock != nullptr) { {
errx(1, "driver has already been started"); test();
} }
/** instantiate global instance **/ extern "C" __EXPORT int irlock_main(int argc, char *argv[])
g_irlock = new IRLOCK(i2cdevice, IRLOCK_I2C_ADDRESS); {
using ThisDriver = IRLOCK;
if (g_irlock == nullptr) { BusCLIArguments cli{true, false};
errx(1, "failed to allocated memory for driver"); cli.i2c_address = IRLOCK_I2C_ADDRESS;
}
const char *verb = cli.parseDefaultArguments(argc, argv);
/** initialise global instance **/
if (g_irlock->init() != OK) { if (!verb) {
IRLOCK *tmp_irlock = g_irlock; ThisDriver::print_usage();
g_irlock = nullptr; return -1;
delete tmp_irlock; }
errx(1, "failed to initialize device, stopping driver");
} BusInstanceIterator iterator(MODULE_NAME, cli, DRV_SENS_DEVTYPE_IRLOCK);
exit(0); if (!strcmp(verb, "start")) {
} return ThisDriver::module_start(cli, iterator);
}
/** need the driver past this point **/
if (g_irlock == nullptr) { if (!strcmp(verb, "stop")) {
warnx("not started"); return ThisDriver::module_stop(iterator);
irlock_usage(); }
exit(1);
} if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
/** stop the driver **/ }
if (!strcmp(command, "stop")) {
IRLOCK *tmp_irlock = g_irlock; if (!strcmp(verb, "test")) {
g_irlock = nullptr; return ThisDriver::module_custom_method(cli, iterator, false);
delete tmp_irlock; }
warnx("irlock stopped");
exit(OK); ThisDriver::print_usage();
} return -1;
/** Print driver information **/
if (!strcmp(command, "info")) {
g_irlock->info();
exit(OK);
}
/** test driver **/
if (!strcmp(command, "test")) {
g_irlock->test();
exit(OK);
}
/** display usage info **/
irlock_usage();
exit(0);
} }