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 pca9685: use driver base class

Browse Source
This commit is contained in:
Beat Küng
2020-03-19 13:23:03 +01:00
committed by Daniel Agar
parent 6588dd0861
commit 030ba24f53
4 changed files with 96 additions and 327 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/uvify/core/init/rc.board_sensors
+1 -1
View File
@@ -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
src/drivers/drv_io_expander.h
-71
View File
@@ -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 <stdint.h>
#include <sys/ioctl.h>
/*
* 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
};
src/drivers/drv_sensor.h
+1
View File
@@ -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
src/drivers/pca9685/pca9685.cpp
+94 -255
View File
@@ -48,6 +48,7 @@
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <drivers/device/i2c.h>
@@ -62,8 +63,7 @@
#include <ctype.h>
#include <math.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
#include <nuttx/clock.h>
#include <perf/perf_counter.h>
@@ -73,7 +73,6 @@
#include <uORB/topics/actuator_controls.h>
#include <board_config.h>
#include <drivers/drv_io_expander.h>
#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<PCA9685>
{
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;
}