mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +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:
SalimTerryLi
GitHub
parent
3309bf21dd
commit
2d0eb4a41a
@ -43,15 +43,10 @@ PCA9685::PCA9685(int bus, int addr):
|
||||
|
||||
}
|
||||
|
||||
int PCA9685::Start()
|
||||
{
|
||||
int ret = init();
|
||||
return ret;
|
||||
}
|
||||
|
||||
int PCA9685::Stop()
|
||||
{
|
||||
disableAllOutput();
|
||||
stopOscillator();
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
@ -59,7 +54,7 @@ int PCA9685::updatePWM(const uint16_t *outputs, unsigned num_outputs)
|
||||
{
|
||||
if (num_outputs > PCA9685_PWM_CHANNEL_COUNT) {
|
||||
num_outputs = PCA9685_PWM_CHANNEL_COUNT;
|
||||
PX4_WARN("PCA9685 can only drive up to 16 channels");
|
||||
PX4_DEBUG("PCA9685 can only drive up to 16 channels");
|
||||
}
|
||||
|
||||
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) {
|
||||
PX4_DEBUG("I2C init failed.");
|
||||
buf[0] = PCA9685_REG_MODE1;
|
||||
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;
|
||||
}
|
||||
|
||||
uint8_t buf[2] = {};
|
||||
buf[0] = PCA9685_REG_MODE1;
|
||||
buf[1] = DEFAULT_MODE1_CFG;
|
||||
ret = transfer(buf, 2, nullptr, 0);
|
||||
ret = transfer(buf, 2, nullptr, 0); // enable EXTCLK if possible
|
||||
|
||||
if (OK != ret) {
|
||||
PX4_DEBUG("i2c::transfer returned %d", ret);
|
||||
PX4_ERR("init: i2c::transfer returned %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
@ -131,12 +127,10 @@ int PCA9685::init()
|
||||
ret = transfer(buf, 2, nullptr, 0);
|
||||
|
||||
if (OK != ret) {
|
||||
PX4_DEBUG("i2c::transfer returned %d", ret);
|
||||
PX4_ERR("init: i2c::transfer returned %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
disableAllOutput();
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
@ -162,7 +156,7 @@ void PCA9685::setPWM(uint8_t channel, const uint16_t &value)
|
||||
int ret = transfer(buf, 5, nullptr, 0);
|
||||
|
||||
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);
|
||||
|
||||
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[1] = 0x00;
|
||||
buf[2] = 0x00;
|
||||
buf[3] = (uint8_t)(0x00 & (uint8_t)0xFF);
|
||||
buf[3] = 0x00;
|
||||
buf[4] = PCA9685_LED_ON_FULL_ON_OFF_MASK;
|
||||
|
||||
int ret = transfer(buf, 5, nullptr, 0);
|
||||
@ -209,8 +203,6 @@ void PCA9685::disableAllOutput()
|
||||
|
||||
void PCA9685::setDivider(uint8_t value)
|
||||
{
|
||||
disableAllOutput();
|
||||
stopOscillator();
|
||||
uint8_t buf[2] = {};
|
||||
buf[0] = PCA9685_REG_PRE_SCALE;
|
||||
buf[1] = value;
|
||||
@ -220,25 +212,15 @@ void PCA9685::setDivider(uint8_t value)
|
||||
PX4_DEBUG("i2c::transfer returned %d", ret);
|
||||
return;
|
||||
}
|
||||
|
||||
restartOscillator();
|
||||
}
|
||||
|
||||
void PCA9685::stopOscillator()
|
||||
{
|
||||
uint8_t buf[2] = {PCA9685_REG_MODE1};
|
||||
int ret = transfer(buf, 1, buf, 1);
|
||||
|
||||
if (OK != ret) {
|
||||
PX4_DEBUG("i2c::transfer returned %d", ret);
|
||||
return;
|
||||
}
|
||||
|
||||
buf[1] = buf[0];
|
||||
buf[0] = PCA9685_REG_MODE1;
|
||||
// clear restart bit
|
||||
buf[1] |= PCA9685_MODE1_SLEEP_MASK;
|
||||
ret = transfer(buf, 2, nullptr, 0);
|
||||
// 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);
|
||||
@ -246,34 +228,31 @@ void PCA9685::stopOscillator()
|
||||
}
|
||||
}
|
||||
|
||||
void PCA9685::restartOscillator()
|
||||
void PCA9685::startOscillator()
|
||||
{
|
||||
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) {
|
||||
PX4_DEBUG("i2c::transfer returned %d", ret);
|
||||
return;
|
||||
}
|
||||
|
||||
// clear restart and sleep bit
|
||||
buf[1] &= PCA9685_MODE1_EXTCLK_MASK | PCA9685_MODE1_AI_MASK
|
||||
| PCA9685_MODE1_SUB1_MASK | PCA9685_MODE1_SUB2_MASK
|
||||
| PCA9685_MODE1_SUB3_MASK | PCA9685_MODE1_ALLCALL_MASK;
|
||||
buf[0] = PCA9685_REG_MODE1;
|
||||
ret = transfer(buf, 2, nullptr, 0);
|
||||
|
||||
if (OK != ret) {
|
||||
PX4_DEBUG("i2c::transfer returned %d", ret);
|
||||
return;
|
||||
}
|
||||
|
||||
usleep(5000);
|
||||
buf[1] |= PCA9685_MODE1_RESTART_MASK;
|
||||
ret = transfer(buf, 2, nullptr, 0);
|
||||
|
||||
if (OK != ret) {
|
||||
PX4_DEBUG("i2c::transfer returned %d", ret);
|
||||
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;
|
||||
}
|
||||
}
|
||||
@ -88,7 +88,7 @@ namespace drv_pca9685_pwm
|
||||
* 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_CLOCL_EXT
|
||||
#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
|
||||
@ -103,8 +103,6 @@ class PCA9685 : public device::I2C
|
||||
public:
|
||||
PCA9685(int bus, int addr);
|
||||
|
||||
int Start();
|
||||
|
||||
int Stop();
|
||||
|
||||
/*
|
||||
@ -116,15 +114,39 @@ public:
|
||||
|
||||
~PCA9685() override = default;
|
||||
|
||||
int init() override;
|
||||
int initReg();
|
||||
|
||||
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:
|
||||
int probe() override;
|
||||
|
||||
static const uint8_t DEFAULT_MODE1_CFG = 0x20;
|
||||
static const uint8_t DEFAULT_MODE2_CFG = 0x04;
|
||||
#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;
|
||||
|
||||
@ -140,26 +162,11 @@ protected:
|
||||
*/
|
||||
void setPWM(uint8_t channel_count, const uint16_t *value);
|
||||
|
||||
/*
|
||||
* disable all of the output
|
||||
*/
|
||||
void disableAllOutput();
|
||||
|
||||
/*
|
||||
* set clock divider
|
||||
* this func has Super Cow Powers
|
||||
*/
|
||||
void setDivider(uint8_t value);
|
||||
|
||||
/*
|
||||
* turn off oscillator
|
||||
*/
|
||||
void stopOscillator();
|
||||
|
||||
/*
|
||||
* restart output
|
||||
*/
|
||||
void restartOscillator();
|
||||
private:
|
||||
|
||||
};
|
||||
|
||||
@ -54,12 +54,12 @@
|
||||
|
||||
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:
|
||||
|
||||
PWMDriverWrapper();
|
||||
~PWMDriverWrapper() override ;
|
||||
PCA9685Wrapper(int schd_rate_limit = 400);
|
||||
~PCA9685Wrapper() override ;
|
||||
|
||||
int init() override;
|
||||
|
||||
@ -77,8 +77,8 @@ public:
|
||||
bool updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs,
|
||||
unsigned num_control_groups_updated) override;
|
||||
|
||||
PWMDriverWrapper(const PWMDriverWrapper &) = delete;
|
||||
PWMDriverWrapper operator=(const PWMDriverWrapper &) = delete;
|
||||
PCA9685Wrapper(const PCA9685Wrapper &) = delete;
|
||||
PCA9685Wrapper operator=(const PCA9685Wrapper &) = delete;
|
||||
|
||||
int print_status() override;
|
||||
|
||||
@ -87,6 +87,18 @@ private:
|
||||
|
||||
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;
|
||||
|
||||
protected:
|
||||
@ -96,6 +108,8 @@ protected:
|
||||
|
||||
void updatePWMParamTrim();
|
||||
|
||||
int _schd_rate_limit = 400;
|
||||
|
||||
PCA9685 *pca9685 = nullptr; // driver 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};
|
||||
};
|
||||
|
||||
PWMDriverWrapper::PWMDriverWrapper() :
|
||||
PCA9685Wrapper::PCA9685Wrapper(int schd_rate_limit) :
|
||||
CDev(nullptr),
|
||||
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.setAllMaxValues(PWM_DEFAULT_MAX);
|
||||
}
|
||||
|
||||
PWMDriverWrapper::~PWMDriverWrapper()
|
||||
PCA9685Wrapper::~PCA9685Wrapper()
|
||||
{
|
||||
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
|
||||
delete pca9685;
|
||||
pca9685 = nullptr;
|
||||
@ -124,7 +139,7 @@ PWMDriverWrapper::~PWMDriverWrapper()
|
||||
perf_free(_cycle_perf);
|
||||
}
|
||||
|
||||
int PWMDriverWrapper::init()
|
||||
int PCA9685Wrapper::init()
|
||||
{
|
||||
int ret = CDev::init();
|
||||
|
||||
@ -132,28 +147,30 @@ int PWMDriverWrapper::init()
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = pca9685->Start();
|
||||
ret = pca9685->init();
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
_class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
void PWMDriverWrapper::updateParams()
|
||||
void PCA9685Wrapper::updateParams()
|
||||
{
|
||||
updatePWMParams();
|
||||
ModuleParams::updateParams();
|
||||
}
|
||||
|
||||
void PWMDriverWrapper::updatePWMParams()
|
||||
void PCA9685Wrapper::updatePWMParams()
|
||||
{
|
||||
// update pwm params
|
||||
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;
|
||||
param_get(param_h, &pval);
|
||||
|
||||
if (pca9685->setFreq((float)pval) != PX4_OK) {
|
||||
PX4_ERR("failed to set pwm frequency, fall back to 50Hz");
|
||||
pca9685->setFreq((float)50); // this should not fail
|
||||
ScheduleClear();
|
||||
ScheduleOnInterval(1000000 / pca9685->getFrequency(), 1000000 / pca9685->getFrequency());
|
||||
|
||||
} else {
|
||||
ScheduleClear();
|
||||
ScheduleOnInterval(1000000 / pval, 1000000 / pval);
|
||||
if (_last_fetched_Freq != pval) {
|
||||
_last_fetched_Freq = pval;
|
||||
_targetFreq = (float)pval; // update only if changed
|
||||
}
|
||||
|
||||
} 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"};
|
||||
|
||||
@ -348,13 +359,13 @@ void PWMDriverWrapper::updatePWMParamTrim()
|
||||
PX4_DEBUG("set %d trims", n_out);
|
||||
}
|
||||
|
||||
bool PWMDriverWrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs,
|
||||
unsigned num_control_groups_updated)
|
||||
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;
|
||||
}
|
||||
|
||||
void PWMDriverWrapper::Run()
|
||||
void PCA9685Wrapper::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
PX4_INFO("PCA9685 stopping.");
|
||||
@ -372,25 +383,81 @@ void PWMDriverWrapper::Run()
|
||||
|
||||
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 (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
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
|
||||
}
|
||||
|
||||
// update parameters from storage
|
||||
updateParams();
|
||||
_targetFreq = -1.0f;
|
||||
|
||||
} 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);
|
||||
}
|
||||
|
||||
// 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;
|
||||
|
||||
@ -438,7 +505,7 @@ int PWMDriverWrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg)
|
||||
return ret;
|
||||
}
|
||||
|
||||
int PWMDriverWrapper::print_usage(const char *reason)
|
||||
int PCA9685Wrapper::print_usage(const char *reason)
|
||||
{
|
||||
if (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_COMMAND_DESCR("start", "Start the task");
|
||||
PRINT_MODULE_USAGE_PARAM_INT('a',64,0,255,"device address on this bus",true);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('b',1,0,255,"bus that pca9685 is connected to",true);
|
||||
PRINT_MODULE_USAGE_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;
|
||||
}
|
||||
|
||||
int PWMDriverWrapper::print_status() {
|
||||
int PCA9685Wrapper::print_status() {
|
||||
int ret = ModuleBase::print_status();
|
||||
PX4_INFO("PCA9685 @I2C Bus %d, address 0x%.2x, true frequency %.5f",
|
||||
pca9685->get_device_bus(),
|
||||
@ -485,43 +553,48 @@ int PWMDriverWrapper::print_status() {
|
||||
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;
|
||||
}
|
||||
|
||||
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) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
int ch;
|
||||
int address=PCA9685_DEFAULT_ADDRESS;
|
||||
int iicbus=PCA9685_DEFAULT_IICBUS;
|
||||
|
||||
int myoptind = 1;
|
||||
const char *myoptarg = nullptr;
|
||||
while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 'a':
|
||||
address = atoi(myoptarg);
|
||||
break;
|
||||
|
||||
case 'b':
|
||||
iicbus = atoi(myoptarg);
|
||||
break;
|
||||
|
||||
case '?':
|
||||
PX4_WARN("Unsupported args");
|
||||
goto driverInstanceAllocFailed;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
instance->pca9685 = new PCA9685(iicbus, address);
|
||||
if(instance->pca9685==nullptr){
|
||||
PX4_ERR("alloc failed");
|
||||
@ -537,6 +610,7 @@ int PWMDriverWrapper::task_spawn(int argc, char **argv) {
|
||||
}
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
driverInstanceAllocFailed:
|
||||
@ -547,7 +621,7 @@ int PWMDriverWrapper::task_spawn(int argc, char **argv) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
void PWMDriverWrapper::mixerChanged() {
|
||||
void PCA9685Wrapper::mixerChanged() {
|
||||
OutputModuleInterface::mixerChanged();
|
||||
if (_mixing_output.mixers()) { // only update trims if mixer loaded
|
||||
updatePWMParamTrim();
|
||||
@ -558,5 +632,5 @@ void PWMDriverWrapper::mixerChanged() {
|
||||
extern "C" __EXPORT int pca9685_pwm_out_main(int argc, char *argv[]);
|
||||
|
||||
int pca9685_pwm_out_main(int argc, char *argv[]){
|
||||
return PWMDriverWrapper::main(argc, argv);
|
||||
return PCA9685Wrapper::main(argc, argv);
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user