fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 02:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

pca9685_pwm_out: small improvement (#16196)

Browse Source 
* 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-27 17:17:45 -05:00
committed by GitHub
parent 3309bf21dd
commit 2d0eb4a41a
3 changed files with 214 additions and 154 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/pca9685_pwm_out/PCA9685.cpp
+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()
{
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;
}
}