From 243ae00e4e8ff7c074885a4216a2a8ad99a07259 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 8 May 2017 13:09:15 +0200 Subject: [PATCH] rpi_pca9685_pwm_out: remove this driver, it's now in rpi_pwm_out --- cmake/configs/posix_rpi_common.cmake | 1 - posix-configs/rpi/px4_no_shield.config | 2 +- .../rpi_pca9685_pwm_out/CMakeLists.txt | 43 -- src/drivers/rpi_pca9685_pwm_out/PCA9685.cpp | 172 -------- src/drivers/rpi_pca9685_pwm_out/PCA9685.h | 132 ------ .../rpi_pca9685_pwm_out.cpp | 389 ------------------ .../rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.h | 127 ------ 7 files changed, 1 insertion(+), 865 deletions(-) delete mode 100644 src/drivers/rpi_pca9685_pwm_out/CMakeLists.txt delete mode 100644 src/drivers/rpi_pca9685_pwm_out/PCA9685.cpp delete mode 100644 src/drivers/rpi_pca9685_pwm_out/PCA9685.h delete mode 100644 src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.cpp delete mode 100644 src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.h diff --git a/cmake/configs/posix_rpi_common.cmake b/cmake/configs/posix_rpi_common.cmake index 84ce025caa..624d40c1aa 100644 --- a/cmake/configs/posix_rpi_common.cmake +++ b/cmake/configs/posix_rpi_common.cmake @@ -82,7 +82,6 @@ set(config_module_list drivers/navio_rgbled drivers/pwm_out_sim drivers/rpi_rc_in - drivers/rpi_pca9685_pwm_out # # Libraries diff --git a/posix-configs/rpi/px4_no_shield.config b/posix-configs/rpi/px4_no_shield.config index ae75b1ed14..a1fd476e5c 100644 --- a/posix-configs/rpi/px4_no_shield.config +++ b/posix-configs/rpi/px4_no_shield.config @@ -22,6 +22,6 @@ mavlink stream -u 14556 -s ATTITUDE -r 50 #mavlink stream -d /dev/ttyUSB0 -s HIGHRES_IMU -r 50 #mavlink stream -d /dev/ttyUSB0 -s ATTITUDE -r 50 rpi_rc_in start -rpi_pca9685_pwm_out start +rpi_pwm_out start -p pca9685 logger start -t mavlink boot_complete diff --git a/src/drivers/rpi_pca9685_pwm_out/CMakeLists.txt b/src/drivers/rpi_pca9685_pwm_out/CMakeLists.txt deleted file mode 100644 index ecf2807cc2..0000000000 --- a/src/drivers/rpi_pca9685_pwm_out/CMakeLists.txt +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# 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 deleted file mode 100644 index 3cbaf1562e..0000000000 --- a/src/drivers/rpi_pca9685_pwm_out/PCA9685.cpp +++ /dev/null @@ -1,172 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 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. - * - * - * Original Author : Georgi Todorov - * Edited by : Tord Wessman - * Created on : Nov 22, 2013 - * Rewrite : Fan.zhang 421395590@qq.com - * Updated on : Mar 2, 2017 - ****************************************************************************/ - -#include -#include -#include -#include -#include /* Standard I/O functions */ -#include -#include -#include -#include -#include -#include - -#include "PCA9685.h" - -#include - -void PCA9685::init(int bus, int address) -{ - _fd = open_fd(bus, address); - reset(); -} - - -PCA9685::PCA9685() -{ - init(PCA9685_DEFAULT_I2C_BUS, PCA9685_DEFAULT_I2C_ADDR); -} - -PCA9685::PCA9685(int bus, int address) -{ - init(bus, address); -} - -PCA9685::~PCA9685() -{ - reset(); - - if (_fd >= 0) { - close(_fd); - } -} - -void PCA9685::reset() -{ - if (_fd != -1) { - write_byte(_fd, MODE1, 0x00); //Normal mode - write_byte(_fd, MODE2, 0x04); //Normal mode - } -} - -void PCA9685::setPWMFreq(int freq) -{ - if (_fd != -1) { - uint8_t prescale = (CLOCK_FREQ / MAX_PWM_RES / 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); - } -} - - -void PCA9685::setPWM(uint8_t led, int value) -{ - setPWM(led, 0, value); -} - -void PCA9685::setPWM(uint8_t led, int on_value, int off_value) -{ - 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); - } - -} - -uint8_t PCA9685::read_byte(int fd, uint8_t address) -{ - - uint8_t buf[1]; - buf[0] = address; - - if (write(fd, buf, 1) != 1) { - return -1; - } - - if (read(fd, buf, 1) != 1) { - return -1; - } - - return buf[0]; -} - -void PCA9685::write_byte(int fd, uint8_t address, uint8_t data) -{ - uint8_t buf[2]; - buf[0] = address; - buf[1] = data; - - if (write(fd, buf, sizeof(buf)) != sizeof(buf)) { - PX4_ERR("Write failed (%i)", errno); - } -} - -int PCA9685::open_fd(int bus, int address) -{ - int fd; - char bus_file[64]; - snprintf(bus_file, sizeof(bus_file), "/dev/i2c-%d", bus); - - if ((fd = open(bus_file, O_RDWR)) < 0) { - PX4_ERR("Couldn't open I2C Bus %d [open_fd():open %d]", bus, errno); - return -1; - } - - if (ioctl(fd, I2C_SLAVE, address) < 0) { - PX4_ERR("I2C slave %d failed [open_fd():ioctl %d]", address, 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 deleted file mode 100644 index fd915f3143..0000000000 --- a/src/drivers/rpi_pca9685_pwm_out/PCA9685.h +++ /dev/null @@ -1,132 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 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. - * - * - * Original Author : Georgi Todorov - * Edited by : Tord Wessman - * Created on : Nov 22, 2013 - * Rewrite : Fan.zhang 421395590@qq.com - * Updated on : Mar 2, 2017 - ****************************************************************************/ - -#pragma once -#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 MAX_PWM_RES 4096 //Resolution 4096=12bit 分辨率，按2的阶乘计算，12bit为4096 -#define CLOCK_FREQ 25000000.0 //25MHz default osc clock -#define PCA9685_DEFAULT_I2C_ADDR 0x40 // default i2c address for pca9685 默认i2c地址为0x40 -#define PCA9685_DEFAULT_I2C_BUS 1 // default i2c bus for pca9685 默认为1 - -//! Main class that exports features for PCA9685 chip -class PCA9685 -{ -public: - - PCA9685(); - - /** - * Constructor takes bus and address arguments - * @param bus the bus to use in /dev/i2c-%d. - * @param address the device address on bus - */ - PCA9685(int bus, int address); - - void init(int bus, int address); - ~PCA9685(); - - /// Sets PCA9685 mode to 00 - void reset(); - - /** - * Set the frequency of PWM - * @param freq desired frequency. 40Hz to 1000Hz using internal 25MHz oscillator. - */ - void setPWMFreq(int freq); - - /** - * PWM a single channel with custom on time. - * send pwm vale to led(channel) - * @param channel - * @param on_value 0-4095 value to turn on the pulse - * @param off_value 0-4095 value to turn off the pulse - */ - void setPWM(uint8_t channel, int on_value, int off_value); - - /** - * send pwm value to led(channel), - * value should be range of 0-4095 - */ - void setPWM(uint8_t channel, int value); - -private: - int _fd = -1; ///< I2C device file descriptor - - /** - * Read a single byte from PCA9685 - * @param fd file descriptor for I/O - * @param address register address to read from - * @return read byte - */ - uint8_t read_byte(int fd, uint8_t address); - - /** - * Write a single byte to PCA9685 - * @param fd file descriptor for I/O - * @param address register address to write to - * @param data 8 bit data to write - */ - void write_byte(int fd, uint8_t address, uint8_t data); - - /** - * Open device file for PCA9685 I2C bus - * @return fd returns the file descriptor number or -1 on error - */ - int open_fd(int bus, int address); -}; 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 deleted file mode 100644 index aa6e954609..0000000000 --- a/src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.cpp +++ /dev/null @@ -1,389 +0,0 @@ -/**************************************************************************** - * - * 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开始*************/ - /**************PCA965 initializing********/ - pwm.init(1, 0x40); - usleep(1000 * 100); - /****12BIT 精度输出下，好赢电调可以到200HZ刷新***/ - /****200HZ for 12bit Resolution, support most of the esc***/ - pwm.setPWMFreq(200); - usleep(1000 * 1000); - /**************初始化PCA9685结束************/ - /**************PCA965 initialized********/ - 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发送数据*************/ - /*************send pwm signal to pca9685 initializing*************/ - int i; - - for (i = 0; i < NUM_PWM; ++i) { - //PX4_WARN("PWM%d:%d\n",i,*(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************/ - /***************rpc_pca9685_pwm_out*************/ - rpi_pca9685_pwm_out::pwm_initialize(); - - // Set up mixer - if (initialize_mixer(_mixer_filename) < 0) { - PX4_ERR("无法初始化通道混合配置文件 Can't loading mixer file"); - 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 deleted file mode 100644 index ffca2efb8a..0000000000 --- a/src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.h +++ /dev/null @@ -1,127 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -#pragma once - -#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 **);