From 030ba24f53cb3b304c53785a0120bb1e655677f8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 19 Mar 2020 13:23:03 +0100 Subject: [PATCH] refactor pca9685: use driver base class --- boards/uvify/core/init/rc.board_sensors | 2 +- src/drivers/drv_io_expander.h | 71 ----- src/drivers/drv_sensor.h | 1 + src/drivers/pca9685/pca9685.cpp | 349 +++++++----------------- 4 files changed, 96 insertions(+), 327 deletions(-) delete mode 100644 src/drivers/drv_io_expander.h diff --git a/boards/uvify/core/init/rc.board_sensors b/boards/uvify/core/init/rc.board_sensors index 58c2b92065..9305a9ff6a 100644 --- a/boards/uvify/core/init/rc.board_sensors +++ b/boards/uvify/core/init/rc.board_sensors @@ -37,7 +37,7 @@ then rgbled_ncp5623c start -X -a 0x38 # IFO rgb LED - pca9685 start + pca9685 -X start mpu9250 -s -R 2 start lis3mdl -R 2 -X start diff --git a/src/drivers/drv_io_expander.h b/src/drivers/drv_io_expander.h deleted file mode 100644 index 106354377f..0000000000 --- a/src/drivers/drv_io_expander.h +++ /dev/null @@ -1,71 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file drv_io_expander.h - * - * IO expander device API - */ - -#pragma once - -#include -#include - -/* - * ioctl() definitions - */ - -#define _IOXIOCBASE (0x2800) -#define _IOXIOC(_n) (_IOC(_IOXIOCBASE, _n)) - -/** set a bitmask (non-blocking) */ -#define IOX_SET_MASK _IOXIOC(1) - -/** get a bitmask (blocking) */ -#define IOX_GET_MASK _IOXIOC(2) - -/** set device mode (non-blocking) */ -#define IOX_SET_MODE _IOXIOC(3) - -/** set constant values (non-blocking) */ -#define IOX_SET_VALUE _IOXIOC(4) - -/* ... to IOX_SET_VALUE + 8 */ - -/* enum passed to RGBLED_SET_MODE ioctl()*/ -enum IOX_MODE { - IOX_MODE_OFF, - IOX_MODE_ON, - IOX_MODE_TEST_OUT -}; diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 6a1463bbc9..ebce13b5fd 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -128,6 +128,7 @@ #define DRV_FLOW_DEVTYPE_PMW3901 0x6c #define DRV_FLOW_DEVTYPE_PAW3902 0x6d #define DRV_FLOW_DEVTYPE_PX4FLOW 0x6e +#define DRV_PWM_DEVTYPE_PCA9685 0x6f #define DRV_DIST_DEVTYPE_LL40LS 0x70 #define DRV_DIST_DEVTYPE_MAPPYDOT 0x71 diff --git a/src/drivers/pca9685/pca9685.cpp b/src/drivers/pca9685/pca9685.cpp index 665438f64d..934b253bd2 100644 --- a/src/drivers/pca9685/pca9685.cpp +++ b/src/drivers/pca9685/pca9685.cpp @@ -48,6 +48,7 @@ #include #include +#include #include @@ -62,8 +63,7 @@ #include #include #include - -#include +#include #include #include @@ -73,7 +73,6 @@ #include #include -#include #define PCA9685_SUBADR1 0x2 #define PCA9685_SUBADR2 0x3 @@ -95,7 +94,6 @@ #define ADDR 0x40 // I2C adress #define PCA9685_DEVICE_PATH "/dev/pca9685" -#define PCA9685_BUS PX4_I2C_BUS_EXPANSION #define PCA9685_PWMFREQ 60.0f #define PCA9685_NCHANS 16 // total amount of pwm outputs @@ -109,27 +107,36 @@ */ #define PCA9685_SCALE ((PCA9685_PWMMAX - PCA9685_PWMCENTER)/(M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) // scales from rad to PWM +enum IOX_MODE { + IOX_MODE_ON, + IOX_MODE_TEST_OUT +}; + using namespace time_literals; -class PCA9685 : public device::I2C, public px4::ScheduledWorkItem +class PCA9685 : public device::I2C, public I2CSPIDriver { public: - PCA9685(int bus = PCA9685_BUS, uint8_t address = ADDR); - virtual ~PCA9685() = default; + PCA9685(I2CSPIBusOption bus_option, int bus, int bus_frequency); + ~PCA9685() override = default; + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); - virtual int init(); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - virtual int info(); - virtual int reset(); - bool is_running() { return _running; } + void print_status(); + int init() override; + int reset(); + + void RunImpl(); + +protected: + void custom_method(const BusCLIArguments &cli) override; private: enum IOX_MODE _mode; - bool _running; uint64_t _i2cpwm_interval; - bool _should_run; perf_counter_t _comms_errors; uint8_t _msg[6]; @@ -141,7 +148,6 @@ private: bool _mode_on_initialized; /** Set to true after the first call of i2cpwm in mode IOX_MODE_ON */ - void Run() override; /** * Helper function to set the pwm frequency @@ -172,23 +178,11 @@ private: }; -/* for now, we only support one board */ -namespace -{ -PCA9685 *g_pca9685; -} - -void pca9685_usage(); - -extern "C" __EXPORT int pca9685_main(int argc, char *argv[]); - -PCA9685::PCA9685(int bus, uint8_t address) : - I2C("pca9685", PCA9685_DEVICE_PATH, bus, address, 100000), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), - _mode(IOX_MODE_OFF), - _running(false), +PCA9685::PCA9685(I2CSPIBusOption bus_option, int bus, int bus_frequency) : + I2C("pca9685", PCA9685_DEVICE_PATH, bus, ADDR, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), + _mode(IOX_MODE_ON), _i2cpwm_interval(1_s / 60.0f), - _should_run(false), _comms_errors(perf_alloc(PC_COUNT, "pca9685_com_err")), _actuator_controls_sub(-1), _actuator_controls(), @@ -216,85 +210,25 @@ PCA9685::init() ret = setPWMFreq(PCA9685_PWMFREQ); - return ret; -} - -int -PCA9685::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - int ret = -EINVAL; - - switch (cmd) { - - case IOX_SET_MODE: - - if (_mode != (IOX_MODE)arg) { - - switch ((IOX_MODE)arg) { - case IOX_MODE_OFF: - warnx("shutting down"); - break; - - case IOX_MODE_ON: - warnx("starting"); - break; - - case IOX_MODE_TEST_OUT: - warnx("test starting"); - break; - - default: - return -1; - } - - _mode = (IOX_MODE)arg; - } - - // if not active, kick it - if (!_running) { - _running = true; - ScheduleNow(); - } - - - return OK; - - default: - // see if the parent class can make any use of it - ret = CDev::ioctl(filp, cmd, arg); - break; + if (ret == 0) { + ScheduleNow(); } return ret; } -int -PCA9685::info() -{ - int ret = OK; - - if (is_running()) { - warnx("Driver is running, mode: %u", _mode); - - } else { - warnx("Driver started but not running"); - } - - return ret; -} - -/** - * Main loop function - */ void -PCA9685::Run() +PCA9685::print_status() +{ + I2CSPIDriverBase::print_status(); + PX4_INFO("Mode: %u", _mode); +} + +void +PCA9685::RunImpl() { if (_mode == IOX_MODE_TEST_OUT) { setPin(0, PCA9685_PWMCENTER); - _should_run = true; - - } else if (_mode == IOX_MODE_OFF) { - _should_run = false; } else { if (!_mode_on_initialized) { @@ -330,18 +264,8 @@ PCA9685::Run() } } } - - _should_run = true; } - // check if any activity remains, else stop - if (!_should_run) { - _running = false; - return; - } - - // re-queue ourselves to run again later - _running = true; ScheduleDelayed(_i2cpwm_interval); } @@ -508,165 +432,80 @@ PCA9685::write8(uint8_t addr, uint8_t value) } void -pca9685_usage() +PCA9685::print_usage() { - warnx("missing command: try 'start', 'test', 'stop', 'info'"); - warnx("options:"); - warnx(" -b i2cbus (%d)", PX4_I2C_BUS_EXPANSION); - warnx(" -a addr (0x%x)", ADDR); + PRINT_MODULE_USAGE_NAME("pca9685", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_COMMAND("reset"); + PRINT_MODULE_USAGE_COMMAND_DESCR("test", "enter test mode"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -int -pca9685_main(int argc, char *argv[]) +I2CSPIDriverBase *PCA9685::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { - int i2cdevice = -1; - int i2caddr = ADDR; // 7bit + PCA9685 *instance = new PCA9685(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency); - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - - - // jump over start/off/etc and look at options first - while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'a': - i2caddr = strtol(myoptarg, NULL, 0); - break; - - case 'b': - i2cdevice = strtol(myoptarg, NULL, 0); - break; - - default: - pca9685_usage(); - exit(0); - } + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; } - if (myoptind >= argc) { - pca9685_usage(); - exit(0); + if (OK != instance->init()) { + delete instance; + return nullptr; } - const char *verb = argv[myoptind]; + return instance; +} - int fd; - int ret; +void PCA9685::custom_method(const BusCLIArguments &cli) +{ + switch (cli.custom1) { + case 0: reset(); break; + + case 1: _mode = IOX_MODE_TEST_OUT; break; + } +} + +extern "C" int pca9685_main(int argc, char *argv[]) +{ + using ThisDriver = PCA9685; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = 100000; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_PWM_DEVTYPE_PCA9685); if (!strcmp(verb, "start")) { - if (g_pca9685 != nullptr) { - errx(1, "already started"); - } - - if (i2cdevice == -1) { - // try the external bus first - i2cdevice = PX4_I2C_BUS_EXPANSION; - g_pca9685 = new PCA9685(PX4_I2C_BUS_EXPANSION, i2caddr); - - if (g_pca9685 != nullptr && OK != g_pca9685->init()) { - delete g_pca9685; - g_pca9685 = nullptr; - } - - if (g_pca9685 == nullptr) { - errx(1, "init failed"); - } - } - - if (g_pca9685 == nullptr) { - g_pca9685 = new PCA9685(i2cdevice, i2caddr); - - if (g_pca9685 == nullptr) { - errx(1, "new failed"); - } - - if (OK != g_pca9685->init()) { - delete g_pca9685; - g_pca9685 = nullptr; - errx(1, "init failed"); - } - } - - fd = open(PCA9685_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " PCA9685_DEVICE_PATH); - } - - ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON); - close(fd); - - - exit(0); - } - - // need the driver past this point - if (g_pca9685 == nullptr) { - warnx("not started, run pca9685 start"); - exit(1); - } - - if (!strcmp(verb, "info")) { - g_pca9685->info(); - exit(0); - } - - if (!strcmp(verb, "reset")) { - g_pca9685->reset(); - exit(0); - } - - - if (!strcmp(verb, "test")) { - fd = open(PCA9685_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " PCA9685_DEVICE_PATH); - } - - ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT); - - close(fd); - exit(ret); + return ThisDriver::module_start(cli, iterator); } if (!strcmp(verb, "stop")) { - fd = open(PCA9685_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " PCA9685_DEVICE_PATH); - } - - ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); - close(fd); - - // wait until we're not running any more - for (unsigned i = 0; i < 15; i++) { - if (!g_pca9685->is_running()) { - break; - } - - usleep(50000); - printf("."); - fflush(stdout); - } - - printf("\n"); - fflush(stdout); - - if (!g_pca9685->is_running()) { - delete g_pca9685; - g_pca9685 = nullptr; - warnx("stopped, exiting"); - exit(0); - - } else { - warnx("stop failed."); - exit(1); - } + return ThisDriver::module_stop(iterator); } - pca9685_usage(); - exit(0); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + if (!strcmp(verb, "reset")) { + cli.custom1 = 0; + return ThisDriver::module_custom_method(cli, iterator); + } + + if (!strcmp(verb, "test")) { + cli.custom1 = 1; + return ThisDriver::module_custom_method(cli, iterator); + } + + ThisDriver::print_usage(); + return -1; }