diff --git a/src/drivers/rpi_pwm_out/CMakeLists.txt b/src/drivers/rpi_pwm_out/CMakeLists.txt index 2c5603adb4..e7909cb2d5 100644 --- a/src/drivers/rpi_pwm_out/CMakeLists.txt +++ b/src/drivers/rpi_pwm_out/CMakeLists.txt @@ -35,6 +35,8 @@ px4_add_module( MAIN rpi_pwm_out COMPILE_FLAGS SRCS + PCA9685.cpp + navio_sysfs.cpp rpi_pwm_out.cpp DEPENDS platforms__common diff --git a/src/drivers/rpi_pwm_out/PCA9685.cpp b/src/drivers/rpi_pwm_out/PCA9685.cpp new file mode 100644 index 0000000000..343013e993 --- /dev/null +++ b/src/drivers/rpi_pwm_out/PCA9685.cpp @@ -0,0 +1,192 @@ +/**************************************************************************** + * + * 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 + +using namespace rpi_pwm_out; + +void PCA9685::init(int bus, int address) +{ + _fd = open_fd(bus, address); + reset(); + + usleep(1000 * 100); + /* 12BIT 精度输出下，好赢电调可以到200HZ刷新 */ + /* 200HZ for 12bit Resolution, supported by most of the esc's */ + setPWMFreq(200); + usleep(1000 * 1000); +} + +int PCA9685::send_output_pwm(const uint16_t *pwm, int num_outputs) +{ + if (num_outputs > 16) { + num_outputs = 16; + } + + for (int i = 0; i < num_outputs; ++i) { + setPWM(i, pwm[i]); + } + + return 0; +} + +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_pwm_out/PCA9685.h b/src/drivers/rpi_pwm_out/PCA9685.h new file mode 100644 index 0000000000..fd1e86803e --- /dev/null +++ b/src/drivers/rpi_pwm_out/PCA9685.h @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * 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 +#include "common.h" + +namespace rpi_pwm_out +{ + +// 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 PWMOutBase +{ +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); + + int init() override { return _fd >= 0; } + + int send_output_pwm(const uint16_t *pwm, int num_outputs) override; + + + void init(int bus, int address); + virtual ~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_pwm_out/common.h b/src/drivers/rpi_pwm_out/common.h new file mode 100644 index 0000000000..c28085c9e1 --- /dev/null +++ b/src/drivers/rpi_pwm_out/common.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +namespace rpi_pwm_out +{ + +/** + ** class PWMOutBase + * common abstract PWM output base class + */ +class PWMOutBase +{ +public: + + virtual ~PWMOutBase() {} + + virtual int init() = 0; + + virtual int send_output_pwm(const uint16_t *pwm, int num_outputs) = 0; +}; + + +} /* namespace rpi_pwm_out */ diff --git a/src/drivers/rpi_pwm_out/navio_sysfs.cpp b/src/drivers/rpi_pwm_out/navio_sysfs.cpp new file mode 100644 index 0000000000..2761c69d7b --- /dev/null +++ b/src/drivers/rpi_pwm_out/navio_sysfs.cpp @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "navio_sysfs.h" + +#include +#include +#include + +using namespace rpi_pwm_out; + +NavioSysfsPWMOut::NavioSysfsPWMOut(const char *device, int max_num_outputs) + : _device(device) +{ + if (max_num_outputs > MAX_NUM_PWM) { + PX4_WARN("number of outputs too large. Setting to %i", MAX_NUM_PWM); + max_num_outputs = MAX_NUM_PWM; + } + + for (int i = 0; i < MAX_NUM_PWM; ++i) { + _pwm_fd[i] = -1; + } + + _pwm_num = max_num_outputs; +} + +NavioSysfsPWMOut::~NavioSysfsPWMOut() +{ + for (int i = 0; i < MAX_NUM_PWM; ++i) { + if (_pwm_fd[i] != -1) { + ::close(_pwm_fd[i]); + } + } +} + +int NavioSysfsPWMOut::init() +{ + int i; + char path[128]; + + for (i = 0; i < _pwm_num; ++i) { + ::snprintf(path, sizeof(path), "%s/export", _device); + + if (pwm_write_sysfs(path, i) < 0) { + PX4_ERR("PWM export failed"); + } + } + + for (i = 0; i < _pwm_num; ++i) { + ::snprintf(path, sizeof(path), "%s/pwm%u/enable", _device, i); + + if (pwm_write_sysfs(path, 1) < 0) { + PX4_ERR("PWM enable failed"); + } + } + + for (i = 0; i < _pwm_num; ++i) { + ::snprintf(path, sizeof(path), "%s/pwm%u/period", _device, i); + + if (pwm_write_sysfs(path, (int)1e9 / FREQUENCY_PWM)) { + PX4_ERR("PWM period failed"); + } + } + + for (i = 0; i < _pwm_num; ++i) { + ::snprintf(path, sizeof(path), "%s/pwm%u/duty_cycle", _device, i); + _pwm_fd[i] = ::open(path, O_WRONLY | O_CLOEXEC); + + if (_pwm_fd[i] == -1) { + PX4_ERR("PWM: Failed to open duty_cycle."); + return -errno; + } + } + + return 0; +} + +int NavioSysfsPWMOut::send_output_pwm(const uint16_t *pwm, int num_outputs) +{ + char data[16]; + + if (num_outputs > _pwm_num) { + num_outputs = _pwm_num; + } + + int ret = 0; + + //convert this to duty_cycle in ns + for (unsigned i = 0; i < num_outputs; ++i) { + int n = ::snprintf(data, sizeof(data), "%u", pwm[i] * 1000); + int write_ret = ::write(_pwm_fd[i], data, n); + + if (n != write_ret) { + ret = -1; + } + } + + return ret; +} + +int NavioSysfsPWMOut::pwm_write_sysfs(char *path, int value) +{ + int fd = ::open(path, O_WRONLY | O_CLOEXEC); + int n; + char data[16]; + + if (fd == -1) { + return -errno; + } + + n = ::snprintf(data, sizeof(data), "%u", value); + + if (n > 0) { + n = ::write(fd, data, n); // This n is not used, but to avoid a compiler error. + } + + ::close(fd); + + return 0; +} + diff --git a/src/drivers/rpi_pwm_out/navio_sysfs.h b/src/drivers/rpi_pwm_out/navio_sysfs.h new file mode 100644 index 0000000000..0605fba334 --- /dev/null +++ b/src/drivers/rpi_pwm_out/navio_sysfs.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include "common.h" + +namespace rpi_pwm_out +{ + +/** + ** class NavioSysfsPWMOut + * PWM output class for Navio Sysfs + */ +class NavioSysfsPWMOut : public PWMOutBase +{ +public: + NavioSysfsPWMOut(const char *device, int max_num_outputs); + virtual ~NavioSysfsPWMOut(); + + int init() override; + + int send_output_pwm(const uint16_t *pwm, int num_outputs) override; + +private: + int pwm_write_sysfs(char *path, int value); + + static const int MAX_NUM_PWM = 14; + static const int FREQUENCY_PWM = 400; + + int _pwm_fd[MAX_NUM_PWM]; + int _pwm_num; + + const char *_device; +}; + +} diff --git a/src/drivers/rpi_pwm_out/rpi_pwm_out.cpp b/src/drivers/rpi_pwm_out/rpi_pwm_out.cpp index 7f545eea76..c1de14f7c3 100644 --- a/src/drivers/rpi_pwm_out/rpi_pwm_out.cpp +++ b/src/drivers/rpi_pwm_out/rpi_pwm_out.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-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 @@ -51,17 +51,19 @@ #include #include +#include "common.h" +#include "navio_sysfs.h" +#include "PCA9685.h" + namespace rpi_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 _protocol[64] = "navio"; +static int _max_mum_outputs = 8; ///< maximum number of outputs the driver should use static char _mixer_filename[64] = "ROMFS/px4fmu_common/mixers/quad_x.main.mix"; // subscriptions @@ -101,14 +103,6 @@ void start(); void stop(); -int pwm_write_sysfs(char *path, int value); - -int pwm_initialize(const char *device); - -void pwm_deinitialize(); - -void send_outputs_pwm(const uint16_t *pwm); - void task_main_trampoline(int argc, char *argv[]); void subscribe(); @@ -163,89 +157,6 @@ int initialize_mixer(const char *mixer_filename) return 0; } -int pwm_write_sysfs(char *path, int value) -{ - int fd = ::open(path, O_WRONLY | O_CLOEXEC); - int n; - char data[16]; - - if (fd == -1) { - return -errno; - } - - n = ::snprintf(data, sizeof(data), "%u", value); - - if (n > 0) { - n = ::write(fd, data, n); // This n is not used, but to avoid a compiler error. - } - - ::close(fd); - - return 0; -} - -int pwm_initialize(const char *device) -{ - int i; - char path[128]; - - for (i = 0; i < NUM_PWM; ++i) { - ::snprintf(path, sizeof(path), "%s/export", device); - - if (pwm_write_sysfs(path, i) < 0) { - PX4_ERR("PWM export failed"); - } - } - - for (i = 0; i < NUM_PWM; ++i) { - ::snprintf(path, sizeof(path), "%s/pwm%u/enable", device, i); - - if (pwm_write_sysfs(path, 1) < 0) { - PX4_ERR("PWM enable failed"); - } - } - - for (i = 0; i < NUM_PWM; ++i) { - ::snprintf(path, sizeof(path), "%s/pwm%u/period", device, i); - - if (pwm_write_sysfs(path, (int)1e9 / FREQUENCY_PWM)) { - PX4_ERR("PWM period failed"); - } - } - - for (i = 0; i < NUM_PWM; ++i) { - ::snprintf(path, sizeof(path), "%s/pwm%u/duty_cycle", device, i); - _pwm_fd[i] = ::open(path, O_WRONLY | O_CLOEXEC); - - if (_pwm_fd[i] == -1) { - PX4_ERR("PWM: Failed to open duty_cycle."); - return -errno; - } - } - - return 0; -} - -void pwm_deinitialize() -{ - for (int i = 0; i < NUM_PWM; ++i) { - if (_pwm_fd[i] != -1) { - ::close(_pwm_fd[i]); - } - } -} - -void send_outputs_pwm(const uint16_t *pwm) -{ - int n; - char data[16]; - - //convert this to duty_cycle in ns - for (unsigned i = 0; i < NUM_PWM; ++i) { - n = ::snprintf(data, sizeof(data), "%u", pwm[i] * 1000); - n = ::write(_pwm_fd[i], data, n); // This n is not used, but to avoid a compiler error. - } -} void subscribe() { @@ -283,17 +194,29 @@ void task_main(int argc, char *argv[]) { _is_running = true; - if (pwm_initialize(_device) < 0) { - PX4_ERR("Failed to initialize PWM."); - return; - } - // Set up mixer if (initialize_mixer(_mixer_filename) < 0) { PX4_ERR("Mixer initialization failed."); return; } + PWMOutBase *pwm_out; + + if (strcmp(_protocol, "pca9685") == 0) { + PX4_INFO("Starting PWM output in PCA9685 mode"); + pwm_out = new PCA9685(); + + } else { // navio + PX4_INFO("Starting PWM output in Navio mode"); + pwm_out = new NavioSysfsPWMOut(_device, _max_mum_outputs); + } + + if (pwm_out->init() != 0) { + PX4_ERR("PWM output init failed"); + delete pwm_out; + return; + } + _mixer_group->groups_required(_groups_required); // subscribe and set up polling subscribe(); @@ -374,10 +297,10 @@ void task_main(int argc, char *argv[]) &_pwm_limit); if (_armed.lockdown || _armed.manual_lockdown) { - send_outputs_pwm(disarmed_pwm); + pwm_out->send_output_pwm(disarmed_pwm, _outputs.noutputs); } else { - send_outputs_pwm(pwm); + pwm_out->send_output_pwm(pwm, _outputs.noutputs); } if (_outputs_pub != nullptr) { @@ -400,7 +323,7 @@ void task_main(int argc, char *argv[]) } } - pwm_deinitialize(); + delete pwm_out; for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_controls_subs[i] >= 0) { @@ -454,11 +377,15 @@ void stop() void usage() { - PX4_INFO("usage: pwm_out start [-d pwmdevice] -m mixerfile"); - PX4_INFO(" -d pwmdevice : sysfs device for pwm generation"); + PX4_INFO("usage: pwm_out start [-d pwmdevice] [-m mixerfile] [-p protocol]"); + PX4_INFO(" -d pwmdevice : sysfs device for pwm generation (only for Navio)"); 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(" -p protocol : driver output protocol (navio|pca9685)"); + PX4_INFO(" (default is navio)"); + PX4_INFO(" -n num_outputs : maximum number of outputs the driver should use"); + PX4_INFO(" (default is 8)"); PX4_INFO(" pwm_out stop"); PX4_INFO(" pwm_out status"); } @@ -483,7 +410,7 @@ int rpi_pwm_out_main(int argc, char *argv[]) return 1; } - while ((ch = px4_getopt(argc, argv, "d:m:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "d:m:p:n:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'd': strncpy(rpi_pwm_out::_device, myoptarg, sizeof(rpi_pwm_out::_device)); @@ -492,6 +419,25 @@ int rpi_pwm_out_main(int argc, char *argv[]) case 'm': strncpy(rpi_pwm_out::_mixer_filename, myoptarg, sizeof(rpi_pwm_out::_mixer_filename)); break; + + case 'p': + strncpy(rpi_pwm_out::_protocol, myoptarg, sizeof(rpi_pwm_out::_protocol)); + break; + + case 'n': { + unsigned long max_num = strtoul(myoptarg, nullptr, 10); + + if (max_num <= 0) { + max_num = 8; + } + + if (max_num > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) { + max_num = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; + } + + rpi_pwm_out::_max_mum_outputs = max_num; + } + break; } }