diff --git a/src/drivers/pca9685_pwm_out/PCA9685.cpp b/src/drivers/pca9685_pwm_out/PCA9685.cpp index a0eb18459c..a8e95b0f1d 100644 --- a/src/drivers/pca9685_pwm_out/PCA9685.cpp +++ b/src/drivers/pca9685_pwm_out/PCA9685.cpp @@ -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] = {}; + + buf[0] = PCA9685_REG_MODE1; + buf[1] = DEFAULT_MODE1_CFG; + int ret = transfer(buf, 2, nullptr, 0); // make sure oscillator is disabled - if (ret != PX4_OK) { - PX4_DEBUG("I2C init failed."); + 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); - - 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); + // 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); + PX4_DEBUG("startOscillator: i2c::transfer returned %d", ret); return; } +} + +void PCA9685::triggerRestart() +{ + uint8_t buf[2] = {PCA9685_REG_MODE1}; - usleep(5000); + // clear sleep bit, with restart bit = 0 + buf[1] = DEFAULT_MODE1_CFG & (~PCA9685_MODE1_SLEEP_MASK); buf[1] |= PCA9685_MODE1_RESTART_MASK; - ret = transfer(buf, 2, nullptr, 0); + int ret = transfer(buf, 2, nullptr, 0); if (OK != ret) { - PX4_DEBUG("i2c::transfer returned %d", ret); + PX4_DEBUG("triggerRestart: i2c::transfer returned %d", ret); return; } -} +} \ No newline at end of file diff --git a/src/drivers/pca9685_pwm_out/PCA9685.h b/src/drivers/pca9685_pwm_out/PCA9685.h index b0f630c7ac..5c7740164c 100644 --- a/src/drivers/pca9685_pwm_out/PCA9685.h +++ b/src/drivers/pca9685_pwm_out/PCA9685.h @@ -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: }; diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index bd49b00266..6eff589003 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -54,12 +54,12 @@ using namespace drv_pca9685_pwm; -class PWMDriverWrapper : public cdev::CDev, public ModuleBase, public OutputModuleInterface +class PCA9685Wrapper : public cdev::CDev, public ModuleBase, 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; - _mixing_output.updateSubscriptions(false); + 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; + } 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) { + + 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; - auto *instance = new PWMDriverWrapper(); + 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); }