mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-01 02:55:07 +08:00
pca9685_pwm_out: small improvement (#16196)
* reduce I2C transfers and enable EXTCLK support * schedule rate limit arg * apply state machine and do actual init in Run()
This commit is contained in:
@@ -43,15 +43,10 @@ PCA9685::PCA9685(int bus, int addr):
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int PCA9685::Start()
|
|
||||||
{
|
|
||||||
int ret = init();
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
int PCA9685::Stop()
|
int PCA9685::Stop()
|
||||||
{
|
{
|
||||||
disableAllOutput();
|
disableAllOutput();
|
||||||
|
stopOscillator();
|
||||||
return PX4_OK;
|
return PX4_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -59,7 +54,7 @@ int PCA9685::updatePWM(const uint16_t *outputs, unsigned num_outputs)
|
|||||||
{
|
{
|
||||||
if (num_outputs > PCA9685_PWM_CHANNEL_COUNT) {
|
if (num_outputs > PCA9685_PWM_CHANNEL_COUNT) {
|
||||||
num_outputs = PCA9685_PWM_CHANNEL_COUNT;
|
num_outputs = PCA9685_PWM_CHANNEL_COUNT;
|
||||||
PX4_WARN("PCA9685 can only drive up to 16 channels");
|
PX4_DEBUG("PCA9685 can only drive up to 16 channels");
|
||||||
}
|
}
|
||||||
|
|
||||||
uint16_t out[PCA9685_PWM_CHANNEL_COUNT];
|
uint16_t out[PCA9685_PWM_CHANNEL_COUNT];
|
||||||
@@ -107,22 +102,23 @@ int PCA9685::setFreq(float freq)
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int PCA9685::init()
|
int PCA9685::initReg()
|
||||||
{
|
{
|
||||||
int ret = I2C::init();
|
uint8_t buf[2] = {};
|
||||||
|
|
||||||
if (ret != PX4_OK) {
|
buf[0] = PCA9685_REG_MODE1;
|
||||||
PX4_DEBUG("I2C init failed.");
|
buf[1] = DEFAULT_MODE1_CFG;
|
||||||
|
int ret = transfer(buf, 2, nullptr, 0); // make sure oscillator is disabled
|
||||||
|
|
||||||
|
if (OK != ret) {
|
||||||
|
PX4_ERR("init: i2c::transfer returned %d", ret);
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t buf[2] = {};
|
ret = transfer(buf, 2, nullptr, 0); // enable EXTCLK if possible
|
||||||
buf[0] = PCA9685_REG_MODE1;
|
|
||||||
buf[1] = DEFAULT_MODE1_CFG;
|
|
||||||
ret = transfer(buf, 2, nullptr, 0);
|
|
||||||
|
|
||||||
if (OK != ret) {
|
if (OK != ret) {
|
||||||
PX4_DEBUG("i2c::transfer returned %d", ret);
|
PX4_ERR("init: i2c::transfer returned %d", ret);
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -131,12 +127,10 @@ int PCA9685::init()
|
|||||||
ret = transfer(buf, 2, nullptr, 0);
|
ret = transfer(buf, 2, nullptr, 0);
|
||||||
|
|
||||||
if (OK != ret) {
|
if (OK != ret) {
|
||||||
PX4_DEBUG("i2c::transfer returned %d", ret);
|
PX4_ERR("init: i2c::transfer returned %d", ret);
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
disableAllOutput();
|
|
||||||
|
|
||||||
return PX4_OK;
|
return PX4_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -162,7 +156,7 @@ void PCA9685::setPWM(uint8_t channel, const uint16_t &value)
|
|||||||
int ret = transfer(buf, 5, nullptr, 0);
|
int ret = transfer(buf, 5, nullptr, 0);
|
||||||
|
|
||||||
if (OK != ret) {
|
if (OK != ret) {
|
||||||
PX4_DEBUG("i2c::transfer returned %d", ret);
|
PX4_DEBUG("setPWM: i2c::transfer returned %d", ret);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -187,7 +181,7 @@ void PCA9685::setPWM(uint8_t channel_count, const uint16_t *value)
|
|||||||
int ret = transfer(buf, channel_count * PCA9685_REG_LED_INCREMENT + 1, nullptr, 0);
|
int ret = transfer(buf, channel_count * PCA9685_REG_LED_INCREMENT + 1, nullptr, 0);
|
||||||
|
|
||||||
if (OK != ret) {
|
if (OK != ret) {
|
||||||
PX4_DEBUG("i2c::transfer returned %d", ret);
|
PX4_DEBUG("setPWM: i2c::transfer returned %d", ret);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -197,7 +191,7 @@ void PCA9685::disableAllOutput()
|
|||||||
buf[0] = PCA9685_REG_ALLLED_ON_L;
|
buf[0] = PCA9685_REG_ALLLED_ON_L;
|
||||||
buf[1] = 0x00;
|
buf[1] = 0x00;
|
||||||
buf[2] = 0x00;
|
buf[2] = 0x00;
|
||||||
buf[3] = (uint8_t)(0x00 & (uint8_t)0xFF);
|
buf[3] = 0x00;
|
||||||
buf[4] = PCA9685_LED_ON_FULL_ON_OFF_MASK;
|
buf[4] = PCA9685_LED_ON_FULL_ON_OFF_MASK;
|
||||||
|
|
||||||
int ret = transfer(buf, 5, nullptr, 0);
|
int ret = transfer(buf, 5, nullptr, 0);
|
||||||
@@ -209,8 +203,6 @@ void PCA9685::disableAllOutput()
|
|||||||
|
|
||||||
void PCA9685::setDivider(uint8_t value)
|
void PCA9685::setDivider(uint8_t value)
|
||||||
{
|
{
|
||||||
disableAllOutput();
|
|
||||||
stopOscillator();
|
|
||||||
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;
|
||||||
@@ -220,25 +212,15 @@ void PCA9685::setDivider(uint8_t value)
|
|||||||
PX4_DEBUG("i2c::transfer returned %d", ret);
|
PX4_DEBUG("i2c::transfer returned %d", ret);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
restartOscillator();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void PCA9685::stopOscillator()
|
void PCA9685::stopOscillator()
|
||||||
{
|
{
|
||||||
uint8_t buf[2] = {PCA9685_REG_MODE1};
|
uint8_t buf[2] = {PCA9685_REG_MODE1};
|
||||||
int ret = transfer(buf, 1, buf, 1);
|
|
||||||
|
|
||||||
if (OK != ret) {
|
// set to sleep
|
||||||
PX4_DEBUG("i2c::transfer returned %d", ret);
|
buf[1] = DEFAULT_MODE1_CFG;
|
||||||
return;
|
int ret = transfer(buf, 2, nullptr, 0);
|
||||||
}
|
|
||||||
|
|
||||||
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) {
|
if (OK != ret) {
|
||||||
PX4_DEBUG("i2c::transfer returned %d", ret);
|
PX4_DEBUG("i2c::transfer returned %d", ret);
|
||||||
@@ -246,34 +228,31 @@ void PCA9685::stopOscillator()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void PCA9685::restartOscillator()
|
void PCA9685::startOscillator()
|
||||||
{
|
{
|
||||||
uint8_t buf[2] = {PCA9685_REG_MODE1};
|
uint8_t buf[2] = {PCA9685_REG_MODE1};
|
||||||
int ret = transfer(buf, 1, buf + 1, 1);
|
|
||||||
|
// 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) {
|
if (OK != ret) {
|
||||||
PX4_DEBUG("i2c::transfer returned %d", ret);
|
PX4_DEBUG("startOscillator: i2c::transfer returned %d", ret);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
}
|
||||||
// clear restart and sleep bit
|
|
||||||
buf[1] &= PCA9685_MODE1_EXTCLK_MASK | PCA9685_MODE1_AI_MASK
|
void PCA9685::triggerRestart()
|
||||||
| PCA9685_MODE1_SUB1_MASK | PCA9685_MODE1_SUB2_MASK
|
{
|
||||||
| PCA9685_MODE1_SUB3_MASK | PCA9685_MODE1_ALLCALL_MASK;
|
uint8_t buf[2] = {PCA9685_REG_MODE1};
|
||||||
buf[0] = PCA9685_REG_MODE1;
|
|
||||||
ret = transfer(buf, 2, nullptr, 0);
|
// clear sleep bit, with restart bit = 0
|
||||||
|
buf[1] = DEFAULT_MODE1_CFG & (~PCA9685_MODE1_SLEEP_MASK);
|
||||||
if (OK != ret) {
|
buf[1] |= PCA9685_MODE1_RESTART_MASK;
|
||||||
PX4_DEBUG("i2c::transfer returned %d", ret);
|
int ret = transfer(buf, 2, nullptr, 0);
|
||||||
return;
|
|
||||||
}
|
if (OK != ret) {
|
||||||
|
PX4_DEBUG("triggerRestart: i2c::transfer returned %d", ret);
|
||||||
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;
|
return;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -88,7 +88,7 @@ namespace drv_pca9685_pwm
|
|||||||
* but it seems most chips have its oscillator working at a higher frequency
|
* 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 */
|
* 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
|
#define PCA9685_CLOCK_INT 26075000.0 //25MHz internal clock
|
||||||
#ifndef PCA9685_CLOCL_EXT
|
#ifndef PCA9685_CLOCK_EXT
|
||||||
#define PCA9685_CLOCK_FREQ PCA9685_CLOCK_INT // use int clk
|
#define PCA9685_CLOCK_FREQ PCA9685_CLOCK_INT // use int clk
|
||||||
#else
|
#else
|
||||||
#define PCA9685_CLOCK_FREQ PCA9685_CLOCK_EXT // use ext clk
|
#define PCA9685_CLOCK_FREQ PCA9685_CLOCK_EXT // use ext clk
|
||||||
@@ -103,8 +103,6 @@ class PCA9685 : public device::I2C
|
|||||||
public:
|
public:
|
||||||
PCA9685(int bus, int addr);
|
PCA9685(int bus, int addr);
|
||||||
|
|
||||||
int Start();
|
|
||||||
|
|
||||||
int Stop();
|
int Stop();
|
||||||
|
|
||||||
/*
|
/*
|
||||||
@@ -116,15 +114,39 @@ public:
|
|||||||
|
|
||||||
~PCA9685() override = default;
|
~PCA9685() override = default;
|
||||||
|
|
||||||
int init() override;
|
int initReg();
|
||||||
|
|
||||||
inline float getFrequency() {return _Freq;}
|
inline float getFrequency() {return _Freq;}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* disable all of the output
|
||||||
|
*/
|
||||||
|
void disableAllOutput();
|
||||||
|
|
||||||
|
/*
|
||||||
|
* turn off oscillator
|
||||||
|
*/
|
||||||
|
void stopOscillator();
|
||||||
|
|
||||||
|
/*
|
||||||
|
* turn on oscillator
|
||||||
|
*/
|
||||||
|
void startOscillator();
|
||||||
|
|
||||||
|
/*
|
||||||
|
* turn on output
|
||||||
|
*/
|
||||||
|
void triggerRestart();
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
int probe() override;
|
int probe() override;
|
||||||
|
|
||||||
static const uint8_t DEFAULT_MODE1_CFG = 0x20;
|
#ifdef PCA9685_CLOCL_EXT
|
||||||
static const uint8_t DEFAULT_MODE2_CFG = 0x04;
|
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;
|
float _Freq = PWM_DEFAULT_FREQUENCY;
|
||||||
|
|
||||||
@@ -140,26 +162,11 @@ protected:
|
|||||||
*/
|
*/
|
||||||
void setPWM(uint8_t channel_count, const uint16_t *value);
|
void setPWM(uint8_t channel_count, const uint16_t *value);
|
||||||
|
|
||||||
/*
|
|
||||||
* disable all of the output
|
|
||||||
*/
|
|
||||||
void disableAllOutput();
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* set clock divider
|
* set clock divider
|
||||||
* this func has Super Cow Powers
|
|
||||||
*/
|
*/
|
||||||
void setDivider(uint8_t value);
|
void setDivider(uint8_t value);
|
||||||
|
|
||||||
/*
|
|
||||||
* turn off oscillator
|
|
||||||
*/
|
|
||||||
void stopOscillator();
|
|
||||||
|
|
||||||
/*
|
|
||||||
* restart output
|
|
||||||
*/
|
|
||||||
void restartOscillator();
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -54,12 +54,12 @@
|
|||||||
|
|
||||||
using namespace drv_pca9685_pwm;
|
using namespace drv_pca9685_pwm;
|
||||||
|
|
||||||
class PWMDriverWrapper : public cdev::CDev, public ModuleBase<PWMDriverWrapper>, public OutputModuleInterface
|
class PCA9685Wrapper : public cdev::CDev, public ModuleBase<PCA9685Wrapper>, public OutputModuleInterface
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
|
||||||
PWMDriverWrapper();
|
PCA9685Wrapper(int schd_rate_limit = 400);
|
||||||
~PWMDriverWrapper() override ;
|
~PCA9685Wrapper() override ;
|
||||||
|
|
||||||
int init() override;
|
int init() override;
|
||||||
|
|
||||||
@@ -77,8 +77,8 @@ public:
|
|||||||
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;
|
||||||
|
|
||||||
PWMDriverWrapper(const PWMDriverWrapper &) = delete;
|
PCA9685Wrapper(const PCA9685Wrapper &) = delete;
|
||||||
PWMDriverWrapper operator=(const PWMDriverWrapper &) = delete;
|
PCA9685Wrapper operator=(const PCA9685Wrapper &) = delete;
|
||||||
|
|
||||||
int print_status() override;
|
int print_status() override;
|
||||||
|
|
||||||
@@ -87,6 +87,18 @@ private:
|
|||||||
|
|
||||||
int _class_instance{-1};
|
int _class_instance{-1};
|
||||||
|
|
||||||
|
enum class STATE : uint8_t {
|
||||||
|
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;
|
||||||
|
|
||||||
|
|
||||||
void Run() override;
|
void Run() override;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
@@ -96,6 +108,8 @@ protected:
|
|||||||
|
|
||||||
void updatePWMParamTrim();
|
void updatePWMParamTrim();
|
||||||
|
|
||||||
|
int _schd_rate_limit = 400;
|
||||||
|
|
||||||
PCA9685 *pca9685 = nullptr; // driver handle.
|
PCA9685 *pca9685 = nullptr; // driver handle.
|
||||||
|
|
||||||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; // param handle
|
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; // param handle
|
||||||
@@ -103,19 +117,20 @@ protected:
|
|||||||
MixingOutput _mixing_output{PCA9685_PWM_CHANNEL_COUNT, *this, MixingOutput::SchedulingPolicy::Disabled, true};
|
MixingOutput _mixing_output{PCA9685_PWM_CHANNEL_COUNT, *this, MixingOutput::SchedulingPolicy::Disabled, true};
|
||||||
};
|
};
|
||||||
|
|
||||||
PWMDriverWrapper::PWMDriverWrapper() :
|
PCA9685Wrapper::PCA9685Wrapper(int schd_rate_limit) :
|
||||||
CDev(nullptr),
|
CDev(nullptr),
|
||||||
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)
|
||||||
{
|
{
|
||||||
_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
|
_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
|
||||||
_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
|
_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
|
||||||
}
|
}
|
||||||
|
|
||||||
PWMDriverWrapper::~PWMDriverWrapper()
|
PCA9685Wrapper::~PCA9685Wrapper()
|
||||||
{
|
{
|
||||||
if (pca9685 != nullptr) { // normally this should not be called.
|
if (pca9685 != nullptr) { // normally this should not be called.
|
||||||
PX4_DEBUG("Destruction of PWMDriverWrapper without pwmDevice unloaded!");
|
PX4_DEBUG("Destruction of PCA9685Wrapper without pwmDevice unloaded!");
|
||||||
pca9685->Stop(); // force stop
|
pca9685->Stop(); // force stop
|
||||||
delete pca9685;
|
delete pca9685;
|
||||||
pca9685 = nullptr;
|
pca9685 = nullptr;
|
||||||
@@ -124,7 +139,7 @@ PWMDriverWrapper::~PWMDriverWrapper()
|
|||||||
perf_free(_cycle_perf);
|
perf_free(_cycle_perf);
|
||||||
}
|
}
|
||||||
|
|
||||||
int PWMDriverWrapper::init()
|
int PCA9685Wrapper::init()
|
||||||
{
|
{
|
||||||
int ret = CDev::init();
|
int ret = CDev::init();
|
||||||
|
|
||||||
@@ -132,28 +147,30 @@ int PWMDriverWrapper::init()
|
|||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
ret = pca9685->Start();
|
ret = pca9685->init();
|
||||||
|
|
||||||
if (ret != PX4_OK) {
|
if (ret != PX4_OK) {
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
_class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
|
||||||
|
|
||||||
this->ChangeWorkQeue(px4::device_bus_to_wq(pca9685->get_device_id()));
|
this->ChangeWorkQeue(px4::device_bus_to_wq(pca9685->get_device_id()));
|
||||||
|
|
||||||
updatePWMParams(); // Schedule is done inside
|
PX4_INFO("running on I2C bus %d address 0x%.2x", pca9685->get_device_bus(), pca9685->get_device_address());
|
||||||
|
|
||||||
_class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
|
ScheduleNow();
|
||||||
|
|
||||||
return PX4_OK;
|
return PX4_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
void PWMDriverWrapper::updateParams()
|
void PCA9685Wrapper::updateParams()
|
||||||
{
|
{
|
||||||
updatePWMParams();
|
updatePWMParams();
|
||||||
ModuleParams::updateParams();
|
ModuleParams::updateParams();
|
||||||
}
|
}
|
||||||
|
|
||||||
void PWMDriverWrapper::updatePWMParams()
|
void PCA9685Wrapper::updatePWMParams()
|
||||||
{
|
{
|
||||||
// update pwm params
|
// update pwm params
|
||||||
const char *pname_format_pwm_ch_max[2] = {"PWM_MAIN_MAX%d", "PWM_AUX_MAX%d"};
|
const char *pname_format_pwm_ch_max[2] = {"PWM_MAIN_MAX%d", "PWM_AUX_MAX%d"};
|
||||||
@@ -191,15 +208,9 @@ void PWMDriverWrapper::updatePWMParams()
|
|||||||
int32_t pval = 0;
|
int32_t pval = 0;
|
||||||
param_get(param_h, &pval);
|
param_get(param_h, &pval);
|
||||||
|
|
||||||
if (pca9685->setFreq((float)pval) != PX4_OK) {
|
if (_last_fetched_Freq != pval) {
|
||||||
PX4_ERR("failed to set pwm frequency, fall back to 50Hz");
|
_last_fetched_Freq = pval;
|
||||||
pca9685->setFreq((float)50); // this should not fail
|
_targetFreq = (float)pval; // update only if changed
|
||||||
ScheduleClear();
|
|
||||||
ScheduleOnInterval(1000000 / pca9685->getFrequency(), 1000000 / pca9685->getFrequency());
|
|
||||||
|
|
||||||
} else {
|
|
||||||
ScheduleClear();
|
|
||||||
ScheduleOnInterval(1000000 / pval, 1000000 / pval);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -311,7 +322,7 @@ void PWMDriverWrapper::updatePWMParams()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void PWMDriverWrapper::updatePWMParamTrim()
|
void PCA9685Wrapper::updatePWMParamTrim()
|
||||||
{
|
{
|
||||||
const char *pname_format_pwm_ch_trim[2] = {"PWM_MAIN_TRIM%d", "PWM_AUX_TRIM%d"};
|
const char *pname_format_pwm_ch_trim[2] = {"PWM_MAIN_TRIM%d", "PWM_AUX_TRIM%d"};
|
||||||
|
|
||||||
@@ -348,13 +359,13 @@ void PWMDriverWrapper::updatePWMParamTrim()
|
|||||||
PX4_DEBUG("set %d trims", n_out);
|
PX4_DEBUG("set %d trims", n_out);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool PWMDriverWrapper::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;
|
return pca9685->updatePWM(outputs, num_outputs) == 0 ? true : false;
|
||||||
}
|
}
|
||||||
|
|
||||||
void PWMDriverWrapper::Run()
|
void PCA9685Wrapper::Run()
|
||||||
{
|
{
|
||||||
if (should_exit()) {
|
if (should_exit()) {
|
||||||
PX4_INFO("PCA9685 stopping.");
|
PX4_INFO("PCA9685 stopping.");
|
||||||
@@ -372,25 +383,81 @@ void PWMDriverWrapper::Run()
|
|||||||
|
|
||||||
perf_begin(_cycle_perf);
|
perf_begin(_cycle_perf);
|
||||||
|
|
||||||
_mixing_output.update();
|
switch (_state) {
|
||||||
|
case STATE::INIT:
|
||||||
|
pca9685->initReg();
|
||||||
|
updatePWMParams(); // target frequency fetched, immediately apply it
|
||||||
|
|
||||||
// check for parameter updates
|
if (_targetFreq > 0.0f) {
|
||||||
if (_parameter_update_sub.updated()) {
|
if (pca9685->setFreq(_targetFreq) != PX4_OK) {
|
||||||
// clear update
|
PX4_ERR("failed to set pwm frequency to %.2f, fall back to 50Hz", (double)_targetFreq);
|
||||||
parameter_update_s pupdate;
|
pca9685->setFreq(50.0f); // this should not fail
|
||||||
_parameter_update_sub.copy(&pupdate);
|
}
|
||||||
|
|
||||||
// update parameters from storage
|
_targetFreq = -1.0f;
|
||||||
updateParams();
|
|
||||||
|
} else {
|
||||||
|
// should not happen
|
||||||
|
PX4_ERR("INIT failed: invalid initial frequency settings");
|
||||||
|
}
|
||||||
|
|
||||||
|
pca9685->startOscillator();
|
||||||
|
_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);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case STATE::RUNNING:
|
||||||
|
_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();
|
||||||
|
}
|
||||||
|
|
||||||
|
_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);
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
_mixing_output.updateSubscriptions(false);
|
|
||||||
|
|
||||||
perf_end(_cycle_perf);
|
perf_end(_cycle_perf);
|
||||||
}
|
}
|
||||||
|
|
||||||
// TODO
|
// TODO
|
||||||
int PWMDriverWrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg)
|
int PCA9685Wrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg)
|
||||||
{
|
{
|
||||||
int ret = OK;
|
int ret = OK;
|
||||||
|
|
||||||
@@ -438,7 +505,7 @@ int PWMDriverWrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg)
|
|||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
int PWMDriverWrapper::print_usage(const char *reason)
|
int PCA9685Wrapper::print_usage(const char *reason)
|
||||||
{
|
{
|
||||||
if (reason) {
|
if (reason) {
|
||||||
PX4_WARN("%s\n", reason);
|
PX4_WARN("%s\n", reason);
|
||||||
@@ -468,13 +535,14 @@ The number X can be acquired by executing
|
|||||||
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_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_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;
|
||||||
}
|
}
|
||||||
|
|
||||||
int PWMDriverWrapper::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, true frequency %.5f",
|
||||||
pca9685->get_device_bus(),
|
pca9685->get_device_bus(),
|
||||||
@@ -485,43 +553,48 @@ int PWMDriverWrapper::print_status() {
|
|||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
int PWMDriverWrapper::custom_command(int argc, char **argv) { // only for test use
|
int PCA9685Wrapper::custom_command(int argc, char **argv) { // only for test use
|
||||||
return PX4_OK;
|
return PX4_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
int PWMDriverWrapper::task_spawn(int argc, char **argv) {
|
int PCA9685Wrapper::task_spawn(int argc, char **argv) {
|
||||||
|
|
||||||
auto *instance = new PWMDriverWrapper();
|
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) {
|
||||||
|
switch (ch) {
|
||||||
|
case 'a':
|
||||||
|
address = atoi(myoptarg);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 'b':
|
||||||
|
iicbus = atoi(myoptarg);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 'r':
|
||||||
|
schd_rate_limit = atoi(myoptarg);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case '?':
|
||||||
|
PX4_WARN("Unsupported args");
|
||||||
|
return PX4_ERROR;
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
auto *instance = new PCA9685Wrapper(schd_rate_limit);
|
||||||
|
|
||||||
if (instance) {
|
if (instance) {
|
||||||
_object.store(instance);
|
_object.store(instance);
|
||||||
_task_id = task_id_is_work_queue;
|
_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);
|
instance->pca9685 = new PCA9685(iicbus, address);
|
||||||
if(instance->pca9685==nullptr){
|
if(instance->pca9685==nullptr){
|
||||||
PX4_ERR("alloc failed");
|
PX4_ERR("alloc failed");
|
||||||
@@ -537,6 +610,7 @@ int PWMDriverWrapper::task_spawn(int argc, char **argv) {
|
|||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
PX4_ERR("alloc failed");
|
PX4_ERR("alloc failed");
|
||||||
|
return PX4_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
driverInstanceAllocFailed:
|
driverInstanceAllocFailed:
|
||||||
@@ -547,7 +621,7 @@ int PWMDriverWrapper::task_spawn(int argc, char **argv) {
|
|||||||
return PX4_ERROR;
|
return PX4_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
void PWMDriverWrapper::mixerChanged() {
|
void PCA9685Wrapper::mixerChanged() {
|
||||||
OutputModuleInterface::mixerChanged();
|
OutputModuleInterface::mixerChanged();
|
||||||
if (_mixing_output.mixers()) { // only update trims if mixer loaded
|
if (_mixing_output.mixers()) { // only update trims if mixer loaded
|
||||||
updatePWMParamTrim();
|
updatePWMParamTrim();
|
||||||
@@ -558,5 +632,5 @@ void PWMDriverWrapper::mixerChanged() {
|
|||||||
extern "C" __EXPORT int pca9685_pwm_out_main(int argc, char *argv[]);
|
extern "C" __EXPORT int pca9685_pwm_out_main(int argc, char *argv[]);
|
||||||
|
|
||||||
int pca9685_pwm_out_main(int argc, char *argv[]){
|
int pca9685_pwm_out_main(int argc, char *argv[]){
|
||||||
return PWMDriverWrapper::main(argc, argv);
|
return PCA9685Wrapper::main(argc, argv);
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user