From bb1a45ac42001d96cbc4a4a4fbe69c989c261094 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Aug 2014 15:22:15 +0200 Subject: [PATCH 1/4] skeleton for adafruiti2cpwm driver --- src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp | 362 ++++++++++++++++++ ...fruit_PWM_Servo_Driver_Library_license.txt | 29 ++ src/drivers/adafruiti2cpwm/module.mk | 41 ++ 3 files changed, 432 insertions(+) create mode 100644 src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp create mode 100644 src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt create mode 100644 src/drivers/adafruiti2cpwm/module.mk diff --git a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp new file mode 100644 index 0000000000..959b90db86 --- /dev/null +++ b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp @@ -0,0 +1,362 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 adafruiti2cpwm.cpp + * + * Driver for the adafruit I2C PWM converter based on the PCA9685 + * https://www.adafruit.com/product/815 + * + * some code is adapted from the arduino library for the board + * https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library + * see the arduino_Adafruit_PWM_Servo_Driver_Library_license.txt file + * see https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library for contributors + * + * @author Thomas Gubler + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#define PCA9685_SUBADR1 0x2 +#define PCA9685_SUBADR2 0x3 +#define PCA9685_SUBADR3 0x4 + +#define PCA9685_MODE1 0x0 +#define PCA9685_PRESCALE 0xFE + +#define LED0_ON_L 0x6 +#define LED0_ON_H 0x7 +#define LED0_OFF_L 0x8 +#define LED0_OFF_H 0x9 + +#define ALLLED_ON_L 0xFA +#define ALLLED_ON_H 0xFB +#define ALLLED_OFF_L 0xFC +#define ALLLED_OFF_H 0xFD + +#define ADDR PCA9685_SUBADR1 ///< I2C adress + +#define ADAFRUITI2CPWM_DEVICE_PATH "/dev/adafruiti2cpwm" +#define ADAFRUITI2CPWM_BUS PX4_I2C_BUS_EXPANSION + +class ADAFRUITI2CPWM : public device::I2C +{ +public: + ADAFRUITI2CPWM(int bus=ADAFRUITI2CPWM_BUS, uint8_t address=ADDR); + virtual ~ADAFRUITI2CPWM(); + + + virtual int init(); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + bool is_running() { return _running; } + +private: + work_s _work; + + + enum IOX_MODE _mode; + bool _running; + int _i2cpwm_interval; + bool _should_run; + + static void i2cpwm_trampoline(void *arg); + void i2cpwm(); + +}; + +/* for now, we only support one board */ +namespace +{ +ADAFRUITI2CPWM *g_adafruiti2cpwm; +} + +void adafruiti2cpwm_usage(); + +extern "C" __EXPORT int adafruiti2cpwm_main(int argc, char *argv[]); + +ADAFRUITI2CPWM::ADAFRUITI2CPWM(int bus, uint8_t address) : + I2C("adafruiti2cpwm", ADAFRUITI2CPWM_DEVICE_PATH, bus, address, 100000), + _mode(IOX_MODE_OFF), + _running(false), + _i2cpwm_interval(SEC2TICK(1.0f/60.0f)), + _should_run(false) +{ + memset(&_work, 0, sizeof(_work)); +} + +ADAFRUITI2CPWM::~ADAFRUITI2CPWM() +{ +} + +int +ADAFRUITI2CPWM::init() +{ + int ret; + ret = I2C::init(); + + if (ret != OK) { + return ret; + } + + return OK; +} + +int +ADAFRUITI2CPWM::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: + case IOX_MODE_ON: + case IOX_MODE_TEST_OUT: + break; + + default: + return -1; + } + + _mode = (IOX_MODE)arg; + } + + return OK; + + default: + // see if the parent class can make any use of it + ret = CDev::ioctl(filp, cmd, arg); + break; + } + + return ret; +} + +void +ADAFRUITI2CPWM::i2cpwm_trampoline(void *arg) +{ + ADAFRUITI2CPWM *i2cpwm = reinterpret_cast(arg); + + i2cpwm->i2cpwm(); +} + +/** + * Main loop function + */ +void +ADAFRUITI2CPWM::i2cpwm() +{ + + if (_mode == IOX_MODE_TEST_OUT) { + + _should_run = true; + } else if (_mode == IOX_MODE_OFF) { + _should_run = false; + } else { + + } + + + // check if any activity remains, else stop + if (!_should_run) { + _running = false; + return; + } + + // re-queue ourselves to run again later + _running = true; + work_queue(LPWORK, &_work, (worker_t)&ADAFRUITI2CPWM::i2cpwm_trampoline, this, _i2cpwm_interval); +} + +void +print_usage() +{ + warnx("missing command: try 'start', 'test', 'stop'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_EXPANSION); + warnx(" -a addr (0x%x)", ADDR); +} + +int +adafruiti2cpwm_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int i2caddr = ADDR; // 7bit + + int ch; + + // jump over start/off/etc and look at options first + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + i2caddr = strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + + default: + print_usage(); + exit(0); + } + } + + if (optind >= argc) { + print_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + int fd; + int ret; + + if (!strcmp(verb, "start")) { + if (g_adafruiti2cpwm != nullptr) { + errx(1, "already started"); + } + + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_adafruiti2cpwm = new ADAFRUITI2CPWM(PX4_I2C_BUS_EXPANSION, i2caddr); + + if (g_adafruiti2cpwm != nullptr && OK != g_adafruiti2cpwm->init()) { + delete g_adafruiti2cpwm; + g_adafruiti2cpwm = nullptr; + } + + if (g_adafruiti2cpwm == nullptr) { + errx(1, "init failed"); + } + } + + if (g_adafruiti2cpwm == nullptr) { + g_adafruiti2cpwm = new ADAFRUITI2CPWM(i2cdevice, i2caddr); + + if (g_adafruiti2cpwm == nullptr) { + errx(1, "new failed"); + } + + if (OK != g_adafruiti2cpwm->init()) { + delete g_adafruiti2cpwm; + g_adafruiti2cpwm = nullptr; + errx(1, "init failed"); + } + } + + exit(0); + } + + // need the driver past this point + if (g_adafruiti2cpwm == nullptr) { + warnx("not started, run adafruiti2cpwm start"); + exit(1); + } + + if (!strcmp(verb, "test")) { + fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " ADAFRUITI2CPWM_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT); + + close(fd); + exit(ret); + } + + if (!strcmp(verb, "stop")) { + fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " ADAFRUITI2CPWM_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_adafruiti2cpwm->is_running()) { + break; + } + + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + fflush(stdout); + + if (!g_adafruiti2cpwm->is_running()) { + delete g_adafruiti2cpwm; + g_adafruiti2cpwm= nullptr; + warnx("stopped, exiting"); + exit(0); + } else { + warnx("stop failed."); + exit(1); + } + } + + print_usage(); + exit(0); +} diff --git a/src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt b/src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt new file mode 100644 index 0000000000..9c5eb42eb6 --- /dev/null +++ b/src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt @@ -0,0 +1,29 @@ +The following license agreement covers re-used code from the arduino driver +for the Adafruit I2C to PWM converter. + +Software License Agreement (BSD License) + +Copyright (c) 2012, Adafruit Industries +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 of the copyright holders 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 ''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 HOLDER 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. diff --git a/src/drivers/adafruiti2cpwm/module.mk b/src/drivers/adafruiti2cpwm/module.mk new file mode 100644 index 0000000000..1f24591cfd --- /dev/null +++ b/src/drivers/adafruiti2cpwm/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012-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. +# +############################################################################ + +# +# Driver for the adafruit I2C PWM converter, +# which allow to control servos via I2C. +# https://www.adafruit.com/product/815 + +MODULE_COMMAND = adafruiti2cpwm + +SRCS = adafruiti2cpwm.cpp From 66991f9bb1493437a05ef4b4a704b43889a3413b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 20 Aug 2014 11:19:18 +0200 Subject: [PATCH 2/4] bring up adafruit i2c pwm driver includes the test function, fucntionality verified with scope and servo --- src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp | 271 ++++++++++++++++-- 1 file changed, 255 insertions(+), 16 deletions(-) diff --git a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp index 959b90db86..6fc4f21aa2 100644 --- a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp +++ b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp @@ -37,9 +37,10 @@ * Driver for the adafruit I2C PWM converter based on the PCA9685 * https://www.adafruit.com/product/815 * - * some code is adapted from the arduino library for the board + * Parts of the code are adapted from the arduino library for the board * https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library - * see the arduino_Adafruit_PWM_Servo_Driver_Library_license.txt file + * for the license of these parts see the + * arduino_Adafruit_PWM_Servo_Driver_Library_license.txt file * see https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library for contributors * * @author Thomas Gubler @@ -84,12 +85,25 @@ #define ALLLED_ON_L 0xFA #define ALLLED_ON_H 0xFB #define ALLLED_OFF_L 0xFC -#define ALLLED_OFF_H 0xFD +#define ALLLED_OF -#define ADDR PCA9685_SUBADR1 ///< I2C adress +#define ADDR 0x40 // I2C adress #define ADAFRUITI2CPWM_DEVICE_PATH "/dev/adafruiti2cpwm" #define ADAFRUITI2CPWM_BUS PX4_I2C_BUS_EXPANSION +#define ADAFRUITI2CPWM_PWMFREQ 60.0f + +#define ADAFRUITI2CPWM_SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096) +#define ADAFRUITI2CPWM_SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f + +#define ADAFRUITI2CPWM_CENTER (ADAFRUITI2CPWM_SERVOMAX - ADAFRUITI2CPWM_SERVOMIN)/2 +#define ADAFRUITI2CPWM_SCALE ADAFRUITI2CPWM_CENTER + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; class ADAFRUITI2CPWM : public device::I2C { @@ -100,6 +114,8 @@ public: 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; } private: @@ -110,10 +126,40 @@ private: bool _running; int _i2cpwm_interval; bool _should_run; + perf_counter_t _comms_errors; + + uint8_t _msg[6]; static void i2cpwm_trampoline(void *arg); void i2cpwm(); + /** + * Helper function to set the pwm frequency + */ + int setPWMFreq(float freq); + + /** + * Helper function to set the demanded pwm value + * @param num pwm output number + */ + int setPWM(uint8_t num, uint16_t on, uint16_t off); + + /** + * Sets pin without having to deal with on/off tick placement and properly handles + * a zero value as completely off. Optional invert parameter supports inverting + * the pulse for sinking to ground. + * @param num pwm output number + * @param val should be a value from 0 to 4095 inclusive. + */ + int setPin(uint8_t num, uint16_t val, bool invert = false); + + + /* Wrapper to read a byte from addr */ + int read8(uint8_t addr, uint8_t &value); + + /* Wrapper to wite a byte to addr */ + int write8(uint8_t addr, uint8_t value); + }; /* for now, we only support one board */ @@ -131,9 +177,11 @@ ADAFRUITI2CPWM::ADAFRUITI2CPWM(int bus, uint8_t address) : _mode(IOX_MODE_OFF), _running(false), _i2cpwm_interval(SEC2TICK(1.0f/60.0f)), - _should_run(false) + _should_run(false), + _comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")) { memset(&_work, 0, sizeof(_work)); + memset(_msg, 0, sizeof(_msg)); } ADAFRUITI2CPWM::~ADAFRUITI2CPWM() @@ -145,12 +193,18 @@ ADAFRUITI2CPWM::init() { int ret; ret = I2C::init(); - if (ret != OK) { return ret; } - return OK; + ret = reset(); + if (ret != OK) { + return ret; + } + + ret = setPWMFreq(ADAFRUITI2CPWM_PWMFREQ); + + return ret; } int @@ -165,8 +219,13 @@ ADAFRUITI2CPWM::ioctl(struct file *filp, int cmd, unsigned long 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: @@ -176,6 +235,13 @@ ADAFRUITI2CPWM::ioctl(struct file *filp, int cmd, unsigned long arg) _mode = (IOX_MODE)arg; } + // if not active, kick it + if (!_running) { + _running = true; + work_queue(LPWORK, &_work, (worker_t)&ADAFRUITI2CPWM::i2cpwm_trampoline, this, 1); + } + + return OK; default: @@ -187,6 +253,20 @@ ADAFRUITI2CPWM::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; } +int +ADAFRUITI2CPWM::info() +{ + int ret = OK; + + if (is_running()) { + warnx("Driver is running, mode: %u", _mode); + } else { + warnx("Driver started but not running"); + } + + return ret; +} + void ADAFRUITI2CPWM::i2cpwm_trampoline(void *arg) { @@ -201,17 +281,16 @@ ADAFRUITI2CPWM::i2cpwm_trampoline(void *arg) void ADAFRUITI2CPWM::i2cpwm() { - if (_mode == IOX_MODE_TEST_OUT) { - + setPin(0, ADAFRUITI2CPWM_CENTER); _should_run = true; } else if (_mode == IOX_MODE_OFF) { _should_run = false; } else { + _should_run = true; } - // check if any activity remains, else stop if (!_should_run) { _running = false; @@ -223,10 +302,152 @@ ADAFRUITI2CPWM::i2cpwm() work_queue(LPWORK, &_work, (worker_t)&ADAFRUITI2CPWM::i2cpwm_trampoline, this, _i2cpwm_interval); } -void -print_usage() +int +ADAFRUITI2CPWM::setPWM(uint8_t num, uint16_t on, uint16_t off) { - warnx("missing command: try 'start', 'test', 'stop'"); + int ret; + /* convert to correct message */ + _msg[0] = LED0_ON_L + 4 * num; + _msg[1] = on; + _msg[2] = on >> 8; + _msg[3] = off; + _msg[4] = off >> 8; + + /* try i2c transfer */ + ret = transfer(_msg, 5, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + } + + return ret; +} + +int +ADAFRUITI2CPWM::setPin(uint8_t num, uint16_t val, bool invert) +{ + // Clamp value between 0 and 4095 inclusive. + if (val > 4095) { + val = 4095; + } + if (invert) { + if (val == 0) { + // Special value for signal fully on. + return setPWM(num, 4096, 0); + } else if (val == 4095) { + // Special value for signal fully off. + return setPWM(num, 0, 4096); + } else { + return setPWM(num, 0, 4095-val); + } + } else { + if (val == 4095) { + // Special value for signal fully on. + return setPWM(num, 4096, 0); + } else if (val == 0) { + // Special value for signal fully off. + return setPWM(num, 0, 4096); + } else { + return setPWM(num, 0, val); + } + } + + return ERROR; +} + +int +ADAFRUITI2CPWM::setPWMFreq(float freq) +{ + int ret = OK; + freq *= 0.9f; /* Correct for overshoot in the frequency setting (see issue + https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11). */ + float prescaleval = 25000000; + prescaleval /= 4096; + prescaleval /= freq; + prescaleval -= 1; + uint8_t prescale = uint8_t(prescaleval + 0.5f); //implicit floor() + uint8_t oldmode; + ret = read8(PCA9685_MODE1, oldmode); + if (ret != OK) { + return ret; + } + uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep + + ret = write8(PCA9685_MODE1, newmode); // go to sleep + if (ret != OK) { + return ret; + } + ret = write8(PCA9685_PRESCALE, prescale); // set the prescaler + if (ret != OK) { + return ret; + } + ret = write8(PCA9685_MODE1, oldmode); + if (ret != OK) { + return ret; + } + + usleep(5000); //5ms delay (from arduino driver) + + ret = write8(PCA9685_MODE1, oldmode | 0xa1); // This sets the MODE1 register to turn on auto increment. + if (ret != OK) { + return ret; + } + + return ret; +} + +/* Wrapper to read a byte from addr */ +int +ADAFRUITI2CPWM::read8(uint8_t addr, uint8_t &value) +{ + int ret = OK; + + /* send addr */ + ret = transfer(&addr, sizeof(addr), nullptr, 0); + if (ret != OK) { + goto fail_read; + } + + /* get value */ + ret = transfer(nullptr, 0, &value, 1); + if (ret != OK) { + goto fail_read; + } + + return ret; + +fail_read: + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + + return ret; +} + +int ADAFRUITI2CPWM::reset(void) { + warnx("resetting"); + return write8(PCA9685_MODE1, 0x0); +} + +/* Wrapper to wite a byte to addr */ +int +ADAFRUITI2CPWM::write8(uint8_t addr, uint8_t value) { + int ret = OK; + _msg[0] = addr; + _msg[1] = value; + /* send addr and value */ + ret = transfer(_msg, 2, nullptr, 0); + if (ret != OK) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + } + return ret; +} + +void +adafruiti2cpwm_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); @@ -252,13 +473,13 @@ adafruiti2cpwm_main(int argc, char *argv[]) break; default: - print_usage(); + adafruiti2cpwm_usage(); exit(0); } } if (optind >= argc) { - print_usage(); + adafruiti2cpwm_usage(); exit(1); } @@ -300,6 +521,13 @@ adafruiti2cpwm_main(int argc, char *argv[]) errx(1, "init failed"); } } + fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); + if (fd == -1) { + errx(1, "Unable to open " ADAFRUITI2CPWM_DEVICE_PATH); + } + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON); + close(fd); + exit(0); } @@ -310,6 +538,17 @@ adafruiti2cpwm_main(int argc, char *argv[]) exit(1); } + if (!strcmp(verb, "info")) { + g_adafruiti2cpwm->info(); + exit(0); + } + + if (!strcmp(verb, "reset")) { + g_adafruiti2cpwm->reset(); + exit(0); + } + + if (!strcmp(verb, "test")) { fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); @@ -357,6 +596,6 @@ adafruiti2cpwm_main(int argc, char *argv[]) } } - print_usage(); + adafruiti2cpwm_usage(); exit(0); } From dd85c0407cbf552c7044d425d81ed346997e8572 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 20 Aug 2014 15:04:24 +0200 Subject: [PATCH 3/4] adafruit i2c pwm: listen to uorb for setpoints --- src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp | 52 +++++++++++++++++-- 1 file changed, 49 insertions(+), 3 deletions(-) diff --git a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp index 6fc4f21aa2..6d3359999c 100644 --- a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp +++ b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp @@ -59,6 +59,7 @@ #include #include #include +#include #include #include @@ -67,6 +68,9 @@ #include #include +#include +#include + #include #include @@ -92,12 +96,15 @@ #define ADAFRUITI2CPWM_DEVICE_PATH "/dev/adafruiti2cpwm" #define ADAFRUITI2CPWM_BUS PX4_I2C_BUS_EXPANSION #define ADAFRUITI2CPWM_PWMFREQ 60.0f +#define ADAFRUITI2CPWM_NCHANS 16 // total amount of pwm outputs #define ADAFRUITI2CPWM_SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096) #define ADAFRUITI2CPWM_SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f -#define ADAFRUITI2CPWM_CENTER (ADAFRUITI2CPWM_SERVOMAX - ADAFRUITI2CPWM_SERVOMIN)/2 -#define ADAFRUITI2CPWM_SCALE ADAFRUITI2CPWM_CENTER +#define ADAFRUITI2CPWM_HALFRANGE ((ADAFRUITI2CPWM_SERVOMAX - ADAFRUITI2CPWM_SERVOMIN)/2) +#define ADAFRUITI2CPWM_CENTER (ADAFRUITI2CPWM_SERVOMIN + ADAFRUITI2CPWM_HALFRANGE) +#define ADAFRUITI2CPWM_MAXSERVODEG 45 //maximal defelction in degrees +#define ADAFRUITI2CPWM_SCALE (ADAFRUITI2CPWM_HALFRANGE / (M_DEG_TO_RAD_F * ADAFRUITI2CPWM_MAXSERVODEG)) /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -130,6 +137,13 @@ private: uint8_t _msg[6]; + int _actuator_controls_sub; + struct actuator_controls_s _actuator_controls; + uint16_t _current_values[NUM_ACTUATOR_CONTROLS]; /**< stores the current pwm output + values as sent to the setPin() */ + + bool _mode_on_initialized; /** Set to true after the first call of i2cpwm in mode IOX_MODE_ON */ + static void i2cpwm_trampoline(void *arg); void i2cpwm(); @@ -178,10 +192,14 @@ ADAFRUITI2CPWM::ADAFRUITI2CPWM(int bus, uint8_t address) : _running(false), _i2cpwm_interval(SEC2TICK(1.0f/60.0f)), _should_run(false), - _comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")) + _comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")), + _actuator_controls_sub(-1), + _actuator_controls(), + _mode_on_initialized(false) { memset(&_work, 0, sizeof(_work)); memset(_msg, 0, sizeof(_msg)); + memset(_current_values, 0, sizeof(_current_values)); } ADAFRUITI2CPWM::~ADAFRUITI2CPWM() @@ -287,7 +305,35 @@ ADAFRUITI2CPWM::i2cpwm() } else if (_mode == IOX_MODE_OFF) { _should_run = false; } else { + if (!_mode_on_initialized) { + /* Subscribe to actuator control 2 (payload group for gimbal) */ + _actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2)); + /* set the uorb update interval lower than the driver pwm interval */ + orb_set_interval(_actuator_controls_sub, 1000.0f / ADAFRUITI2CPWM_PWMFREQ - 5); + _mode_on_initialized = true; + } + + /* Read the servo setpoints from the actuator control topics (gimbal) */ + bool updated; + orb_check(_actuator_controls_sub, &updated); + if (updated) { + orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls); + for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { + uint16_t new_value = ADAFRUITI2CPWM_CENTER + + (_actuator_controls.control[i] * ADAFRUITI2CPWM_SCALE); + debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, + (double)_actuator_controls.control[i]); + if (new_value != _current_values[i] && + isfinite(new_value) && + new_value >= 0 && + new_value <= ADAFRUITI2CPWM_SERVOMAX) { + /* This value was updated, send the command to adjust the PWM value */ + setPin(i, new_value); + _current_values[i] = new_value; + } + } + } _should_run = true; } From c414417cf8801f690582876cd723eca7273cbe6b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 20 Aug 2014 16:21:02 +0200 Subject: [PATCH 4/4] rename adafruiti2cpwm to pca9685 --- ...fruit_PWM_Servo_Driver_Library_license.txt | 0 .../{adafruiti2cpwm => pca9685}/module.mk | 9 +- .../pca9685.cpp} | 146 +++++++++--------- 3 files changed, 78 insertions(+), 77 deletions(-) rename src/drivers/{adafruiti2cpwm => pca9685}/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt (100%) rename src/drivers/{adafruiti2cpwm => pca9685}/module.mk (90%) rename src/drivers/{adafruiti2cpwm/adafruiti2cpwm.cpp => pca9685/pca9685.cpp} (76%) diff --git a/src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt b/src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt similarity index 100% rename from src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt rename to src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt diff --git a/src/drivers/adafruiti2cpwm/module.mk b/src/drivers/pca9685/module.mk similarity index 90% rename from src/drivers/adafruiti2cpwm/module.mk rename to src/drivers/pca9685/module.mk index 1f24591cfd..7a5c7996ec 100644 --- a/src/drivers/adafruiti2cpwm/module.mk +++ b/src/drivers/pca9685/module.mk @@ -32,10 +32,11 @@ ############################################################################ # -# Driver for the adafruit I2C PWM converter, -# which allow to control servos via I2C. +# Driver for the PCA9685 I2C PWM controller +# The chip is used on the adafruit I2C PWM converter, +# which allows to control servos via I2C. # https://www.adafruit.com/product/815 -MODULE_COMMAND = adafruiti2cpwm +MODULE_COMMAND = pca9685 -SRCS = adafruiti2cpwm.cpp +SRCS = pca9685.cpp diff --git a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp b/src/drivers/pca9685/pca9685.cpp similarity index 76% rename from src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp rename to src/drivers/pca9685/pca9685.cpp index 6d3359999c..c97d1e9abe 100644 --- a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp +++ b/src/drivers/pca9685/pca9685.cpp @@ -32,10 +32,10 @@ ****************************************************************************/ /** - * @file adafruiti2cpwm.cpp + * @file pca9685.cpp * - * Driver for the adafruit I2C PWM converter based on the PCA9685 - * https://www.adafruit.com/product/815 + * Driver for the PCA9685 I2C PWM module + * The chip is used on the Adafruit I2C/PWM converter https://www.adafruit.com/product/815 * * Parts of the code are adapted from the arduino library for the board * https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library @@ -93,18 +93,18 @@ #define ADDR 0x40 // I2C adress -#define ADAFRUITI2CPWM_DEVICE_PATH "/dev/adafruiti2cpwm" -#define ADAFRUITI2CPWM_BUS PX4_I2C_BUS_EXPANSION -#define ADAFRUITI2CPWM_PWMFREQ 60.0f -#define ADAFRUITI2CPWM_NCHANS 16 // total amount of pwm outputs +#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 -#define ADAFRUITI2CPWM_SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096) -#define ADAFRUITI2CPWM_SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f +#define PCA9685_SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096) +#define PCA9685_SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f -#define ADAFRUITI2CPWM_HALFRANGE ((ADAFRUITI2CPWM_SERVOMAX - ADAFRUITI2CPWM_SERVOMIN)/2) -#define ADAFRUITI2CPWM_CENTER (ADAFRUITI2CPWM_SERVOMIN + ADAFRUITI2CPWM_HALFRANGE) -#define ADAFRUITI2CPWM_MAXSERVODEG 45 //maximal defelction in degrees -#define ADAFRUITI2CPWM_SCALE (ADAFRUITI2CPWM_HALFRANGE / (M_DEG_TO_RAD_F * ADAFRUITI2CPWM_MAXSERVODEG)) +#define PCA9685_HALFRANGE ((PCA9685_SERVOMAX - PCA9685_SERVOMIN)/2) +#define PCA9685_CENTER (PCA9685_SERVOMIN + PCA9685_HALFRANGE) +#define PCA9685_MAXSERVODEG 45 //maximal defelction in degrees +#define PCA9685_SCALE (PCA9685_HALFRANGE / (M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -112,11 +112,11 @@ #endif static const int ERROR = -1; -class ADAFRUITI2CPWM : public device::I2C +class PCA9685 : public device::I2C { public: - ADAFRUITI2CPWM(int bus=ADAFRUITI2CPWM_BUS, uint8_t address=ADDR); - virtual ~ADAFRUITI2CPWM(); + PCA9685(int bus=PCA9685_BUS, uint8_t address=ADDR); + virtual ~PCA9685(); virtual int init(); @@ -179,15 +179,15 @@ private: /* for now, we only support one board */ namespace { -ADAFRUITI2CPWM *g_adafruiti2cpwm; +PCA9685 *g_pca9685; } -void adafruiti2cpwm_usage(); +void pca9685_usage(); -extern "C" __EXPORT int adafruiti2cpwm_main(int argc, char *argv[]); +extern "C" __EXPORT int pca9685_main(int argc, char *argv[]); -ADAFRUITI2CPWM::ADAFRUITI2CPWM(int bus, uint8_t address) : - I2C("adafruiti2cpwm", ADAFRUITI2CPWM_DEVICE_PATH, bus, address, 100000), +PCA9685::PCA9685(int bus, uint8_t address) : + I2C("pca9685", PCA9685_DEVICE_PATH, bus, address, 100000), _mode(IOX_MODE_OFF), _running(false), _i2cpwm_interval(SEC2TICK(1.0f/60.0f)), @@ -202,12 +202,12 @@ ADAFRUITI2CPWM::ADAFRUITI2CPWM(int bus, uint8_t address) : memset(_current_values, 0, sizeof(_current_values)); } -ADAFRUITI2CPWM::~ADAFRUITI2CPWM() +PCA9685::~PCA9685() { } int -ADAFRUITI2CPWM::init() +PCA9685::init() { int ret; ret = I2C::init(); @@ -220,13 +220,13 @@ ADAFRUITI2CPWM::init() return ret; } - ret = setPWMFreq(ADAFRUITI2CPWM_PWMFREQ); + ret = setPWMFreq(PCA9685_PWMFREQ); return ret; } int -ADAFRUITI2CPWM::ioctl(struct file *filp, int cmd, unsigned long arg) +PCA9685::ioctl(struct file *filp, int cmd, unsigned long arg) { int ret = -EINVAL; switch (cmd) { @@ -256,7 +256,7 @@ ADAFRUITI2CPWM::ioctl(struct file *filp, int cmd, unsigned long arg) // if not active, kick it if (!_running) { _running = true; - work_queue(LPWORK, &_work, (worker_t)&ADAFRUITI2CPWM::i2cpwm_trampoline, this, 1); + work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, 1); } @@ -272,7 +272,7 @@ ADAFRUITI2CPWM::ioctl(struct file *filp, int cmd, unsigned long arg) } int -ADAFRUITI2CPWM::info() +PCA9685::info() { int ret = OK; @@ -286,9 +286,9 @@ ADAFRUITI2CPWM::info() } void -ADAFRUITI2CPWM::i2cpwm_trampoline(void *arg) +PCA9685::i2cpwm_trampoline(void *arg) { - ADAFRUITI2CPWM *i2cpwm = reinterpret_cast(arg); + PCA9685 *i2cpwm = reinterpret_cast(arg); i2cpwm->i2cpwm(); } @@ -297,10 +297,10 @@ ADAFRUITI2CPWM::i2cpwm_trampoline(void *arg) * Main loop function */ void -ADAFRUITI2CPWM::i2cpwm() +PCA9685::i2cpwm() { if (_mode == IOX_MODE_TEST_OUT) { - setPin(0, ADAFRUITI2CPWM_CENTER); + setPin(0, PCA9685_CENTER); _should_run = true; } else if (_mode == IOX_MODE_OFF) { _should_run = false; @@ -309,7 +309,7 @@ ADAFRUITI2CPWM::i2cpwm() /* Subscribe to actuator control 2 (payload group for gimbal) */ _actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2)); /* set the uorb update interval lower than the driver pwm interval */ - orb_set_interval(_actuator_controls_sub, 1000.0f / ADAFRUITI2CPWM_PWMFREQ - 5); + orb_set_interval(_actuator_controls_sub, 1000.0f / PCA9685_PWMFREQ - 5); _mode_on_initialized = true; } @@ -320,14 +320,14 @@ ADAFRUITI2CPWM::i2cpwm() if (updated) { orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls); for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - uint16_t new_value = ADAFRUITI2CPWM_CENTER + - (_actuator_controls.control[i] * ADAFRUITI2CPWM_SCALE); + uint16_t new_value = PCA9685_CENTER + + (_actuator_controls.control[i] * PCA9685_SCALE); debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, (double)_actuator_controls.control[i]); if (new_value != _current_values[i] && isfinite(new_value) && new_value >= 0 && - new_value <= ADAFRUITI2CPWM_SERVOMAX) { + new_value <= PCA9685_SERVOMAX) { /* This value was updated, send the command to adjust the PWM value */ setPin(i, new_value); _current_values[i] = new_value; @@ -345,11 +345,11 @@ ADAFRUITI2CPWM::i2cpwm() // re-queue ourselves to run again later _running = true; - work_queue(LPWORK, &_work, (worker_t)&ADAFRUITI2CPWM::i2cpwm_trampoline, this, _i2cpwm_interval); + work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval); } int -ADAFRUITI2CPWM::setPWM(uint8_t num, uint16_t on, uint16_t off) +PCA9685::setPWM(uint8_t num, uint16_t on, uint16_t off) { int ret; /* convert to correct message */ @@ -371,7 +371,7 @@ ADAFRUITI2CPWM::setPWM(uint8_t num, uint16_t on, uint16_t off) } int -ADAFRUITI2CPWM::setPin(uint8_t num, uint16_t val, bool invert) +PCA9685::setPin(uint8_t num, uint16_t val, bool invert) { // Clamp value between 0 and 4095 inclusive. if (val > 4095) { @@ -403,7 +403,7 @@ ADAFRUITI2CPWM::setPin(uint8_t num, uint16_t val, bool invert) } int -ADAFRUITI2CPWM::setPWMFreq(float freq) +PCA9685::setPWMFreq(float freq) { int ret = OK; freq *= 0.9f; /* Correct for overshoot in the frequency setting (see issue @@ -445,7 +445,7 @@ ADAFRUITI2CPWM::setPWMFreq(float freq) /* Wrapper to read a byte from addr */ int -ADAFRUITI2CPWM::read8(uint8_t addr, uint8_t &value) +PCA9685::read8(uint8_t addr, uint8_t &value) { int ret = OK; @@ -470,14 +470,14 @@ fail_read: return ret; } -int ADAFRUITI2CPWM::reset(void) { +int PCA9685::reset(void) { warnx("resetting"); return write8(PCA9685_MODE1, 0x0); } /* Wrapper to wite a byte to addr */ int -ADAFRUITI2CPWM::write8(uint8_t addr, uint8_t value) { +PCA9685::write8(uint8_t addr, uint8_t value) { int ret = OK; _msg[0] = addr; _msg[1] = value; @@ -491,7 +491,7 @@ ADAFRUITI2CPWM::write8(uint8_t addr, uint8_t value) { } void -adafruiti2cpwm_usage() +pca9685_usage() { warnx("missing command: try 'start', 'test', 'stop', 'info'"); warnx("options:"); @@ -500,7 +500,7 @@ adafruiti2cpwm_usage() } int -adafruiti2cpwm_main(int argc, char *argv[]) +pca9685_main(int argc, char *argv[]) { int i2cdevice = -1; int i2caddr = ADDR; // 7bit @@ -519,13 +519,13 @@ adafruiti2cpwm_main(int argc, char *argv[]) break; default: - adafruiti2cpwm_usage(); + pca9685_usage(); exit(0); } } if (optind >= argc) { - adafruiti2cpwm_usage(); + pca9685_usage(); exit(1); } @@ -535,41 +535,41 @@ adafruiti2cpwm_main(int argc, char *argv[]) int ret; if (!strcmp(verb, "start")) { - if (g_adafruiti2cpwm != nullptr) { + if (g_pca9685 != nullptr) { errx(1, "already started"); } if (i2cdevice == -1) { // try the external bus first i2cdevice = PX4_I2C_BUS_EXPANSION; - g_adafruiti2cpwm = new ADAFRUITI2CPWM(PX4_I2C_BUS_EXPANSION, i2caddr); + g_pca9685 = new PCA9685(PX4_I2C_BUS_EXPANSION, i2caddr); - if (g_adafruiti2cpwm != nullptr && OK != g_adafruiti2cpwm->init()) { - delete g_adafruiti2cpwm; - g_adafruiti2cpwm = nullptr; + if (g_pca9685 != nullptr && OK != g_pca9685->init()) { + delete g_pca9685; + g_pca9685 = nullptr; } - if (g_adafruiti2cpwm == nullptr) { + if (g_pca9685 == nullptr) { errx(1, "init failed"); } } - if (g_adafruiti2cpwm == nullptr) { - g_adafruiti2cpwm = new ADAFRUITI2CPWM(i2cdevice, i2caddr); + if (g_pca9685 == nullptr) { + g_pca9685 = new PCA9685(i2cdevice, i2caddr); - if (g_adafruiti2cpwm == nullptr) { + if (g_pca9685 == nullptr) { errx(1, "new failed"); } - if (OK != g_adafruiti2cpwm->init()) { - delete g_adafruiti2cpwm; - g_adafruiti2cpwm = nullptr; + if (OK != g_pca9685->init()) { + delete g_pca9685; + g_pca9685 = nullptr; errx(1, "init failed"); } } - fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); + fd = open(PCA9685_DEVICE_PATH, 0); if (fd == -1) { - errx(1, "Unable to open " ADAFRUITI2CPWM_DEVICE_PATH); + errx(1, "Unable to open " PCA9685_DEVICE_PATH); } ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON); close(fd); @@ -579,27 +579,27 @@ adafruiti2cpwm_main(int argc, char *argv[]) } // need the driver past this point - if (g_adafruiti2cpwm == nullptr) { - warnx("not started, run adafruiti2cpwm start"); + if (g_pca9685 == nullptr) { + warnx("not started, run pca9685 start"); exit(1); } if (!strcmp(verb, "info")) { - g_adafruiti2cpwm->info(); + g_pca9685->info(); exit(0); } if (!strcmp(verb, "reset")) { - g_adafruiti2cpwm->reset(); + g_pca9685->reset(); exit(0); } if (!strcmp(verb, "test")) { - fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); + fd = open(PCA9685_DEVICE_PATH, 0); if (fd == -1) { - errx(1, "Unable to open " ADAFRUITI2CPWM_DEVICE_PATH); + errx(1, "Unable to open " PCA9685_DEVICE_PATH); } ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT); @@ -609,10 +609,10 @@ adafruiti2cpwm_main(int argc, char *argv[]) } if (!strcmp(verb, "stop")) { - fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); + fd = open(PCA9685_DEVICE_PATH, 0); if (fd == -1) { - errx(1, "Unable to open " ADAFRUITI2CPWM_DEVICE_PATH); + errx(1, "Unable to open " PCA9685_DEVICE_PATH); } ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); @@ -620,7 +620,7 @@ adafruiti2cpwm_main(int argc, char *argv[]) // wait until we're not running any more for (unsigned i = 0; i < 15; i++) { - if (!g_adafruiti2cpwm->is_running()) { + if (!g_pca9685->is_running()) { break; } @@ -631,9 +631,9 @@ adafruiti2cpwm_main(int argc, char *argv[]) printf("\n"); fflush(stdout); - if (!g_adafruiti2cpwm->is_running()) { - delete g_adafruiti2cpwm; - g_adafruiti2cpwm= nullptr; + if (!g_pca9685->is_running()) { + delete g_pca9685; + g_pca9685= nullptr; warnx("stopped, exiting"); exit(0); } else { @@ -642,6 +642,6 @@ adafruiti2cpwm_main(int argc, char *argv[]) } } - adafruiti2cpwm_usage(); + pca9685_usage(); exit(0); }