diff --git a/src/drivers/rpi_pca9685_pwm_out/CMakeLists.txt b/src/drivers/rpi_pca9685_pwm_out/CMakeLists.txt new file mode 100644 index 0000000000..c800fc73cf --- /dev/null +++ b/src/drivers/rpi_pca9685_pwm_out/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015-2016 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. +# +############################################################################ +px4_add_module( + MODULE drivers__rpi_pca9685_pwm_out + MAIN rpi_pca9685_pwm_out + COMPILE_FLAGS + SRCS + PCA9685.cpp + rpi_pca9685_pwm_out.cpp + DEPENDS + platforms__common +) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/rpi_pca9685_pwm_out/PCA9685.cpp b/src/drivers/rpi_pca9685_pwm_out/PCA9685.cpp new file mode 100644 index 0000000000..851c27853b --- /dev/null +++ b/src/drivers/rpi_pca9685_pwm_out/PCA9685.cpp @@ -0,0 +1,204 @@ +/* + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + * Name : PCA9685.cpp + * Original Author : Georgi Todorov + * Edited by : Tord Wessman + * Version : + * Created on : Dec 9, 2012 + * + * Copyright © 2012 Georgi Todorov + */ + +#include +#include +#include +#include +#include /* Standard I/O functions */ +#include +#include /* Syslog functionallity */ +#include +#include +#include +#include +#include +// + +#include "PCA9685.h" + +//! Constructor takes bus and address arguments +/*! + \param bus the bus to use in /dev/i2c-%d. + \param address the device address on bus + */ + +void PCA9685::init(int bus, int address) +{ + _i2cbus = bus; + _i2caddr = address; + snprintf(busfile, sizeof(busfile), "/dev/i2c-%d", bus); + reset(); + //usleep(10*1000); +} + +PCA9685::PCA9685() : + _i2caddr(0x40), + _i2cbus(1), + busfile {}, + dataBuffer {} +{ + +} + +PCA9685::~PCA9685() +{ + reset(); +} +//! Sets PCA9685 mode to 00 +void PCA9685::reset() +{ + int fd = openfd(); + + if (fd != -1) { + write_byte(fd, MODE1, 0x00); //Normal mode + write_byte(fd, MODE2, 0x04); //Normal mode + close(fd); + } +} +//! Set the frequency of PWM +/*! + \param freq desired frequency. 40Hz to 1000Hz using internal 25MHz oscillator. + */ +void PCA9685::setPWMFreq(int freq) +{ + int fd = openfd(); + + if (fd != -1) { + uint8_t prescale = (CLOCK_FREQ / 4096 / freq) - 1; + //printf ("Setting prescale value to: %d\n", prescale); + //printf ("Using Frequency: %d\n", freq); + + uint8_t oldmode = read_byte(fd, MODE1); + uint8_t newmode = (oldmode & 0x7F) | 0x10; //sleep + write_byte(fd, MODE1, newmode); // go to sleep + write_byte(fd, PRE_SCALE, prescale); + write_byte(fd, MODE1, oldmode); + usleep(10 * 1000); + write_byte(fd, MODE1, oldmode | 0x80); + + close(fd); + } +} + +//! PWM a single channel +/*! + \param led channel to set PWM value for + \param value 0-4095 value for PWM + */ +void PCA9685::setPWM(uint8_t led, int value) +{ + setPWM(led, 0, value); +} +//! PWM a single channel with custom on time +/*! + \param led channel to set PWM value for + \param on_value 0-4095 value to turn on the pulse + \param off_value 0-4095 value to turn off the pulse + */ +void PCA9685::setPWM(uint8_t led, int on_value, int off_value) +{ + int fd = openfd(); + + if (fd != -1) { + + write_byte(fd, LED0_ON_L + LED_MULTIPLYER * led, on_value & 0xFF); + + write_byte(fd, LED0_ON_H + LED_MULTIPLYER * led, on_value >> 8); + + write_byte(fd, LED0_OFF_L + LED_MULTIPLYER * led, off_value & 0xFF); + + write_byte(fd, LED0_OFF_H + LED_MULTIPLYER * led, off_value >> 8); + + close(fd); + } + +} + +//! Read a single byte from PCA9685 +/*! + \param fd file descriptor for I/O + \param address register address to read from + */ +uint8_t PCA9685::read_byte(int fd, uint8_t address) +{ + + return 0; + + uint8_t buff[BUFFER_SIZE]; + buff[0] = address; + + if (write(fd, buff, BUFFER_SIZE) != BUFFER_SIZE) { + printf("I2C slave 0x%x failed to go to register 0x%x [read_byte():write %d]", _i2caddr, address, errno); + return (-1); + + } else { + if (read(fd, dataBuffer, BUFFER_SIZE) != BUFFER_SIZE) { + printf("Could not read from I2C slave 0x%x, register 0x%x [read_byte():read %d]", _i2caddr, address, errno); + return (-1); + } + } + + +} +//! Write a single byte from PCA9685 +/*! + \param fd file descriptor for I/O + \param address register address to write to + \param data 8 bit data to write + */ +void PCA9685::write_byte(int fd, uint8_t address, uint8_t data) +{ + uint8_t buff[2]; + buff[0] = address; + buff[1] = data; + + if (write(fd, buff, sizeof(buff)) != 2) { + printf("Failed to write to I2C Slave 0x%x @ register 0x%x [write_byte():write %d]", _i2caddr, address, errno); + usleep(5000); + + } else { + //printf("Wrote to I2C Slave 0x%x @ register 0x%x [0x%x]\n", _i2caddr, address, data); + } +} +//! Open device file for PCA9685 I2C bus +/*! + \return fd returns the file descriptor number or -1 on error + */ +int PCA9685::openfd() +{ + int fd; + + if ((fd = open(busfile, O_RDWR)) < 0) { + printf("Couldn't open I2C Bus %d [openfd():open %d]", _i2cbus, errno); + return -1; + } + + if (ioctl(fd, I2C_SLAVE, _i2caddr) < 0) { + printf("I2C slave %d failed [openfd():ioctl %d]", _i2caddr, errno); + return -1; + } + + return fd; +} diff --git a/src/drivers/rpi_pca9685_pwm_out/PCA9685.h b/src/drivers/rpi_pca9685_pwm_out/PCA9685.h new file mode 100644 index 0000000000..ed1b78153c --- /dev/null +++ b/src/drivers/rpi_pca9685_pwm_out/PCA9685.h @@ -0,0 +1,73 @@ +/* + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + * Name : PCA9685.h + * Original Author : Georgi Todorov + * Edited by : Tord Wessman + * Version : + * Created on : Nov 22, 2013 + * + * Copyright © 2012 Georgi Todorov + */ + +#ifndef _PCA9685_H +#define _PCA9685_H +#include + +// Register Definitions + +#define MODE1 0x00 //Mode register 1 +#define MODE2 0x01 //Mode register 2 +#define SUBADR1 0x02 //I2C-bus subaddress 1 +#define SUBADR2 0x03 //I2C-bus subaddress 2 +#define SUBADR3 0x04 //I2C-bus subaddress 3 +#define ALLCALLADR 0x05 //LED All Call I2C-bus address +#define LED0 0x6 //LED0 start register +#define LED0_ON_L 0x6 //LED0 output and brightness control byte 0 +#define LED0_ON_H 0x7 //LED0 output and brightness control byte 1 +#define LED0_OFF_L 0x8 //LED0 output and brightness control byte 2 +#define LED0_OFF_H 0x9 //LED0 output and brightness control byte 3 +#define LED_MULTIPLYER 4 // For the other 15 channels +#define ALLLED_ON_L 0xFA //load all the LEDn_ON registers, byte 0 (turn 0-7 channels on) +#define ALLLED_ON_H 0xFB //load all the LEDn_ON registers, byte 1 (turn 8-15 channels on) +#define ALLLED_OFF_L 0xFC //load all the LEDn_OFF registers, byte 0 (turn 0-7 channels off) +#define ALLLED_OFF_H 0xFD //load all the LEDn_OFF registers, byte 1 (turn 8-15 channels off) +#define PRE_SCALE 0xFE //prescaler for output frequency +#define CLOCK_FREQ 25000000.0 //25MHz default osc clock +#define BUFFER_SIZE 0x08 //1 byte buffer +//! Main class that exports features for PCA9685 chip +class PCA9685 +{ +public: + + PCA9685(); + void init(int, int); + virtual ~PCA9685(); + void reset(void); + void setPWMFreq(int); + void setPWM(uint8_t, int, int); + void setPWM(uint8_t, int); +private: + int _i2caddr; + int _i2cbus; + char busfile[64]; + uint8_t dataBuffer[BUFFER_SIZE]; + uint8_t read_byte(int, uint8_t); + void write_byte(int, uint8_t, uint8_t); + int openfd(); + + +}; +#endif diff --git a/src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.cpp b/src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.cpp new file mode 100644 index 0000000000..98172bf67a --- /dev/null +++ b/src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.cpp @@ -0,0 +1,382 @@ +/**************************************************************************** + * + * Copyright (c) 2015-2016 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. + * + ****************************************************************************/ +#include "rpi_pca9685_pwm_out.h" + +using namespace rpi_pca9685_pwm_out; +//--------------------------------------------------------------------------// +int rpi_pca9685_pwm_out::mixer_control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + const actuator_controls_s *controls = (actuator_controls_s *)handle; + input = controls[control_group].control[control_index]; + + return 0; +} + +//----------------------------------------------------------------------------// +int rpi_pca9685_pwm_out::initialize_mixer(const char *mixer_filename) +{ + char buf[4096]; + unsigned buflen = sizeof(buf); + memset(buf, '\0', buflen); + + _mixer_group = new MixerGroup(mixer_control_callback, (uintptr_t) &_controls); + + // PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename); + + if (load_mixer_file(mixer_filename, buf, buflen) == 0) { + if (_mixer_group->load_from_buf(buf, buflen) == 0) { + PX4_INFO("Loaded mixer from file %s", mixer_filename); + return 0; + + } else { + PX4_ERR("Unable to parse from mixer config file %s", mixer_filename); + } + + } else { + PX4_ERR("Unable to load config file %s", mixer_filename); + } + + if (_mixer_group->count() <= 0) { + PX4_ERR("Mixer initialization failed"); + return -1; + } + + return 0; +} +//----------------------------------------------------------------------------// +int rpi_pca9685_pwm_out::pwm_initialize() +{ + /**************初始化PCA9685开始*************/ + pwm.init(1, 0x40); + usleep(1000 * 100); + pwm.setPWMFreq(61); + usleep(1000 * 1000); + /**************初始化PCA9685结束************/ + return 0; +} +//----------------------------------------------------------------------------// +void rpi_pca9685_pwm_out::pwm_deinitialize() +{ + rpi_pca9685_pwm_out::pwm.reset(); +} +//----------------------------------------------------------------------------// +void rpi_pca9685_pwm_out::send_outputs_pwm(const uint16_t *pwm) +{ + //向PCA9685发送数据 + int i; + + for (i = 0; i < NUM_PWM; ++i) { + rpi_pca9685_pwm_out::pwm.setPWM(i, *(pwm + i)); + } +} +//----------------------------------------------------------------------------// +void rpi_pca9685_pwm_out::subscribe() +{ + memset(_controls, 0, sizeof(_controls)); + memset(_poll_fds, 0, sizeof(_poll_fds)); + + /* set up ORB topic names */ + _controls_topics[0] = ORB_ID(actuator_controls_0); + _controls_topics[1] = ORB_ID(actuator_controls_1); + _controls_topics[2] = ORB_ID(actuator_controls_2); + _controls_topics[3] = ORB_ID(actuator_controls_3); + + // Subscribe for orb topics + for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_groups_required & (1 << i)) { + PX4_DEBUG("subscribe to actuator_controls_%d", i); + _controls_subs[i] = orb_subscribe(_controls_topics[i]); + + } else { + _controls_subs[i] = -1; + } + + if (_controls_subs[i] >= 0) { + _poll_fds[_poll_fds_num].fd = _controls_subs[i]; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num++; + } + + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + + } +} +//----------------------------------------------------------------------------// +void rpi_pca9685_pwm_out::task_main(int argc, char *argv[]) +{ + _is_running = true; + + /***************初始化PCA9685************/ + rpi_pca9685_pwm_out::pwm_initialize(); + + // Set up mixer + if (initialize_mixer(_mixer_filename) < 0) { + PX4_ERR("无法初始化通道混合配置文件"); + return; + } + + _mixer_group->groups_required(_groups_required); + // subscribe and set up polling + subscribe(); + + // Start disarmed + _armed.armed = false; + _armed.prearmed = false; + + pwm_limit_init(&_pwm_limit); + + // Main loop + while (!_task_should_exit) { + int pret = px4_poll(_poll_fds, _poll_fds_num, 10); + + /* Timed out, do a periodic check for _task_should_exit. */ + if (pret == 0) { + continue; + } + + /* This is undesirable but not much we can do. */ + if (pret < 0) { + PX4_WARN("poll error %d, %d", pret, errno); + /* sleep a bit before next try */ + usleep(10000); + continue; + } + + /* get controls for required topics */ + unsigned poll_id = 0; + + for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_controls_subs[i] >= 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + orb_copy(_controls_topics[i], _controls_subs[i], &_controls[i]); + } + + poll_id++; + } + } + + if (_mixer_group != nullptr) { + _outputs.timestamp = hrt_absolute_time(); + /* do mixing */ + _outputs.noutputs = _mixer_group->mix(_outputs.output, + _outputs.NUM_ACTUATOR_OUTPUTS, + NULL); + + /* disable unused ports by setting their output to NaN */ + for (size_t i = _outputs.noutputs; + i < _outputs.NUM_ACTUATOR_OUTPUTS; + i++) { + _outputs.output[i] = NAN; + } + + const uint16_t reverse_mask = 0; + uint16_t disarmed_pwm[4]; + uint16_t min_pwm[4]; + uint16_t max_pwm[4]; + + for (unsigned int i = 0; i < NUM_PWM; i++) { + disarmed_pwm[i] = _pwm_disarmed; + min_pwm[i] = _pwm_min; + max_pwm[i] = _pwm_max; + } + + uint16_t pwm[4]; + + // TODO FIXME: pre-armed seems broken + pwm_limit_calc(_armed.armed, + false/*_armed.prearmed*/, + _outputs.noutputs, + reverse_mask, + disarmed_pwm, + min_pwm, + max_pwm, + _outputs.output, + pwm, + &_pwm_limit); + + if (_armed.lockdown) { + send_outputs_pwm(disarmed_pwm); + + } else { + send_outputs_pwm(pwm); + } + + if (_outputs_pub != nullptr) { + orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs); + + } else { + _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs); + } + + } else { + PX4_ERR("Could not mix output! Exiting..."); + _task_should_exit = true; + } + + bool updated; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + } + } + + pwm_deinitialize(); + + for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_controls_subs[i] >= 0) { + orb_unsubscribe(_controls_subs[i]); + } + } + + orb_unsubscribe(_armed_sub); + + _is_running = false; + +} +//----------------------------------------------------------------------------// +void rpi_pca9685_pwm_out::task_main_trampoline(int argc, char *argv[]) +{ + task_main(argc, argv); +} +//----------------------------------------------------------------------------// +void rpi_pca9685_pwm_out::start() +{ + ASSERT(_task_handle == -1); + _task_should_exit = false; + + /* start the task */ + _task_handle = px4_task_spawn_cmd("pwm_out_main", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 1500, + (px4_main_t)&task_main_trampoline, + nullptr); + + if (_task_handle < 0) { + warn("task start failed"); + return; + } + +} +//----------------------------------------------------------------------------// +void rpi_pca9685_pwm_out::stop() +{ + _task_should_exit = true; + + while (_is_running) { + usleep(200000); + PX4_INFO("."); + } + + _task_handle = -1; +} +//----------------------------------------------------------------------------// +void rpi_pca9685_pwm_out::usage() +{ + PX4_INFO("usage: pwm_out start [-d pwmdevice] -m mixerfile"); + PX4_INFO(" -d pwmdevice : sysfs device for pwm generation"); + PX4_INFO(" (default /sys/class/pwm/pwmchip0)"); + PX4_INFO(" -m mixerfile : path to mixerfile"); + PX4_INFO(" (default ROMFS/px4fmu_common/mixers/quad_x.main.mix)"); + PX4_INFO(" pwm_out stop"); + PX4_INFO(" pwm_out status"); +} +//----------------------------------------------------------------------------// +int rpi_pca9685_pwm_out_main(int argc, char **argv) +{ + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + char *verb = nullptr; + + if (argc >= 2) { + verb = argv[1]; + + } else { + return 1; + } + + while ((ch = px4_getopt(argc, argv, "d:m:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + strncpy(rpi_pca9685_pwm_out::_device, myoptarg, sizeof(rpi_pca9685_pwm_out::_device)); + break; + + case 'm': + strncpy(rpi_pca9685_pwm_out::_mixer_filename, myoptarg, sizeof(rpi_pca9685_pwm_out::_mixer_filename)); + break; + } + } + + // gets the parameters for the esc's pwm + param_get(param_find("PWM_DISARMED"), &rpi_pca9685_pwm_out::_pwm_disarmed); + param_get(param_find("PWM_MIN"), &rpi_pca9685_pwm_out::_pwm_min); + param_get(param_find("PWM_MAX"), &rpi_pca9685_pwm_out::_pwm_max); + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + if (rpi_pca9685_pwm_out::_is_running) { + PX4_WARN("pwm_out already running"); + return 1; + } + + rpi_pca9685_pwm_out::start(); + } + + else if (!strcmp(verb, "stop")) { + if (!rpi_pca9685_pwm_out::_is_running) { + PX4_WARN("pwm_out is not running"); + return 1; + } + + rpi_pca9685_pwm_out::stop(); + } + + else if (!strcmp(verb, "status")) { + PX4_WARN("pwm_out is %s", rpi_pca9685_pwm_out::_is_running ? "running" : "not running"); + return 0; + + } else { + rpi_pca9685_pwm_out::usage(); + return 1; + } + + return 0; +} diff --git a/src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.h b/src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.h new file mode 100644 index 0000000000..aadd773f62 --- /dev/null +++ b/src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.h @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * Copyright (c) 2015-2016 zhangfan. 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. + * + ****************************************************************************/ +#include + +#include +#include +#include +#include +#include // NAN + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "PCA9685.h" + +namespace rpi_pca9685_pwm_out +{ +static px4_task_t _task_handle = -1; +volatile bool _task_should_exit = false; +static bool _is_running = false; + +static const int NUM_PWM = 4; +static char _device[64] = "/sys/class/pwm/pwmchip0"; +//static int _pwm_fd[NUM_PWM]; + +static const int FREQUENCY_PWM = 400; +static char _mixer_filename[64] = "ROMFS/px4fmu_common/mixers/quad_x.main.mix"; + +// subscriptions +int _controls_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; +int _armed_sub; + +// publications +orb_advert_t _outputs_pub = nullptr; +orb_advert_t _rc_pub = nullptr; + +// topic structures +actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; +orb_id_t _controls_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; +actuator_outputs_s _outputs; +actuator_armed_s _armed; + +// polling +uint8_t _poll_fds_num = 0; +px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + +// control groups related +uint32_t _groups_required = 0; +uint32_t _groups_subscribed = 0; + +pwm_limit_t _pwm_limit; + +// esc parameters +int32_t _pwm_disarmed; +int32_t _pwm_min; +int32_t _pwm_max; + +//PCA9685 device +::PCA9685 pwm; + +MixerGroup *_mixer_group = nullptr; + +void usage(); + +void start(); + +void stop(); + +int pwm_initialize(); + +void pwm_deinitialize(); + +void send_outputs_pwm(const uint16_t *pwm); + +void task_main_trampoline(int argc, char *argv[]); + +void subscribe(); + +void task_main(int argc, char *argv[]); + +/* mixer initialization */ +int initialize_mixer(const char *mixer_filename); +int mixer_control_callback(uintptr_t handle, uint8_t control_group, + uint8_t control_index, float &input); +} +extern "C" __EXPORT int rpi_pca9685_pwm_out_main(int, char **);