forked from Archive/PX4-Autopilot
Merge pull request #1296 from PX4/adafruiti2cpwm
Adafruit i2c to pwm board driver
This commit is contained in:
Lorenz Meier
commit
24b1ff23f2
|
|
@ -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.
|
||||||
|
|
@ -0,0 +1,42 @@
|
||||||
|
############################################################################
|
||||||
|
#
|
||||||
|
# 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 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 = pca9685
|
||||||
|
|
||||||
|
SRCS = pca9685.cpp
|
||||||
|
|
@ -0,0 +1,647 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* 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 pca9685.cpp
|
||||||
|
*
|
||||||
|
* 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
|
||||||
|
* 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 <thomasgubler@gmail.com>
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <nuttx/config.h>
|
||||||
|
|
||||||
|
#include <drivers/device/i2c.h>
|
||||||
|
|
||||||
|
#include <sys/types.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <ctype.h>
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
#include <nuttx/wqueue.h>
|
||||||
|
#include <nuttx/clock.h>
|
||||||
|
|
||||||
|
#include <systemlib/perf_counter.h>
|
||||||
|
#include <systemlib/err.h>
|
||||||
|
#include <systemlib/systemlib.h>
|
||||||
|
|
||||||
|
#include <uORB/uORB.h>
|
||||||
|
#include <uORB/topics/actuator_controls.h>
|
||||||
|
|
||||||
|
#include <board_config.h>
|
||||||
|
#include <drivers/drv_io_expander.h>
|
||||||
|
|
||||||
|
#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_OF
|
||||||
|
|
||||||
|
#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
|
||||||
|
|
||||||
|
#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 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
|
||||||
|
# undef ERROR
|
||||||
|
#endif
|
||||||
|
static const int ERROR = -1;
|
||||||
|
|
||||||
|
class PCA9685 : public device::I2C
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
PCA9685(int bus=PCA9685_BUS, uint8_t address=ADDR);
|
||||||
|
virtual ~PCA9685();
|
||||||
|
|
||||||
|
|
||||||
|
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:
|
||||||
|
work_s _work;
|
||||||
|
|
||||||
|
|
||||||
|
enum IOX_MODE _mode;
|
||||||
|
bool _running;
|
||||||
|
int _i2cpwm_interval;
|
||||||
|
bool _should_run;
|
||||||
|
perf_counter_t _comms_errors;
|
||||||
|
|
||||||
|
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();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* 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 */
|
||||||
|
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),
|
||||||
|
_mode(IOX_MODE_OFF),
|
||||||
|
_running(false),
|
||||||
|
_i2cpwm_interval(SEC2TICK(1.0f/60.0f)),
|
||||||
|
_should_run(false),
|
||||||
|
_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));
|
||||||
|
}
|
||||||
|
|
||||||
|
PCA9685::~PCA9685()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
PCA9685::init()
|
||||||
|
{
|
||||||
|
int ret;
|
||||||
|
ret = I2C::init();
|
||||||
|
if (ret != OK) {
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
ret = reset();
|
||||||
|
if (ret != OK) {
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
return OK;
|
||||||
|
|
||||||
|
default:
|
||||||
|
// see if the parent class can make any use of it
|
||||||
|
ret = CDev::ioctl(filp, cmd, arg);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
PCA9685::i2cpwm_trampoline(void *arg)
|
||||||
|
{
|
||||||
|
PCA9685 *i2cpwm = reinterpret_cast<PCA9685 *>(arg);
|
||||||
|
|
||||||
|
i2cpwm->i2cpwm();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Main loop function
|
||||||
|
*/
|
||||||
|
void
|
||||||
|
PCA9685::i2cpwm()
|
||||||
|
{
|
||||||
|
if (_mode == IOX_MODE_TEST_OUT) {
|
||||||
|
setPin(0, PCA9685_CENTER);
|
||||||
|
_should_run = true;
|
||||||
|
} 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 / PCA9685_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 = 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 <= PCA9685_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;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval);
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
PCA9685::setPWM(uint8_t num, uint16_t on, uint16_t off)
|
||||||
|
{
|
||||||
|
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
|
||||||
|
PCA9685::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
|
||||||
|
PCA9685::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
|
||||||
|
PCA9685::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 PCA9685::reset(void) {
|
||||||
|
warnx("resetting");
|
||||||
|
return write8(PCA9685_MODE1, 0x0);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Wrapper to wite a byte to addr */
|
||||||
|
int
|
||||||
|
PCA9685::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
|
||||||
|
pca9685_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);
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
pca9685_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:
|
||||||
|
pca9685_usage();
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (optind >= argc) {
|
||||||
|
pca9685_usage();
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
const char *verb = argv[optind];
|
||||||
|
|
||||||
|
int fd;
|
||||||
|
int ret;
|
||||||
|
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pca9685_usage();
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue