diff --git a/boards/scumaker/pilotpi/default.px4board b/boards/scumaker/pilotpi/default.px4board index 0de2a4311a..ccb3bb3ce9 100644 --- a/boards/scumaker/pilotpi/default.px4board +++ b/boards/scumaker/pilotpi/default.px4board @@ -16,6 +16,8 @@ CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL=y +CONFIG_PCA9685_EXTERNAL_CRYSTAL_FREQ=25000000 CONFIG_COMMON_RC=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_SMART_BATTERY_BATMON=y diff --git a/src/drivers/pca9685_pwm_out/Kconfig b/src/drivers/pca9685_pwm_out/Kconfig index 84cc71cda7..ad7c25b8d7 100644 --- a/src/drivers/pca9685_pwm_out/Kconfig +++ b/src/drivers/pca9685_pwm_out/Kconfig @@ -2,4 +2,20 @@ menuconfig DRIVERS_PCA9685_PWM_OUT bool "pca9685_pwm_out" default n ---help--- - Enable support for pca9685_pwm_out \ No newline at end of file + Enable support for pca9685_pwm_out + +if DRIVERS_PCA9685_PWM_OUT + config PCA9685_USE_EXTERNAL_CRYSTAL + bool "Use external crystal for clock reference" + default n + + config PCA9685_EXTERNAL_CRYSTAL_FREQ + int "External crystal frequency" + depends on PCA9685_USE_EXTERNAL_CRYSTAL + default 25000000 + + config PCA9685_INTERNAL_CRYSTAL_FREQ + int "Corrected frequency of internal oscillator" + depends on !PCA9685_USE_EXTERNAL_CRYSTAL + default 26075000 +endif \ No newline at end of file diff --git a/src/drivers/pca9685_pwm_out/PCA9685.cpp b/src/drivers/pca9685_pwm_out/PCA9685.cpp index c2a1af4b16..7eccd6c04b 100644 --- a/src/drivers/pca9685_pwm_out/PCA9685.cpp +++ b/src/drivers/pca9685_pwm_out/PCA9685.cpp @@ -32,23 +32,66 @@ ****************************************************************************/ #include +#include #include #include "PCA9685.h" -#include + +#ifdef CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL +#define PCA9685_CLOCK_REFERENCE CONFIG_PCA9685_EXTERNAL_CRYSTAL_FREQ +#else +#define PCA9685_CLOCK_REFERENCE CONFIG_PCA9685_INTERNAL_CRYSTAL_FREQ +#endif + +#define PCA9685_DEFAULT_MODE1_CFG PCA9685_MODE1_AI_MASK +#define PCA9685_DEFAULT_MODE2_CFG PCA9685_MODE2_OUTDRV_MASK using namespace drv_pca9685_pwm; PCA9685::PCA9685(int bus, int addr): - I2C(DRV_PWM_DEVTYPE_PCA9685, MODULE_NAME, bus, addr, 400000) + I2C(DRV_PWM_DEVTYPE_PCA9685, MODULE_NAME, bus, addr, 400000), + currentFreq(0.0) { } -int PCA9685::Stop() +int PCA9685::init() { - disableAllOutput(); - stopOscillator(); + int ret = I2C::init(); + + if (ret != PX4_OK) { return ret; } + + uint8_t buf[2] = {}; + + buf[0] = PCA9685_REG_MODE1; + buf[1] = PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_SLEEP_MASK; // put into sleep mode + ret = transfer(buf, 2, nullptr, 0); + + if (OK != ret) { + PX4_ERR("init: i2c::transfer returned %d", ret); + return ret; + } + +#ifdef CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL + buf[1] = PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_SLEEP_MASK | PCA9685_MODE1_EXTCLK_MASK; + ret = transfer(buf, 2, nullptr, 0); // enable EXTCLK if possible + + if (OK != ret) { + PX4_ERR("init: i2c::transfer returned %d", ret); + return ret; + } + +#endif + + buf[0] = PCA9685_REG_MODE2; + buf[1] = PCA9685_DEFAULT_MODE2_CFG; + ret = transfer(buf, 2, nullptr, 0); + + if (OK != ret) { + PX4_ERR("init: i2c::transfer returned %d", ret); + return ret; + } + return PX4_OK; } @@ -63,198 +106,118 @@ int PCA9685::updatePWM(const uint16_t *outputs, unsigned num_outputs) 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 + out[i] = calcRawFromPulse(out[i]); } - setPWM(num_outputs, out); - - return 0; + return writePWM(0, out, num_outputs); } -int PCA9685::setFreq(float freq) +int PCA9685::updateFreq(float freq) { - uint16_t realResolution = floorl((float)PCA9685_CLOCK_FREQ / freq); + uint16_t divider = (uint16_t)round((float)PCA9685_CLOCK_REFERENCE / freq / PCA9685_PWM_RES) - 1; - if (realResolution < PCA9685_PWM_RES) { // unable to provide enough resolution - PX4_DEBUG("frequency too high"); - return -EINVAL; + if (divider > 0x00FF) { + PX4_ERR("frequency too low"); + return PX4_ERROR; } - uint16_t divider = (uint16_t)round((float)PCA9685_CLOCK_FREQ / freq / PCA9685_PWM_RES) - 1; - - if (divider > 0x00FF) { // out of divider - PX4_DEBUG("frequency too low"); - return -EINVAL; + if (divider < 0x0003) { + PX4_ERR("frequency too high"); + return PX4_ERROR; } - 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; + currentFreq = (float)PCA9685_CLOCK_REFERENCE / (float)((divider + 1) * 4096); + PX4_INFO("PCA9685 PWM frequency: target=%.2f real=%.2f", (double)freq, (double)currentFreq); + return setDivider(divider); } -int PCA9685::initReg() +int PCA9685::updateRAW(const uint16_t *outputs, unsigned int num_outputs) { - uint8_t buf[2] = {}; + return writePWM(0, outputs, num_outputs); +} - buf[0] = PCA9685_REG_MODE1; - buf[1] = DEFAULT_MODE1_CFG; - int ret = transfer(buf, 2, nullptr, 0); // make sure oscillator is disabled +int PCA9685::setAllPWM(uint16_t output) +{ + uint16_t val = (uint16_t)roundl((output * currentFreq * PCA9685_PWM_RES / (float)1e6)); + uint8_t buf[] = { + PCA9685_REG_ALLLED_ON_L, + 0x00, 0x00, + (uint8_t)(val & (uint8_t)0xFF), + val != 0 ? (uint8_t)(val >> 8) : (uint8_t)PCA9685_LED_ON_FULL_ON_OFF_MASK + }; + return transfer(buf, sizeof(buf), nullptr, 0); +} - if (OK != ret) { - PX4_ERR("init: i2c::transfer returned %d", ret); - return ret; - } +int PCA9685::sleep() +{ + uint8_t buf[2] = { + PCA9685_REG_MODE1, + PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_SLEEP_MASK + }; + return transfer(buf, 2, nullptr, 0); +} - ret = transfer(buf, 2, nullptr, 0); // enable EXTCLK if possible +int PCA9685::wake() +{ + uint8_t buf[2] = { + PCA9685_REG_MODE1, + PCA9685_DEFAULT_MODE1_CFG + }; + return transfer(buf, 2, nullptr, 0); +} - if (OK != ret) { - PX4_ERR("init: 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_ERR("init: i2c::transfer returned %d", ret); - return ret; - } - - return PX4_OK; +int PCA9685::doRestart() +{ + uint8_t buf[2] = { + PCA9685_REG_MODE1, + PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_RESTART_MASK + }; + return transfer(buf, 2, nullptr, 0); } int PCA9685::probe() { - return I2C::probe(); + int ret = I2C::probe(); + + if (ret != PX4_OK) { return ret; } + + uint8_t buf[2] = {0x00}; + return transfer(buf, 2, buf, 1); } -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("setPWM: i2c::transfer returned %d", ret); - } -} - -void PCA9685::setPWM(uint8_t channel_count, const uint16_t *value) +int PCA9685::writePWM(uint8_t idx, const uint16_t *value, uint8_t num) { 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[0] = PCA9685_REG_LED0 + PCA9685_REG_LED_INCREMENT * idx; + for (int i = 0; i < num; ++i) { 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; + + if (value[i] == 0) { + buf[2 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + buf[3 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + buf[4 + i * PCA9685_REG_LED_INCREMENT] = PCA9685_LED_ON_FULL_ON_OFF_MASK; + + } else if (value[i] == 4096) { + buf[2 + i * PCA9685_REG_LED_INCREMENT] = PCA9685_LED_ON_FULL_ON_OFF_MASK; + buf[3 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + buf[4 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + + } else { + buf[2 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + buf[3 + i * PCA9685_REG_LED_INCREMENT] = (uint8_t)(value[i] & 0xFF); + buf[4 + i * PCA9685_REG_LED_INCREMENT] = (uint8_t)(value[i] >> 8); + } } - int ret = transfer(buf, channel_count * PCA9685_REG_LED_INCREMENT + 1, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("setPWM: i2c::transfer returned %d", ret); - } + return transfer(buf, num * PCA9685_REG_LED_INCREMENT + 1, nullptr, 0); } -void PCA9685::disableAllOutput() -{ - uint8_t buf[5] = {}; - buf[0] = PCA9685_REG_ALLLED_ON_L; - buf[1] = 0x00; - buf[2] = 0x00; - buf[3] = 0x00; - 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) +int PCA9685::setDivider(uint8_t value) { 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; - } -} - -void PCA9685::stopOscillator() -{ - uint8_t buf[2] = {PCA9685_REG_MODE1}; - - // set to sleep - buf[1] = DEFAULT_MODE1_CFG; - int ret = transfer(buf, 2, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("i2c::transfer returned %d", ret); - return; - } -} - -void PCA9685::startOscillator() -{ - uint8_t buf[2] = {PCA9685_REG_MODE1}; - - // clear sleep bit, with restart bit = 0 - buf[1] = DEFAULT_MODE1_CFG & (~PCA9685_MODE1_SLEEP_MASK); - int ret = transfer(buf, 2, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("startOscillator: i2c::transfer returned %d", ret); - return; - } -} - -void PCA9685::triggerRestart() -{ - uint8_t buf[2] = {PCA9685_REG_MODE1}; - - // clear sleep bit, with restart bit = 0 - buf[1] = DEFAULT_MODE1_CFG & (~PCA9685_MODE1_SLEEP_MASK); - buf[1] |= PCA9685_MODE1_RESTART_MASK; - int ret = transfer(buf, 2, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("triggerRestart: i2c::transfer returned %d", ret); - return; - } + return transfer(buf, 2, nullptr, 0); } diff --git a/src/drivers/pca9685_pwm_out/PCA9685.h b/src/drivers/pca9685_pwm_out/PCA9685.h index 5c7740164c..e8bd5eb4d8 100644 --- a/src/drivers/pca9685_pwm_out/PCA9685.h +++ b/src/drivers/pca9685_pwm_out/PCA9685.h @@ -34,10 +34,7 @@ #pragma once #include #include -#include - -namespace drv_pca9685_pwm -{ +#include #define PCA9685_REG_MODE1 0x00 // Mode register 1 #define PCA9685_REG_MODE2 0x01 // Mode register 2 @@ -82,93 +79,98 @@ namespace drv_pca9685_pwm // PRE_SCALE register #define PCA9685_PRE_SCALE_MASK 0xFF +// common sense #define PCA9685_PWM_CHANNEL_COUNT 16 -#define PCA9685_PWM_RES 4096 //Resolution 4096=12bit -/* This should be 25000000 ideally, - * but it seems most chips have its oscillator working at a higher frequency - * Reference: https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/blob/6664ce936210eea53259b814062009d9569a4213/Adafruit_PWMServoDriver.h#L66 */ -#define PCA9685_CLOCK_INT 26075000.0 //25MHz internal clock -#ifndef PCA9685_CLOCK_EXT -#define PCA9685_CLOCK_FREQ PCA9685_CLOCK_INT // use int clk -#else -#define PCA9685_CLOCK_FREQ PCA9685_CLOCK_EXT // use ext clk -#endif +#define PCA9685_PWM_RES 4096 -#define PCA9685_DEVICE_BASE_PATH "/dev/pca9685" -#define PWM_DEFAULT_FREQUENCY 50 // default pwm frequency +namespace drv_pca9685_pwm +{ //! Main class that exports features for PCA9685 chip class PCA9685 : public device::I2C { public: PCA9685(int bus, int addr); + ~PCA9685() override = default; - int Stop(); + int init() override; /* - * outputs formatted to us. + * Write new PWM value to device + * + * *output: pulse width, us */ int updatePWM(const uint16_t *outputs, unsigned num_outputs); - int setFreq(float freq); - - ~PCA9685() override = default; - - int initReg(); - - inline float getFrequency() {return _Freq;} - /* - * disable all of the output + * Set PWM frequency to new value. + * + * Only a few of precious frequency can be set, while others will be rounded to the nearest possible value. + * + * Only allowed when PCA9685 is put into sleep mode + * + * freq: Hz */ - void disableAllOutput(); + int updateFreq(float freq); /* - * turn off oscillator - */ - void stopOscillator(); - - /* - * turn on oscillator + * Write new PWM value to device, in raw counter value + * + * *output: 0~4095 */ - void startOscillator(); + int updateRAW(const uint16_t *outputs, unsigned num_outputs); /* - * turn on output + * Get the real frequency */ - void triggerRestart(); + float inline getFreq() {return currentFreq;} + + uint16_t inline calcRawFromPulse(uint16_t pulse_width) + { + return (uint16_t)roundl((pulse_width * currentFreq * PCA9685_PWM_RES / (float)1e6)); + } + + /* + * Set PWM value on all channels at once + */ + int setAllPWM(uint16_t output); + + /* + * Put PCA9685 into sleep mode + * + * This will disable the clock reference inside PCA9685 + */ + int sleep(); + + /* + * Put PCA9685 out of sleep mode. + * + * Must wait 500 us for oscillator stabilization before outputting anything + */ + int wake(); + + /* + * If PCA9685 is put into sleep without clearing all the outputs, + * then the restart command will be available, and it can bring back PWM output without the + * need of updatePWM() call. + */ + int doRestart(); protected: int probe() override; -#ifdef PCA9685_CLOCL_EXT - static const uint8_t DEFAULT_MODE1_CFG = 0x70; // Auto-Increment, Sleep, EXTCLK -#else - static const uint8_t DEFAULT_MODE1_CFG = 0x30; // Auto-Increment, Sleep -#endif - static const uint8_t DEFAULT_MODE2_CFG = 0x04; // totem pole - - 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); - /* * set clock divider */ - void setDivider(uint8_t value); + int setDivider(uint8_t value); + + /* + * Write PWM value to PCA9685 + */ + int writePWM(uint8_t idx, const uint16_t *value, uint8_t num); private: - + float currentFreq; }; } diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index 4c3ee60c54..0e9bfa63df 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -33,9 +33,10 @@ /** * @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 + * + * This file serves as the wrapper layer for PCA9685 driver, working with parameters + * and scheduling stuffs on PX4 side. + * */ #include @@ -45,11 +46,10 @@ #include #include #include +#include #include "PCA9685.h" -#include - #define PCA9685_DEFAULT_IICBUS 1 #define PCA9685_DEFAULT_ADDRESS (0x40) @@ -59,26 +59,25 @@ using namespace time_literals; class PCA9685Wrapper : public ModuleBase, public OutputModuleInterface { public: - PCA9685Wrapper(int schd_rate_limit = 400); - ~PCA9685Wrapper() override ; + PCA9685Wrapper(); + ~PCA9685Wrapper() override; + PCA9685Wrapper(const PCA9685Wrapper &) = delete; + PCA9685Wrapper operator=(const PCA9685Wrapper &) = delete; int init(); - /** @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; - PCA9685Wrapper(const PCA9685Wrapper &) = delete; - PCA9685Wrapper operator=(const PCA9685Wrapper &) = delete; - int print_status() override; +protected: + void updateParams() override; + private: perf_counter_t _cycle_perf; @@ -86,40 +85,37 @@ private: INIT, WAIT_FOR_OSC, RUNNING - }; - STATE _state{STATE::INIT}; - // used to compare and cancel unecessary scheduling changes caused by parameter update - int32_t _last_fetched_Freq = -1; - // If this value is above zero, then change freq and scheduling in running state. - float _targetFreq = -1.0f; + } state{STATE::INIT}; + PCA9685 *pca9685 = nullptr; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + MixingOutput _mixing_output { + "PCA9685", + PCA9685_PWM_CHANNEL_COUNT, + *this, + MixingOutput::SchedulingPolicy::Disabled, + true + }; + + float param_pwm_freq, previous_pwm_freq; + float param_schd_rate, previous_schd_rate; + uint32_t param_duty_mode; void Run() override; - -protected: - int _schd_rate_limit = 400; - - PCA9685 *pca9685 = nullptr; // driver handle. - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - MixingOutput _mixing_output{"PCA9685", PCA9685_PWM_CHANNEL_COUNT, *this, MixingOutput::SchedulingPolicy::Disabled, true}; }; -PCA9685Wrapper::PCA9685Wrapper(int schd_rate_limit) : +PCA9685Wrapper::PCA9685Wrapper() : OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), - _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), - _schd_rate_limit(schd_rate_limit) + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { } PCA9685Wrapper::~PCA9685Wrapper() { - if (pca9685 != nullptr) { // normally this should not be called. - PX4_DEBUG("Destruction of PCA9685Wrapper without pwmDevice unloaded!"); - pca9685->Stop(); // force stop + if (pca9685 != nullptr) { + pca9685->setAllPWM(0); + pca9685->sleep(); delete pca9685; - pca9685 = nullptr; } perf_free(_cycle_perf); @@ -129,18 +125,7 @@ int PCA9685Wrapper::init() { int ret = pca9685->init(); - if (ret != PX4_OK) { - return ret; - } - - param_t param_h = param_find("PCA9685_RATE"); - - if (param_h != PARAM_INVALID) { - param_get(param_h, &_targetFreq); - - } else { - PX4_DEBUG("PARAM_INVALID: PCA9685_RATE"); - } + if (ret != PX4_OK) { return ret; } this->ChangeWorkQueue(px4::device_bus_to_wq(pca9685->get_device_id())); @@ -154,7 +139,26 @@ int PCA9685Wrapper::init() bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs, unsigned num_control_groups_updated) { - return pca9685->updatePWM(outputs, num_outputs) == 0 ? true : false; + if (state != STATE::RUNNING) { return false; } + + uint16_t low_level_outputs[PCA9685_PWM_CHANNEL_COUNT] = {}; + num_outputs = num_outputs > PCA9685_PWM_CHANNEL_COUNT ? PCA9685_PWM_CHANNEL_COUNT : num_outputs; + + for (uint8_t i = 0; i < num_outputs; ++i) { + if (param_duty_mode & (1 << i)) { + low_level_outputs[i] = outputs[i]; + + } else { + low_level_outputs[i] = pca9685->calcRawFromPulse(outputs[i]); + } + } + + if (pca9685->updateRAW(low_level_outputs, num_outputs) != PX4_OK) { + PX4_ERR("Failed to write PWM to PCA9685"); + return false; + } + + return true; } void PCA9685Wrapper::Run() @@ -163,7 +167,8 @@ void PCA9685Wrapper::Run() ScheduleClear(); _mixing_output.unregister(); - pca9685->Stop(); + pca9685->setAllPWM(0); + pca9685->sleep(); delete pca9685; pca9685 = nullptr; @@ -171,44 +176,27 @@ void PCA9685Wrapper::Run() return; } - perf_begin(_cycle_perf); - - switch (_state) { + switch (state) { case STATE::INIT: - pca9685->initReg(); + updateParams(); + pca9685->updateFreq(param_pwm_freq); + previous_pwm_freq = param_pwm_freq; + previous_schd_rate = param_schd_rate; - if (_targetFreq > 0.0f) { - if (pca9685->setFreq(_targetFreq) != PX4_OK) { - PX4_ERR("failed to set pwm frequency to %.2f, fall back to 50Hz", (double)_targetFreq); - pca9685->setFreq(50.0f); // this should not fail - } - - _targetFreq = -1.0f; - - } else { - // should not happen - PX4_ERR("INIT failed: invalid initial frequency settings"); - } - - pca9685->startOscillator(); - _state = STATE::WAIT_FOR_OSC; + pca9685->wake(); + state = STATE::WAIT_FOR_OSC; ScheduleDelayed(500); break; case STATE::WAIT_FOR_OSC: { - pca9685->triggerRestart(); // start actual outputting - _state = STATE::RUNNING; - float schedule_rate = pca9685->getFrequency(); - - if (_schd_rate_limit < pca9685->getFrequency()) { - schedule_rate = _schd_rate_limit; - } - - ScheduleOnInterval(1000000 / schedule_rate, 1000000 / schedule_rate); + state = STATE::RUNNING; + ScheduleOnInterval(1000000 / param_schd_rate, 0); } break; case STATE::RUNNING: + perf_begin(_cycle_perf); + _mixing_output.update(); // check for parameter updates @@ -219,30 +207,36 @@ void PCA9685Wrapper::Run() // update parameters from storage updateParams(); + + // apply param updates + if ((float)fabs(previous_pwm_freq - param_pwm_freq) > 0.01f) { + previous_pwm_freq = param_pwm_freq; + + ScheduleClear(); + + pca9685->sleep(); + pca9685->updateFreq(param_pwm_freq); + pca9685->wake(); + + // update of PWM freq will always trigger scheduling change + previous_schd_rate = param_schd_rate; + + state = STATE::WAIT_FOR_OSC; + ScheduleDelayed(500); + + } else if ((float)fabs(previous_schd_rate - param_schd_rate) > 0.01f) { + // case when PWM freq not changed but scheduling rate does + previous_schd_rate = param_schd_rate; + ScheduleClear(); + ScheduleOnInterval(1000000 / param_schd_rate, 1000000 / param_schd_rate); + } } _mixing_output.updateSubscriptions(false); - if (_targetFreq > 0.0f) { // check if frequency should be changed - ScheduleClear(); - pca9685->disableAllOutput(); - pca9685->stopOscillator(); - - if (pca9685->setFreq(_targetFreq) != PX4_OK) { - PX4_ERR("failed to set pwm frequency, fall back to 50Hz"); - pca9685->setFreq(50.0f); // this should not fail - } - - _targetFreq = -1.0f; - pca9685->startOscillator(); - _state = STATE::WAIT_FOR_OSC; - ScheduleDelayed(500); - } - + perf_end(_cycle_perf); break; } - - perf_end(_cycle_perf); } int PCA9685Wrapper::print_usage(const char *reason) @@ -254,27 +248,24 @@ int PCA9685Wrapper::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -This module is responsible for generate pwm pulse with PCA9685 chip. +This is a PCA9685 PWM output driver. -It listens on the actuator_controls topics, does the mixing and writes the PWM outputs. +It runs on I2C workqueue which is asynchronous with FC control loop, +fetching the latest mixing result and write them to PCA9685 at its scheduling ticks. -### Implementation -This module depends on ModuleBase and OutputModuleInterface. -IIC communication is based on CDev::I2C +It can do full 12bits output as duty-cycle mode, while also able to output precious pulse width +that can be accepted by most ESCs and servos. ### Examples It is typically started with: -$ pca9685_pwm_out start -a 64 -b 1 +$ pca9685_pwm_out start -a 0x40 -b 1 -The number X can be acquired by executing -`pca9685_pwm_out status` when this driver is running. )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_STRING('a',"0x40","","7-bits I2C address of PCA9685",true); PRINT_MODULE_USAGE_PARAM_INT('b',1,0,255,"bus that pca9685 is connected to",true); - PRINT_MODULE_USAGE_PARAM_INT('r',400,50,400,"schedule rate limit",true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; @@ -282,39 +273,42 @@ The number X can be acquired by executing int PCA9685Wrapper::print_status() { int ret = ModuleBase::print_status(); - PX4_INFO("PCA9685 @I2C Bus %d, address 0x%.2x, true frequency %.5f", + PX4_INFO("PCA9685 @I2C Bus %d, address 0x%.2x, real frequency %.2f", pca9685->get_device_bus(), pca9685->get_device_address(), - (double)(pca9685->getFrequency())); + (double)(pca9685->getFreq())); return ret; } -int PCA9685Wrapper::custom_command(int argc, char **argv) { // only for test use +int PCA9685Wrapper::custom_command(int argc, char **argv) { return PX4_OK; } int PCA9685Wrapper::task_spawn(int argc, char **argv) { - int ch; int address=PCA9685_DEFAULT_ADDRESS; int iicbus=PCA9685_DEFAULT_IICBUS; - int schd_rate_limit=400; int myoptind = 1; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "a:b:r:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'a': - address = atoi(myoptarg); + errno = 0; + address = strtol(myoptarg, nullptr, 16); + if (errno != 0) { + PX4_WARN("Invalid address"); + return PX4_ERROR; + } break; case 'b': - iicbus = atoi(myoptarg); - break; - - case 'r': - schd_rate_limit = atoi(myoptarg); + iicbus = strtol(myoptarg, nullptr, 10); + if (errno != 0) { + PX4_WARN("Invalid bus"); + return PX4_ERROR; + } break; case '?': @@ -326,7 +320,7 @@ int PCA9685Wrapper::task_spawn(int argc, char **argv) { } } - auto *instance = new PCA9685Wrapper(schd_rate_limit); + auto *instance = new PCA9685Wrapper(); if (instance) { _object.store(instance); @@ -358,6 +352,31 @@ int PCA9685Wrapper::task_spawn(int argc, char **argv) { return PX4_ERROR; } +void PCA9685Wrapper::updateParams() { + ModuleParams::updateParams(); + + param_t param = param_find("PCA9685_SCHD_HZ"); + if (param != PARAM_INVALID) { + param_get(param, ¶m_schd_rate); + } else { + PX4_ERR("param PCA9685_SCHD_HZ not found"); + } + + param = param_find("PCA9685_PWM_FREQ"); + if (param != PARAM_INVALID) { + param_get(param, ¶m_pwm_freq); + } else { + PX4_ERR("param PCA9685_PWM_FREQ not found"); + } + + param = param_find("PCA9685_DUTY_EN"); + if (param != PARAM_INVALID) { + param_get(param, (int32_t*)¶m_duty_mode); + } else { + PX4_ERR("param PCA9685_DUTY_EN not found"); + } +} + extern "C" __EXPORT int pca9685_pwm_out_main(int argc, char *argv[]){ return PCA9685Wrapper::main(argc, argv); } diff --git a/src/drivers/pca9685_pwm_out/module.yaml b/src/drivers/pca9685_pwm_out/module.yaml index 78801fbdb6..198810f8cb 100644 --- a/src/drivers/pca9685_pwm_out/module.yaml +++ b/src/drivers/pca9685_pwm_out/module.yaml @@ -5,23 +5,75 @@ actuator_output: channel_label: 'Channel' standard_params: disarmed: { min: 800, max: 2200, default: 1000 } - min: { min: 800, max: 1400, default: 1000 } - max: { min: 1600, max: 2200, default: 2000 } + min: { min: 800, max: 1400, default: 1100 } + max: { min: 1600, max: 2200, default: 1900 } failsafe: { min: 800, max: 2200 } + custom_params: + - name: 'DUTY_EN' + label: "Duty-Cycle\n Mode" + index_offset: -1 + show_as: bitset + advanced: true num_channels: 16 + parameters: - - group: PCA9685 - definitions: - PCA9685_RATE: - description: - short: PWM frequency for PCA9685 - long: | - Update rate at which the PWM signal is sent to the ESC. - type: float - decimal: 1 - increment: 0.1 - default: 50 - min: 50 - max: 450 - unit: Hz - reboot_required: true + - group: Actuator Outputs + definitions: + PCA9685_SCHD_HZ: + description: + short: PWM update rate + long: | + Controls the update rate of PWM output. + Flight Controller will inform those numbers of update events in a second, to PCA9685. + Higher update rate will consume more I2C bandwidth, which may even lead to worse + output latency, or completely block I2C bus. + type: float + decimal: 2 + min: 50.0 + max: 400.0 + default: 50.0 + PCA9685_PWM_FREQ: + description: + short: PWM cycle frequency + long: | + Controls the PWM frequency at timing perspective. + This is independent from PWM update frequency, as PCA9685 is capable to output + without being continuously commanded by FC. + Higher frequency leads to more accurate pulse width, but some ESCs and servos may not support it. + This parameter should be set to the same value as PWM update rate in most case. + This parameter MUST NOT exceed upper limit of 400.0, if any outputs as generic 1000~2000us + pulse width is desired. Frequency higher than 400 only makes sense in duty-cycle mode. + type: float + decimal: 2 + min: 23.8 + max: 1525.87 + default: 50.0 + PCA9685_DUTY_EN: + description: + short: Put the selected channels into Duty-Cycle output mode + long: | + The driver will output standard pulse-width encoded signal without this bit set. + To make PCA9685 output in duty-cycle fashion, please enable the corresponding + channel bit here and adjusting standard params to suit your need. + The driver will have 12bits resolution for duty-cycle output. That means to achieve 0% to 100% + output range on one channel, the corresponding params MIN and MAX for the channel should be set + to 0 and 4096. Other standard params follows the same rule. + type: bitmask + bit: + 0: Put CH1 to Duty-Cycle mode + 1: Put CH2 to Duty-Cycle mode + 2: Put CH3 to Duty-Cycle mode + 3: Put CH4 to Duty-Cycle mode + 4: Put CH5 to Duty-Cycle mode + 5: Put CH6 to Duty-Cycle mode + 6: Put CH7 to Duty-Cycle mode + 7: Put CH8 to Duty-Cycle mode + 8: Put CH9 to Duty-Cycle mode + 9: Put CH10 to Duty-Cycle mode + 10: Put CH11 to Duty-Cycle mode + 11: Put CH12 to Duty-Cycle mode + 12: Put CH13 to Duty-Cycle mode + 13: Put CH14 to Duty-Cycle mode + 14: Put CH15 to Duty-Cycle mode + 15: Put CH16 to Duty-Cycle mode + default: 0