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
2020-11-28 06:17:45 +08:00
committed by GitHub
parent 3309bf21dd
commit 2d0eb4a41a
3 changed files with 214 additions and 154 deletions
+40 -61
View File
@@ -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;
} }
} }
+28 -21
View File
@@ -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:
}; };
+146 -72
View File
@@ -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);
} }