Skip to content

Commit

Permalink
pca9685_pwm_out: small improvement (#16196)
Browse files Browse the repository at this point in the history 
* reduce I2C transfers and enable EXTCLK support
* schedule rate limit arg
* apply state machine and do actual init in Run()
  • Loading branch information
@SalimTerryLi
SalimTerryLi authored Nov 27, 2020
1 parent 3309bf2 commit 2d0eb4a
Show file tree
Hide file tree
Showing 3 changed files with 208 additions and 148 deletions.
89 changes: 34 additions & 55 deletions src/drivers/pca9685_pwm_out/PCA9685.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,23 +43,18 @@ PCA9685::PCA9685(int bus, int addr):

}

int PCA9685::Start()
{
int ret = init();
return ret;
}

int PCA9685::Stop()
{
disableAllOutput();
stopOscillator();
return PX4_OK;
}

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];
Expand Down Expand Up @@ -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;
}

Expand All @@ -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;
}

Expand All @@ -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);
}
}

Expand All @@ -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);
}
}

Expand All @@ -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);
Expand All @@ -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;
Expand All @@ -220,60 +212,47 @@ 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);
return;
}
}

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;
}
}
}
49 changes: 28 additions & 21 deletions src/drivers/pca9685_pwm_out/PCA9685.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -103,8 +103,6 @@ class PCA9685 : public device::I2C
public:
PCA9685(int bus, int addr);

int Start();

int Stop();

/*
Expand All @@ -116,15 +114,39 @@ class PCA9685 : public device::I2C

~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;

Expand All @@ -140,26 +162,11 @@ class PCA9685 : public device::I2C
*/
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:

};
Expand Down
Loading

0 comments on commit 2d0eb4a

Please sign in to comment.