dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

driver/pca9685_pwm_output: bugfixs & support outputting in duty-cycle mode (#21528) 

- make it work again
 - also supports Kconfig based clk source selection
 - adapt to recent changes of default PWM limits
 - support outputting in duty-cycle mode
 - i2c addr use use hex representation
 - revert back to common min/max value & move duty-cycle mode to advanced
This commit is contained in:
SalimTerryLi 2023-10-22 01:39:45 +08:00 committed by GitHub
parent 63b5c790b7
commit f68f88b97c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 413 additions and 359 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
boards/scumaker/pilotpi/default.px4board
View File

@ -16,6 +16,8 @@ CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
CONFIG_DRIVERS_PCA9685_PWM_OUT=y CONFIG_DRIVERS_PCA9685_PWM_OUT=y
CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL=y
CONFIG_PCA9685_EXTERNAL_CRYSTAL_FREQ=25000000
CONFIG_COMMON_RC=y CONFIG_COMMON_RC=y
CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_RC_INPUT=y
CONFIG_DRIVERS_SMART_BATTERY_BATMON=y CONFIG_DRIVERS_SMART_BATTERY_BATMON=y

18
src/drivers/pca9685_pwm_out/Kconfig
View File

@ -2,4 +2,20 @@ menuconfig DRIVERS_PCA9685_PWM_OUT
bool "pca9685_pwm_out" bool "pca9685_pwm_out"
default n default n
---help--- ---help---
Enable support for pca9685_pwm_out 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

289
src/drivers/pca9685_pwm_out/PCA9685.cpp
View File

@ -32,23 +32,66 @@
****************************************************************************/ ****************************************************************************/
#include <px4_log.h> #include <px4_log.h>
#include <px4_defines.h>
#include <cmath> #include <cmath>
#include "PCA9685.h" #include "PCA9685.h"
#include <px4_platform_common/sem.hpp>
#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; using namespace drv_pca9685_pwm;
PCA9685::PCA9685(int bus, int addr): 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(); int ret = I2C::init();
stopOscillator();
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; 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); memcpy(out, outputs, sizeof(uint16_t) * num_outputs);
for (unsigned i = 0; i < num_outputs; ++i) { 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 writePWM(0, out, num_outputs);
return 0;
} }
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 if (divider > 0x00FF) {
PX4_DEBUG("frequency too high"); PX4_ERR("frequency too low");
return -EINVAL; return PX4_ERROR;
} }
uint16_t divider = (uint16_t)round((float)PCA9685_CLOCK_FREQ / freq / PCA9685_PWM_RES) - 1; if (divider < 0x0003) {
PX4_ERR("frequency too high");
if (divider > 0x00FF) { // out of divider return PX4_ERROR;
PX4_DEBUG("frequency too low");
return -EINVAL;
} }
float freq_err = ((float)PCA9685_CLOCK_FREQ / (float)(divider + (uint16_t)1) currentFreq = (float)PCA9685_CLOCK_REFERENCE / (float)((divider + 1) * 4096);
- (float)(freq * PCA9685_PWM_RES)) PX4_INFO("PCA9685 PWM frequency: target=%.2f real=%.2f", (double)freq, (double)currentFreq);
/ (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;
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; int PCA9685::setAllPWM(uint16_t output)
buf[1] = DEFAULT_MODE1_CFG; {
int ret = transfer(buf, 2, nullptr, 0); // make sure oscillator is disabled 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) { int PCA9685::sleep()
PX4_ERR("init: i2c::transfer returned %d", ret); {
return ret; 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) { int PCA9685::doRestart()
PX4_ERR("init: i2c::transfer returned %d", ret); {
return ret; uint8_t buf[2] = {
} PCA9685_REG_MODE1,
PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_RESTART_MASK
buf[0] = PCA9685_REG_MODE2; };
buf[1] = DEFAULT_MODE2_CFG; return transfer(buf, 2, nullptr, 0);
ret = transfer(buf, 2, nullptr, 0);
if (OK != ret) {
PX4_ERR("init: i2c::transfer returned %d", ret);
return ret;
}
return PX4_OK;
} }
int PCA9685::probe() 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) int PCA9685::writePWM(uint8_t idx, const uint16_t *value, uint8_t num)
{
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)
{ {
uint8_t buf[PCA9685_PWM_CHANNEL_COUNT * PCA9685_REG_LED_INCREMENT + 1] = {}; uint8_t buf[PCA9685_PWM_CHANNEL_COUNT * PCA9685_REG_LED_INCREMENT + 1] = {};
buf[0] = PCA9685_REG_LED0; buf[0] = PCA9685_REG_LED0 + PCA9685_REG_LED_INCREMENT * idx;
for (int i = 0; i < channel_count; ++i) {
if (value[i] >= 4096) {
PX4_DEBUG("invalid pwm value");
return;
}
for (int i = 0; i < num; ++i) {
buf[1 + i * PCA9685_REG_LED_INCREMENT] = 0x00; 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); if (value[i] == 0) {
buf[4 + i * PCA9685_REG_LED_INCREMENT] = value[i] != 0 ? ((uint8_t)(value[i] >> (uint8_t)8)) : buf[2 + i * PCA9685_REG_LED_INCREMENT] = 0x00;
PCA9685_LED_ON_FULL_ON_OFF_MASK; 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); return transfer(buf, num * PCA9685_REG_LED_INCREMENT + 1, nullptr, 0);
if (OK != ret) {
PX4_DEBUG("setPWM: i2c::transfer returned %d", ret);
}
} }
void PCA9685::disableAllOutput() int PCA9685::setDivider(uint8_t value)
{
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)
{ {
uint8_t buf[2] = {}; uint8_t buf[2] = {};
buf[0] = PCA9685_REG_PRE_SCALE; buf[0] = PCA9685_REG_PRE_SCALE;
buf[1] = value; buf[1] = value;
int ret = transfer(buf, 2, nullptr, 0); return 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;
}
} }

122
src/drivers/pca9685_pwm_out/PCA9685.h
View File

@ -34,10 +34,7 @@
#pragma once #pragma once
#include <cstdint> #include <cstdint>
#include <drivers/device/i2c.h> #include <drivers/device/i2c.h>
#include <drivers/drv_pwm_output.h> #include <px4_boardconfig.h>
namespace drv_pca9685_pwm
{
#define PCA9685_REG_MODE1 0x00 // Mode register 1 #define PCA9685_REG_MODE1 0x00 // Mode register 1
#define PCA9685_REG_MODE2 0x01 // Mode register 2 #define PCA9685_REG_MODE2 0x01 // Mode register 2
@ -82,93 +79,98 @@ namespace drv_pca9685_pwm
// PRE_SCALE register // PRE_SCALE register
#define PCA9685_PRE_SCALE_MASK 0xFF #define PCA9685_PRE_SCALE_MASK 0xFF
// common sense
#define PCA9685_PWM_CHANNEL_COUNT 16 #define PCA9685_PWM_CHANNEL_COUNT 16
#define PCA9685_PWM_RES 4096 //Resolution 4096=12bit #define PCA9685_PWM_RES 4096
/* 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_DEVICE_BASE_PATH "/dev/pca9685" namespace drv_pca9685_pwm
#define PWM_DEFAULT_FREQUENCY 50 // default pwm frequency {
//! Main class that exports features for PCA9685 chip //! Main class that exports features for PCA9685 chip
class PCA9685 : public device::I2C class PCA9685 : public device::I2C
{ {
public: public:
PCA9685(int bus, int addr); 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 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 * Write new PWM value to device, in raw counter value
*/ *
void stopOscillator(); * *output: 0~4095
/*
* turn on oscillator
*/ */
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: protected:
int probe() override; 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 * 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: private:
float currentFreq;
}; };
} }

255
src/drivers/pca9685_pwm_out/main.cpp
View File

@ -33,9 +33,10 @@
/** /**
* @file pca9685/main.cpp * @file pca9685/main.cpp
* A cross-platform driver and wrapper for pca9685. *
* Designed to support all control-groups by binding to correct mixer files * This file serves as the wrapper layer for PCA9685 driver, working with parameters
* @author SalimTerryLi <lhf2613@gmail.com> * and scheduling stuffs on PX4 side.
*
*/ */
#include <px4_log.h> #include <px4_log.h>
@ -45,11 +46,10 @@
#include <lib/perf/perf_counter.h> #include <lib/perf/perf_counter.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <px4_platform_common/getopt.h> #include <px4_platform_common/getopt.h>
#include <px4_platform_common/sem.hpp>
#include "PCA9685.h" #include "PCA9685.h"
#include <px4_platform_common/sem.hpp>
#define PCA9685_DEFAULT_IICBUS 1 #define PCA9685_DEFAULT_IICBUS 1
#define PCA9685_DEFAULT_ADDRESS (0x40) #define PCA9685_DEFAULT_ADDRESS (0x40)
@ -59,26 +59,25 @@ using namespace time_literals;
class PCA9685Wrapper : public ModuleBase<PCA9685Wrapper>, public OutputModuleInterface class PCA9685Wrapper : public ModuleBase<PCA9685Wrapper>, public OutputModuleInterface
{ {
public: public:
PCA9685Wrapper(int schd_rate_limit = 400); PCA9685Wrapper();
~PCA9685Wrapper() override ; ~PCA9685Wrapper() override;
PCA9685Wrapper(const PCA9685Wrapper &) = delete;
PCA9685Wrapper operator=(const PCA9685Wrapper &) = delete;
int init(); int init();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]); static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]); static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr); static int print_usage(const char *reason = nullptr);
bool updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs, bool updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs,
unsigned num_control_groups_updated) override; unsigned num_control_groups_updated) override;
PCA9685Wrapper(const PCA9685Wrapper &) = delete;
PCA9685Wrapper operator=(const PCA9685Wrapper &) = delete;
int print_status() override; int print_status() override;
protected:
void updateParams() override;
private: private:
perf_counter_t _cycle_perf; perf_counter_t _cycle_perf;
@ -86,40 +85,37 @@ private:
INIT, INIT,
WAIT_FOR_OSC, WAIT_FOR_OSC,
RUNNING RUNNING
}; } state{STATE::INIT};
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;
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; 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), OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
_schd_rate_limit(schd_rate_limit)
{ {
} }
PCA9685Wrapper::~PCA9685Wrapper() PCA9685Wrapper::~PCA9685Wrapper()
{ {
if (pca9685 != nullptr) { // normally this should not be called. if (pca9685 != nullptr) {
PX4_DEBUG("Destruction of PCA9685Wrapper without pwmDevice unloaded!"); pca9685->setAllPWM(0);
pca9685->Stop(); // force stop pca9685->sleep();
delete pca9685; delete pca9685;
pca9685 = nullptr;
} }
perf_free(_cycle_perf); perf_free(_cycle_perf);
@ -129,18 +125,7 @@ int PCA9685Wrapper::init()
{ {
int ret = pca9685->init(); int ret = pca9685->init();
if (ret != PX4_OK) { if (ret != PX4_OK) { return ret; }
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");
}
this->ChangeWorkQueue(px4::device_bus_to_wq(pca9685->get_device_id())); 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, bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs,
unsigned num_control_groups_updated) 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() void PCA9685Wrapper::Run()
@ -163,7 +167,8 @@ void PCA9685Wrapper::Run()
ScheduleClear(); ScheduleClear();
_mixing_output.unregister(); _mixing_output.unregister();
pca9685->Stop(); pca9685->setAllPWM(0);
pca9685->sleep();
delete pca9685; delete pca9685;
pca9685 = nullptr; pca9685 = nullptr;
@ -171,44 +176,27 @@ void PCA9685Wrapper::Run()
return; return;
} }
perf_begin(_cycle_perf); switch (state) {
switch (_state) {
case STATE::INIT: 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) { pca9685->wake();
if (pca9685->setFreq(_targetFreq) != PX4_OK) { state = STATE::WAIT_FOR_OSC;
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;
ScheduleDelayed(500); ScheduleDelayed(500);
break; break;
case STATE::WAIT_FOR_OSC: { case STATE::WAIT_FOR_OSC: {
pca9685->triggerRestart(); // start actual outputting state = STATE::RUNNING;
_state = STATE::RUNNING; ScheduleOnInterval(1000000 / param_schd_rate, 0);
float schedule_rate = pca9685->getFrequency();
if (_schd_rate_limit < pca9685->getFrequency()) {
schedule_rate = _schd_rate_limit;
}
ScheduleOnInterval(1000000 / schedule_rate, 1000000 / schedule_rate);
} }
break; break;
case STATE::RUNNING: case STATE::RUNNING:
perf_begin(_cycle_perf);
_mixing_output.update(); _mixing_output.update();
// check for parameter updates // check for parameter updates
@ -219,30 +207,36 @@ void PCA9685Wrapper::Run()
// update parameters from storage // update parameters from storage
updateParams(); 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); _mixing_output.updateSubscriptions(false);
if (_targetFreq > 0.0f) { // check if frequency should be changed perf_end(_cycle_perf);
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);
}
break; break;
} }
perf_end(_cycle_perf);
} }
int PCA9685Wrapper::print_usage(const char *reason) int PCA9685Wrapper::print_usage(const char *reason)
@ -254,27 +248,24 @@ int PCA9685Wrapper::print_usage(const char *reason)
PRINT_MODULE_DESCRIPTION( PRINT_MODULE_DESCRIPTION(
R"DESCR_STR( R"DESCR_STR(
### Description ### 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 It can do full 12bits output as duty-cycle mode, while also able to output precious pulse width
This module depends on ModuleBase and OutputModuleInterface. that can be accepted by most ESCs and servos.
IIC communication is based on CDev::I2C
### Examples ### Examples
It is typically started with: 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"); )DESCR_STR");
PRINT_MODULE_USAGE_NAME("pca9685_pwm_out", "driver"); PRINT_MODULE_USAGE_NAME("pca9685_pwm_out", "driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task"); 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","<addr>","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('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(); PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0; return 0;
@ -282,39 +273,42 @@ The number X can be acquired by executing
int PCA9685Wrapper::print_status() { int PCA9685Wrapper::print_status() {
int ret = ModuleBase::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_bus(),
pca9685->get_device_address(), pca9685->get_device_address(),
(double)(pca9685->getFrequency())); (double)(pca9685->getFreq()));
return ret; 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; return PX4_OK;
} }
int PCA9685Wrapper::task_spawn(int argc, char **argv) { int PCA9685Wrapper::task_spawn(int argc, char **argv) {
int ch; int ch;
int address=PCA9685_DEFAULT_ADDRESS; int address=PCA9685_DEFAULT_ADDRESS;
int iicbus=PCA9685_DEFAULT_IICBUS; int iicbus=PCA9685_DEFAULT_IICBUS;
int schd_rate_limit=400;
int myoptind = 1; int myoptind = 1;
const char *myoptarg = nullptr; 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) { switch (ch) {
case 'a': case 'a':
address = atoi(myoptarg); errno = 0;
address = strtol(myoptarg, nullptr, 16);
if (errno != 0) {
PX4_WARN("Invalid address");
return PX4_ERROR;
}
break; break;
case 'b': case 'b':
iicbus = atoi(myoptarg); iicbus = strtol(myoptarg, nullptr, 10);
break; if (errno != 0) {
PX4_WARN("Invalid bus");
case 'r': return PX4_ERROR;
schd_rate_limit = atoi(myoptarg); }
break; break;
case '?': 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) { if (instance) {
_object.store(instance); _object.store(instance);
@ -358,6 +352,31 @@ int PCA9685Wrapper::task_spawn(int argc, char **argv) {
return PX4_ERROR; return PX4_ERROR;
} }
void PCA9685Wrapper::updateParams() {
ModuleParams::updateParams();
param_t param = param_find("PCA9685_SCHD_HZ");
if (param != PARAM_INVALID) {
param_get(param, &param_schd_rate);
} else {
PX4_ERR("param PCA9685_SCHD_HZ not found");
}
param = param_find("PCA9685_PWM_FREQ");
if (param != PARAM_INVALID) {
param_get(param, &param_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*)&param_duty_mode);
} else {
PX4_ERR("param PCA9685_DUTY_EN not found");
}
}
extern "C" __EXPORT int pca9685_pwm_out_main(int argc, char *argv[]){ extern "C" __EXPORT int pca9685_pwm_out_main(int argc, char *argv[]){
return PCA9685Wrapper::main(argc, argv); return PCA9685Wrapper::main(argc, argv);
} }

86
src/drivers/pca9685_pwm_out/module.yaml
View File

@ -5,23 +5,75 @@ actuator_output:
channel_label: 'Channel' channel_label: 'Channel'
standard_params: standard_params:
disarmed: { min: 800, max: 2200, default: 1000 } disarmed: { min: 800, max: 2200, default: 1000 }
min: { min: 800, max: 1400, default: 1000 } min: { min: 800, max: 1400, default: 1100 }
max: { min: 1600, max: 2200, default: 2000 } max: { min: 1600, max: 2200, default: 1900 }
failsafe: { min: 800, max: 2200 } 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 num_channels: 16
parameters: parameters:
- group: PCA9685 - group: Actuator Outputs
definitions: definitions:
PCA9685_RATE: PCA9685_SCHD_HZ:
description: description:
short: PWM frequency for PCA9685 short: PWM update rate
long: | long: |
Update rate at which the PWM signal is sent to the ESC. Controls the update rate of PWM output.
type: float Flight Controller will inform those numbers of update events in a second, to PCA9685.
decimal: 1 Higher update rate will consume more I2C bandwidth, which may even lead to worse
increment: 0.1 output latency, or completely block I2C bus.
default: 50 type: float
min: 50 decimal: 2
max: 450 min: 50.0
unit: Hz max: 400.0
reboot_required: true 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