forked from Archive/PX4-Autopilot
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
GitHub
parent
63b5c790b7
commit
f68f88b97c
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -2,4 +2,20 @@ menuconfig DRIVERS_PCA9685_PWM_OUT
|
|||
bool "pca9685_pwm_out"
|
||||
default n
|
||||
---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
|
||||
|
|
@ -32,23 +32,66 @@
|
|||
****************************************************************************/
|
||||
|
||||
#include <px4_log.h>
|
||||
#include <px4_defines.h>
|
||||
#include <cmath>
|
||||
#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;
|
||||
|
||||
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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -34,10 +34,7 @@
|
|||
#pragma once
|
||||
#include <cstdint>
|
||||
#include <drivers/device/i2c.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
|
||||
namespace drv_pca9685_pwm
|
||||
{
|
||||
#include <px4_boardconfig.h>
|
||||
|
||||
#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;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 <lhf2613@gmail.com>
|
||||
*
|
||||
* This file serves as the wrapper layer for PCA9685 driver, working with parameters
|
||||
* and scheduling stuffs on PX4 side.
|
||||
*
|
||||
*/
|
||||
|
||||
#include <px4_log.h>
|
||||
|
|
@ -45,11 +46,10 @@
|
|||
#include <lib/perf/perf_counter.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/sem.hpp>
|
||||
|
||||
#include "PCA9685.h"
|
||||
|
||||
#include <px4_platform_common/sem.hpp>
|
||||
|
||||
#define PCA9685_DEFAULT_IICBUS 1
|
||||
#define PCA9685_DEFAULT_ADDRESS (0x40)
|
||||
|
||||
|
|
@ -59,26 +59,25 @@ using namespace time_literals;
|
|||
class PCA9685Wrapper : public ModuleBase<PCA9685Wrapper>, 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","<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('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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue