From 3ed8e876b7b5836f2aebf33e966b992878c622c8 Mon Sep 17 00:00:00 2001 From: SalimTerryLi Date: Thu, 23 Jan 2020 04:19:47 +0800 Subject: [PATCH] New pca9685 pwm output driver --- boards/px4/raspberrypi/default.cmake | 1 + src/drivers/pca9685_pwm_out/CMakeLists.txt | 44 ++ src/drivers/pca9685_pwm_out/PCA9685.cpp | 279 ++++++++++ src/drivers/pca9685_pwm_out/PCA9685.h | 163 ++++++ src/drivers/pca9685_pwm_out/main.cpp | 617 +++++++++++++++++++++ 5 files changed, 1104 insertions(+) create mode 100644 src/drivers/pca9685_pwm_out/CMakeLists.txt create mode 100644 src/drivers/pca9685_pwm_out/PCA9685.cpp create mode 100644 src/drivers/pca9685_pwm_out/PCA9685.h create mode 100644 src/drivers/pca9685_pwm_out/main.cpp diff --git a/boards/px4/raspberrypi/default.cmake b/boards/px4/raspberrypi/default.cmake index 8b0c6c4b3f..b316bc3605 100644 --- a/boards/px4/raspberrypi/default.cmake +++ b/boards/px4/raspberrypi/default.cmake @@ -22,6 +22,7 @@ px4_add_board( linux_sbus #magnetometer # all available magnetometer drivers magnetometer/hmc5883 + pca9685_pwm_out pwm_out_sim rc_input rpi_rc_in diff --git a/src/drivers/pca9685_pwm_out/CMakeLists.txt b/src/drivers/pca9685_pwm_out/CMakeLists.txt new file mode 100644 index 0000000000..d584d0daf3 --- /dev/null +++ b/src/drivers/pca9685_pwm_out/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2019 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__pca9685 + MAIN pca9685_pwm_out + COMPILE_FLAGS + SRCS + main.cpp + PCA9685.cpp + DEPENDS + mixer + mixer_module + output_limit + ) diff --git a/src/drivers/pca9685_pwm_out/PCA9685.cpp b/src/drivers/pca9685_pwm_out/PCA9685.cpp new file mode 100644 index 0000000000..32b03b0eec --- /dev/null +++ b/src/drivers/pca9685_pwm_out/PCA9685.cpp @@ -0,0 +1,279 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 +#include +#include "PCA9685.h" + +using namespace drv_pca9685_pwm; + +PCA9685::PCA9685(int bus, int addr): + I2C("PCA9685", PWM_OUTPUT0_DEVICE_PATH, bus, addr, 400000) +{ + +} + +int PCA9685::Start() +{ + int ret = init(); + return ret; +} + +int PCA9685::Stop() +{ + disableAllOutput(); + return PX4_OK; +} + +int PCA9685::updatePWM(const uint16_t *outputs, unsigned num_outputs) +{ + if (num_outputs > PCA9685_PWM_CHANNEL_COUNT) { + num_outputs = PCA9685_PWM_CHANNEL_COUNT; + PX4_WARN("PCA9685 can only drive up to 16 channels"); + } + + uint16_t out[PCA9685_PWM_CHANNEL_COUNT]; + memcpy(out, outputs, sizeof(uint16_t) * num_outputs); + + for (unsigned i = 0; i < num_outputs; ++i) { + out[i] = (uint16_t)roundl((out[i] * _Freq * PCA9685_PWM_RES / (float)1e6)); // convert us to 12 bit resolution + } + + setPWM(num_outputs, out); + + return 0; +} + +int PCA9685::setFreq(int freq) +{ + uint16_t realResolution = floorl((double)PCA9685_CLOCK_FREQ / freq); + + if (realResolution < PCA9685_PWM_RES) { // unable to provide enough resolution + PX4_DEBUG("frequency too high"); + return -EINVAL; + } + + uint16_t divider = (uint16_t)round((double)PCA9685_CLOCK_FREQ / freq / PCA9685_PWM_RES) - 1; + + if (divider > 0x00FF) { // out of divider + PX4_DEBUG("frequency too low"); + return -EINVAL; + } + + float freq_err = ((float)PCA9685_CLOCK_FREQ / (float)(divider + (uint16_t)1) + - (float)(freq * PCA9685_PWM_RES)) + / (float)(freq * PCA9685_PWM_RES); // actually asked for (freq * PCA9685_PWM_RES) + + if (fabsf(freq_err) > 0.01f) { // TODO decide threshold + PX4_WARN("frequency error too large: %.4f", (double)freq_err); + // should we return an error? + } + + _Freq = (float)PCA9685_CLOCK_FREQ / (float)(divider + (uint16_t)1) / PCA9685_PWM_RES; // use actual pwm freq instead. + + setDivider(divider); + + return PX4_OK; + +} + +int PCA9685::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + PX4_DEBUG("I2C init failed."); + return ret; + } + + uint8_t buf[2] = {}; + buf[0] = PCA9685_REG_MODE1; + buf[1] = DEFAULT_MODE1_CFG; + ret = transfer(buf, 2, nullptr, 0); + + if (OK != ret) { + PX4_DEBUG("i2c::transfer returned %d", ret); + return ret; + } + + buf[0] = PCA9685_REG_MODE2; + buf[1] = DEFAULT_MODE2_CFG; + ret = transfer(buf, 2, nullptr, 0); + + if (OK != ret) { + PX4_DEBUG("i2c::transfer returned %d", ret); + return ret; + } + + disableAllOutput(); + + return PX4_OK; +} + +int PCA9685::probe() +{ + return I2C::probe(); +} + +void PCA9685::setPWM(uint8_t channel, const uint16_t &value) +{ + if (value >= 4096) { + PX4_DEBUG("invalid pwm value"); + return; + } + + uint8_t buf[5] = {}; + buf[0] = PCA9685_REG_LED0 + channel * PCA9685_REG_LED_INCREMENT; + buf[1] = 0x00; + buf[2] = 0x00; + buf[3] = (uint8_t)(value & (uint8_t)0xFF); + buf[4] = value != 0 ? ((uint8_t)(value >> (uint8_t)8)) : PCA9685_LED_ON_FULL_ON_OFF_MASK; + + int ret = transfer(buf, 5, nullptr, 0); + + if (OK != ret) { + PX4_DEBUG("i2c::transfer returned %d", ret); + } +} + +void PCA9685::setPWM(uint8_t channel_count, const uint16_t *value) +{ + uint8_t buf[PCA9685_PWM_CHANNEL_COUNT * PCA9685_REG_LED_INCREMENT + 1] = {}; + buf[0] = PCA9685_REG_LED0; + + for (int i = 0; i < channel_count; ++i) { + if (value[i] >= 4096) { + PX4_DEBUG("invalid pwm value"); + return; + } + + buf[1 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + buf[2 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + buf[3 + i * PCA9685_REG_LED_INCREMENT] = (uint8_t)(value[i] & (uint8_t)0xFF); + buf[4 + i * PCA9685_REG_LED_INCREMENT] = value[i] != 0 ? ((uint8_t)(value[i] >> (uint8_t)8)) : + PCA9685_LED_ON_FULL_ON_OFF_MASK; + } + + int ret = transfer(buf, channel_count * PCA9685_REG_LED_INCREMENT + 1, nullptr, 0); + + if (OK != ret) { + PX4_DEBUG("i2c::transfer returned %d", ret); + } +} + +void PCA9685::disableAllOutput() +{ + uint8_t buf[5] = {}; + buf[0] = PCA9685_REG_ALLLED_ON_L; + buf[1] = 0x00; + buf[2] = 0x00; + buf[3] = (uint8_t)(0x00 & (uint8_t)0xFF); + buf[4] = PCA9685_LED_ON_FULL_ON_OFF_MASK; + + int ret = transfer(buf, 5, nullptr, 0); + + if (OK != ret) { + PX4_DEBUG("i2c::transfer returned %d", ret); + } +} + +void PCA9685::setDivider(uint8_t value) +{ + disableAllOutput(); + stopOscillator(); + uint8_t buf[2] = {}; + buf[0] = PCA9685_REG_PRE_SCALE; + buf[1] = value; + int ret = transfer(buf, 2, nullptr, 0); + + if (OK != ret) { + PX4_DEBUG("i2c::transfer returned %d", ret); + return; + } + + restartOscillator(); +} + +void PCA9685::stopOscillator() +{ + uint8_t buf[2] = {PCA9685_REG_MODE1}; + int ret = transfer(buf, 1, buf, 1); + + if (OK != ret) { + PX4_DEBUG("i2c::transfer returned %d", ret); + return; + } + + buf[1] = buf[0]; + buf[0] = PCA9685_REG_MODE1; + // clear restart bit + buf[1] |= PCA9685_MODE1_SLEEP_MASK; + ret = transfer(buf, 2, nullptr, 0); + + if (OK != ret) { + PX4_DEBUG("i2c::transfer returned %d", ret); + return; + } +} + +void PCA9685::restartOscillator() +{ + uint8_t buf[2] = {PCA9685_REG_MODE1}; + int ret = transfer(buf, 1, buf + 1, 1); + + if (OK != ret) { + PX4_DEBUG("i2c::transfer returned %d", ret); + return; + } + + // clear restart and sleep bit + buf[1] &= PCA9685_MODE1_EXTCLK_MASK | PCA9685_MODE1_AI_MASK + | PCA9685_MODE1_SUB1_MASK | PCA9685_MODE1_SUB2_MASK + | PCA9685_MODE1_SUB3_MASK | PCA9685_MODE1_ALLCALL_MASK; + buf[0] = PCA9685_REG_MODE1; + ret = transfer(buf, 2, nullptr, 0); + + if (OK != ret) { + PX4_DEBUG("i2c::transfer returned %d", ret); + return; + } + + usleep(5000); + buf[1] |= PCA9685_MODE1_RESTART_MASK; + ret = transfer(buf, 2, nullptr, 0); + + if (OK != ret) { + PX4_DEBUG("i2c::transfer returned %d", ret); + return; + } +} diff --git a/src/drivers/pca9685_pwm_out/PCA9685.h b/src/drivers/pca9685_pwm_out/PCA9685.h new file mode 100644 index 0000000000..655026f0b7 --- /dev/null +++ b/src/drivers/pca9685_pwm_out/PCA9685.h @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 +#include +#include + +namespace drv_pca9685_pwm +{ + +#define PCA9685_REG_MODE1 0x00 // Mode register 1 +#define PCA9685_REG_MODE2 0x01 // Mode register 2 +#define PCA9685_REG_SUBADR1 0x02 // I2C-bus subaddress 1 +#define PCA9685_REG_SUBADR2 0x03 // I2C-bus subaddress 2 +#define PCA9685_REG_SUBADR3 0x04 // I2C-bus subaddress 3 +#define PCA9685_REG_ALLCALLADR 0x05 // LED All Call I2C-bus address +#define PCA9685_REG_LED0 0x06 // LED0 start register +#define PCA9685_REG_LED0_ON_L 0x06 // LED0 output and brightness control byte 0 +#define PCA9685_REG_LED0_ON_H 0x07 // LED0 output and brightness control byte 1 +#define PCA9685_REG_LED0_OFF_L 0x08 // LED0 output and brightness control byte 2 +#define PCA9685_REG_LED0_OFF_H 0x09 // LED0 output and brightness control byte 3 +#define PCA9685_REG_LED_INCREMENT 4 // compute the other 15 channels +#define PCA9685_REG_ALLLED_ON_L 0xFA // load all the LEDn_ON registers, byte 0 (turn 0-7 channels on) +#define PCA9685_REG_ALLLED_ON_H 0xFB // load all the LEDn_ON registers, byte 1 (turn 8-15 channels on) +#define PCA9685_REG_ALLLED_OFF_L 0xFC // load all the LEDn_OFF registers, byte 0 (turn 0-7 channels off) +#define PCA9685_REG_ALLLED_OFF_H 0xFD // load all the LEDn_OFF registers, byte 1 (turn 8-15 channels off) +#define PCA9685_REG_PRE_SCALE 0xFE // prescaler for output frequency +#define PCA9685_REG_TESTMODE 0xFF // test mode register + +// Mode register 1 +#define PCA9685_MODE1_RESTART_MASK 0x80 +#define PCA9685_MODE1_EXTCLK_MASK 0x40 +#define PCA9685_MODE1_AI_MASK 0x20 +#define PCA9685_MODE1_SLEEP_MASK 0x10 +#define PCA9685_MODE1_SUB1_MASK 0x08 +#define PCA9685_MODE1_SUB2_MASK 0x04 +#define PCA9685_MODE1_SUB3_MASK 0x02 +#define PCA9685_MODE1_ALLCALL_MASK 0x01 + +// Mode register 2 +#define PCA9685_MODE2_INVRT_MASK 0x10 +#define PCA9685_MODE2_OCH_MASK 0x08 +#define PCA9685_MODE2_OUTDRV_MASK 0x04 +#define PCA9685_MODE2_OUTNE_MASK 0x03 + +// LED_ON, LED_OFF control registers +#define PCA9685_LED0_ON_L_MASK 0xFF +#define PCA9685_LED_ON_FULL_ON_OFF_MASK 0x10 +#define PCA9685_LED0_ON_H_MASK 0x0F + +// PRE_SCALE register +#define PCA9685_PRE_SCALE_MASK 0xFF + +#define PCA9685_PWM_CHANNEL_COUNT 16 +#define PCA9685_PWM_RES 4096 //Resolution 4096=12bit +#define PCA9685_CLOCK_INT 25000000.0 //25MHz internal clock +#ifndef PCA9685_CLOCL_EXT +#define PCA9685_CLOCK_FREQ PCA9685_CLOCK_INT // use int clk +#else +#define PCA9685_CLOCK_FREQ PCA9685_CLOCK_EXT // use ext clk +#endif + +#define PWM_DEFAULT_FREQUENCY 50 // default pwm frequency + +//! Main class that exports features for PCA9685 chip +class PCA9685 : public device::I2C +{ +public: + PCA9685(int bus, int addr); + + int Start(); + + int Stop(); + + /* + * outputs formatted to us. + */ + int updatePWM(const uint16_t *outputs, unsigned num_outputs); + + int setFreq(int freq); + + ~PCA9685() override = default; + + int init() override; + + inline float getFrequency() {return _Freq;} + +protected: + int probe() override; + + static const uint8_t DEFAULT_MODE1_CFG = 0x20; + static const uint8_t DEFAULT_MODE2_CFG = 0x04; + + float _Freq = PWM_DEFAULT_FREQUENCY; + + /** + * set PWM value for a channel[0,15]. + * value should be range of 0-4095 + */ + void setPWM(uint8_t channel, const uint16_t &value); + + /** + * set all PWMs in a single I2C transmission. + * value should be range of 0-4095 + */ + void setPWM(uint8_t channel_count, const uint16_t *value); + + /* + * disable all of the output + */ + void disableAllOutput(); + + /* + * set clock divider + * this func has Super Cow Powers + */ + void setDivider(uint8_t value); + + /* + * turn off oscillator + */ + void stopOscillator(); + + /* + * restart output + */ + void restartOscillator(); +private: + +}; + +} diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp new file mode 100644 index 0000000000..cea1f8f5c0 --- /dev/null +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -0,0 +1,617 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2019 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/main.cpp + * A cross-platform driver and wrapper for pca9685. + * Designed to support all control-groups by binding to correct mixer files + * @author SalimTerryLi + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "PCA9685.h" + +#define PCA9685_MAX_OUTPUT_CHANNELS 16 +#define PCA9685_DEVICE_BASE_PATH "/dev/pca9685" + +#define PCA9685_DEFAULT_IICBUS 1 +#define PCA9685_DEFAULT_ADDRESS (0x40) + +using namespace drv_pca9685_pwm; + +class PWMDriverWrapper : public cdev::CDev, public ModuleBase, public OutputModuleInterface +{ +public: + + PWMDriverWrapper(); + ~PWMDriverWrapper() override ; + + int init() override; + + int ioctl(cdev::file_t *filep, int cmd, unsigned long arg) override; + + void mixerChanged() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs, + unsigned num_control_groups_updated) override; + + PWMDriverWrapper(const PWMDriverWrapper &) = delete; + PWMDriverWrapper operator=(const PWMDriverWrapper &) = delete; + + int print_status() override; + +private: + perf_counter_t _cycle_perf; + + void Run() override; + +protected: + void updateParams() override; + + void updatePWMParams(); + + void updatePWMParamTrim(); + + PCA9685 *pca9685 = nullptr; // driver handle. + + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; // param handle + + MixingOutput _mixing_output{PCA9685_MAX_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, true}; + +private: + +}; + +PWMDriverWrapper::PWMDriverWrapper() : + CDev(PCA9685_DEVICE_BASE_PATH), + OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + _mixing_output.setAllMinValues(PWM_DEFAULT_MIN); + _mixing_output.setAllMaxValues(PWM_DEFAULT_MAX); +} + +PWMDriverWrapper::~PWMDriverWrapper() +{ + if (pca9685 != nullptr) { // normally this should not be called. + PX4_DEBUG("Destruction of PWMDriverWrapper without pwmDevice unloaded!"); + pca9685->Stop(); // force stop + delete pca9685; + pca9685 = nullptr; + } + + perf_free(_cycle_perf); +} + +int PWMDriverWrapper::init() +{ + int ret = CDev::init(); + + if (ret != PX4_OK) { + return ret; + } + + ret = pca9685->Start(); + + if (ret != PX4_OK) { + return ret; + } + + updatePWMParams(); + + if (!_mixing_output.updateSubscriptions(true)) { // change to proper wq + ScheduleNow(); + } + + return PX4_OK; +} + +void PWMDriverWrapper::updateParams() +{ + updatePWMParams(); + ModuleParams::updateParams(); +} + +void PWMDriverWrapper::updatePWMParams() +{ + // update pwm params + const char *pname_format_main_max = "PWM_MAIN_MAX%d"; + const char *pname_format_main_min = "PWM_MAIN_MIN%d"; + const char *pname_format_main_fail = "PWM_MAIN_FAIL%d"; + const char *pname_format_main_dis = "PWM_MAIN_DIS%d"; + const char *pname_format_main_rev = "PWM_MAIN_REV%d"; + const char *pname_format_aux_max = "PWM_AUX_MAX%d"; + const char *pname_format_aux_min = "PWM_AUX_MIN%d"; + const char *pname_format_aux_fail = "PWM_AUX_FAIL%d"; + const char *pname_format_aux_dis = "PWM_AUX_DIS%d"; + const char *pname_format_aux_rev = "PWM_AUX_REV%d"; + + param_t param_h = param_find("PWM_MAX"); + + if (param_h != PARAM_INVALID) { + int32_t pval = 0; + param_get(param_h, &pval); + _mixing_output.setAllMaxValues(pval); + + } else { + PX4_DEBUG("PARAM_INVALID: %s", "PWM_MAX"); + } + + param_h = param_find("PWM_MIN"); + + if (param_h != PARAM_INVALID) { + int32_t pval = 0; + param_get(param_h, &pval); + _mixing_output.setAllMinValues(pval); + + } else { + PX4_DEBUG("PARAM_INVALID: %s", "PWM_MIN"); + } + + param_h = param_find("PWM_RATE"); + + if (param_h != PARAM_INVALID) { + int32_t pval = 0; + param_get(param_h, &pval); + + if (pca9685->setFreq(pval) != PX4_OK) { + PX4_DEBUG("failed to set pwm frequency"); + } + + } else { + PX4_DEBUG("PARAM_INVALID: %s", "PWM_RATE"); + } + + for (int i = 0; i < 8; i++) { + char pname[16]; + + sprintf(pname, pname_format_main_max, i + 1); + param_h = param_find(pname); + + if (param_h != PARAM_INVALID) { + int32_t pval = 0; + param_get(param_h, &pval); + + if (pval != -1) { + _mixing_output.maxValue(i) = pval; + } + + } else { + PX4_DEBUG("PARAM_INVALID: %s", pname); + } + + sprintf(pname, pname_format_main_min, i + 1); + param_h = param_find(pname); + + if (param_h != PARAM_INVALID) { + int32_t pval = 0; + param_get(param_h, &pval); + + if (pval != -1) { + _mixing_output.minValue(i) = pval; + } + + } else { + PX4_DEBUG("PARAM_INVALID: %s", pname); + } + + sprintf(pname, pname_format_main_fail, i + 1); + param_h = param_find(pname); + + if (param_h != PARAM_INVALID) { + int32_t pval = 0; + param_get(param_h, &pval); + + if (pval != -1) { + _mixing_output.failsafeValue(i) = pval; + } + + } else { + PX4_DEBUG("PARAM_INVALID: %s", pname); + } + + sprintf(pname, pname_format_main_dis, i + 1); + param_h = param_find(pname); + + if (param_h != PARAM_INVALID) { + int32_t pval = 0; + param_get(param_h, &pval); + + if (pval != -1) { + _mixing_output.disarmedValue(i) = pval; + } + + } else { + PX4_DEBUG("PARAM_INVALID: %s", pname); + } + + sprintf(pname, pname_format_main_rev, i + 1); + param_h = param_find(pname); + + if (param_h != PARAM_INVALID) { + uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask(); + int32_t pval = 0; + param_get(param_h, &pval); + reverse_pwm_mask &= (0xfffe << i); // clear this bit + reverse_pwm_mask |= (((uint16_t)(pval != 0)) << i); // set to new val + + } else { + PX4_DEBUG("PARAM_INVALID: %s", pname); + } + + sprintf(pname, pname_format_aux_max, i + 1); + param_h = param_find(pname); + + if (param_h != PARAM_INVALID) { + int32_t pval = 0; + param_get(param_h, &pval); + + if (pval != -1) { + _mixing_output.maxValue(i + 8) = pval; + } + + } else { + PX4_DEBUG("PARAM_INVALID: %s", pname); + } + + sprintf(pname, pname_format_aux_min, i + 1); + param_h = param_find(pname); + + if (param_h != PARAM_INVALID) { + int32_t pval = 0; + param_get(param_h, &pval); + + if (pval != -1) { + _mixing_output.minValue(i + 8) = pval; + } + + } else { + PX4_DEBUG("PARAM_INVALID: %s", pname); + } + + sprintf(pname, pname_format_aux_fail, i + 1); + param_h = param_find(pname); + + if (param_h != PARAM_INVALID) { + int32_t pval = 0; + param_get(param_h, &pval); + + if (pval != -1) { + _mixing_output.failsafeValue(i + 8) = pval; + } + + } else { + PX4_DEBUG("PARAM_INVALID: %s", pname); + } + + sprintf(pname, pname_format_aux_dis, i + 1); + param_h = param_find(pname); + + if (param_h != PARAM_INVALID) { + int32_t pval = 0; + param_get(param_h, &pval); + + if (pval != -1) { + _mixing_output.disarmedValue(i + 8) = pval; + } + + } else { + PX4_DEBUG("PARAM_INVALID: %s", pname); + } + + sprintf(pname, pname_format_aux_rev, i + 1); + param_h = param_find(pname); + + if (param_h != PARAM_INVALID) { + uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask(); + int32_t pval = 0; + param_get(param_h, &pval); + reverse_pwm_mask &= (0xfffe << (i + 8)); // clear this bit + reverse_pwm_mask |= (((uint16_t)(pval != 0)) << (i + 8)); // set to new val + + } else { + PX4_DEBUG("PARAM_INVALID: %s", pname); + } + } + + if (_mixing_output.mixers()) { // only update trims if mixer loaded + updatePWMParamTrim(); + } +} + +void PWMDriverWrapper::updatePWMParamTrim() +{ + const char *pname_format_main_trim = "PWM_MAIN_TRIM%d"; + const char *pname_format_aux_trim = "PWM_AUX_TRIM%d"; + + int16_t trim_values[PCA9685_MAX_OUTPUT_CHANNELS] = {}; + + for (int i = 0; i < 8; i++) { + char pname[16]; + + sprintf(pname, pname_format_main_trim, i + 1); + param_t param_h = param_find(pname); + + if (param_h != PARAM_INVALID) { + float pval = 0.0f; + param_get(param_h, &pval); + trim_values[i] = (int16_t)(10000 * pval); + + } else { + PX4_DEBUG("PARAM_INVALID: %s", pname); + } + + sprintf(pname, pname_format_aux_trim, i + 1); + param_h = param_find(pname); + + if (param_h != PARAM_INVALID) { + float pval = 0.0f; + param_get(param_h, &pval); + trim_values[i + 8] = (int16_t)(10000 * pval); + + } else { + PX4_DEBUG("PARAM_INVALID: %s", pname); + } + } + + unsigned n_out = _mixing_output.mixers()->set_trims(trim_values, PCA9685_MAX_OUTPUT_CHANNELS); + PX4_DEBUG("set %d trims", n_out); +} + +bool PWMDriverWrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs, + unsigned num_control_groups_updated) +{ + /*char buf[1024]="PWM:"; + for(uint i=0;iupdatePWM(outputs, num_outputs);; +} + +void PWMDriverWrapper::Run() +{ + hrt_abstime timestamp = hrt_absolute_time(); + + if (should_exit()) { + PX4_INFO("PCA9685 stopping."); + ScheduleClear(); + _mixing_output.unregister(); + + pca9685->Stop(); + delete pca9685; + pca9685 = nullptr; + + exit_and_cleanup(); + return; + } + + perf_begin(_cycle_perf); + + _mixing_output.update(); + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + } + + perf_end(_cycle_perf); + + int nextDelay = 1000000 / pca9685->getFrequency() - (hrt_absolute_time() - timestamp); + + if (nextDelay < 0) { + PX4_DEBUG("PCA9685: can not follow up. %d us"); + ScheduleNow(); + + } else { + ScheduleDelayed((uint32_t)nextDelay); + } +} + +// TODO +int PWMDriverWrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg) +{ + int ret = OK; + + lock(); + + switch (cmd) { + case MIXERIOCRESET: + _mixing_output.resetMixerThreadSafe(); + + break; + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strlen(buf); + + ret = _mixing_output.loadMixerThreadSafe(buf, buflen); + + break; + } + + default: + ret = -ENOTTY; + break; + } + + unlock(); + + if (ret == -ENOTTY) { + ret = CDev::ioctl(filep, cmd, arg); + } + + return ret; +} + +int PWMDriverWrapper::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module is responsible for generate pwm pulse with PCA9685 chip. + +It listens on the actuator_controls topics, does the mixing and writes the PWM outputs. + +### Implementation +This module depends on ModuleBase and OutputModuleInterface. +IIC communication is based on CDev::I2C + +### Examples +It is typically started with: +$ pca9685_pwm_out start -a 64 -b 1 + +Use the `mixer` command to load mixer files. +`mixer load /dev/pca9685 ROMFS/px4fmu_common/mixers/quad_x.main.mix` +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("pca9685_pwm_out", "driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task"); + PRINT_MODULE_USAGE_PARAM_INT('a',64,0,255,"device address on this bus",true); + PRINT_MODULE_USAGE_PARAM_INT('b',1,0,255,"bus that pca9685 is connected to",true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int PWMDriverWrapper::print_status() { + int ret = ModuleBase::print_status(); + PX4_INFO("PCA9685 @I2C Bus %d, address 0x%.2x, true frequency %.5f", + pca9685->get_device_bus(), + pca9685->get_device_address(), + (double)(pca9685->getFrequency())); + + return ret; +} + +int PWMDriverWrapper::custom_command(int argc, char **argv) { // only for test use + return PX4_OK; +} + +int PWMDriverWrapper::task_spawn(int argc, char **argv) { + + auto *instance = new PWMDriverWrapper(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + int ch; + int address=PCA9685_DEFAULT_ADDRESS; + int iicbus=PCA9685_DEFAULT_IICBUS; + + int myoptind = 1; + const char *myoptarg = nullptr; + while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'a': + address = atoi(myoptarg); + break; + + case 'b': + iicbus = atoi(myoptarg); + break; + + case '?': + PX4_WARN("Unsupported args"); + goto driverInstanceAllocFailed; + + default: + break; + } + } + + instance->pca9685 = new PCA9685(iicbus, address); + if(instance->pca9685==nullptr){ + PX4_ERR("alloc failed"); + goto driverInstanceAllocFailed; + } + + if (instance->init() == PX4_OK) { + return PX4_OK; + } else { + PX4_ERR("driver init failed"); + delete instance->pca9685; + instance->pca9685=nullptr; + } + } else { + PX4_ERR("alloc failed"); + } + + driverInstanceAllocFailed: + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +void PWMDriverWrapper::mixerChanged() { + OutputModuleInterface::mixerChanged(); + if (_mixing_output.mixers()) { // only update trims if mixer loaded + updatePWMParamTrim(); + } + if(!_mixing_output.updateSubscriptions(true)) { // change to proper wq + ScheduleNow(); + } +} + +extern "C" __EXPORT int pca9685_pwm_out_main(int argc, char *argv[]); + +int pca9685_pwm_out_main(int argc, char *argv[]){ + return PWMDriverWrapper::main(argc, argv); +} \ No newline at end of file