fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

drivers: PCA9685 robustness & logging improvements

Browse Source 
Co-authored-by: Phil-Engljaehringer <philipp.engljahringer@auterion.com>
This commit is contained in:
Alexander Lerach 2025-11-25 15:39:03 +01:00
parent 14b38f2eba
commit bd3b3d647f
3 changed files with 210 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

127
src/drivers/pca9685_pwm_out/PCA9685.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2025 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -57,61 +57,61 @@ PCA9685::PCA9685(int bus, int addr):
int PCA9685::init()
{
int ret = I2C::init();
if (ret != PX4_OK) { return ret; }
return I2C::init();
}
int PCA9685::configure()
{
uint8_t buf[2] = {};
buf[0] = PCA9685_REG_MODE1;
buf[1] = PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_SLEEP_MASK; // put into sleep mode
ret = transfer(buf, 2, nullptr, 0);
if (OK != ret) {
PX4_ERR("init: i2c::transfer returned %d", ret);
return ret;
}
int ret = transfer(buf, 2, nullptr, 0);
#ifdef CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL
/* EXTCLK is sticky, so writing it once is enough. Its not a problem when its written to 0 later. */
buf[1] = PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_SLEEP_MASK | PCA9685_MODE1_EXTCLK_MASK;
ret = transfer(buf, 2, nullptr, 0); // enable EXTCLK if possible
if (OK != ret) {
PX4_ERR("init: i2c::transfer returned %d", ret);
return ret;
}
ret |= transfer(buf, 2, nullptr, 0);
#endif
buf[0] = PCA9685_REG_MODE2;
buf[1] = PCA9685_DEFAULT_MODE2_CFG;
ret = transfer(buf, 2, nullptr, 0);
ret |= transfer(buf, 2, nullptr, 0);
return ret;
}
if (OK != ret) {
PX4_ERR("init: i2c::transfer returned %d", ret);
return ret;
int PCA9685::registers_check()
{
/* Check MODE1 register */
uint8_t send_buf = PCA9685_REG_MODE1;
uint8_t recv_buf;
int ret = transfer(&send_buf, 1, &recv_buf, 1);
uint8_t ignore_extclk_mask = ~PCA9685_MODE1_EXTCLK_MASK;
if (ret != PX4_OK) {
return -EIO;
}
if ((recv_buf & ignore_extclk_mask) != (PCA9685_DEFAULT_MODE1_CFG & ignore_extclk_mask)) {
return -EFAULT;
}
/* Check MODE2 register */
send_buf = PCA9685_REG_MODE2;
ret = transfer(&send_buf, 1, &recv_buf, 1);
if (ret != PX4_OK) {
return -EIO;
}
if (recv_buf != PCA9685_DEFAULT_MODE2_CFG) {
return -EFAULT;
}
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_DEBUG("PCA9685 can only drive up to 16 channels");
}
uint16_t out[PCA9685_PWM_CHANNEL_COUNT];
memcpy(out, outputs, sizeof(uint16_t) * num_outputs);
for (unsigned i = 0; i < num_outputs; ++i) {
out[i] = calcRawFromPulse(out[i]);
}
return writePWM(0, out, num_outputs);
}
int PCA9685::updateFreq(float freq)
{
uint16_t divider = (uint16_t)round((float)PCA9685_CLOCK_REFERENCE / freq / PCA9685_PWM_RES) - 1;
@ -160,30 +160,47 @@ int PCA9685::sleep()
int PCA9685::wake()
{
uint8_t buf[2] = {
PCA9685_REG_MODE1,
PCA9685_DEFAULT_MODE1_CFG
};
return transfer(buf, 2, nullptr, 0);
}
uint8_t send_buf[2];
uint8_t recv_buf;
int PCA9685::doRestart()
{
uint8_t buf[2] = {
PCA9685_REG_MODE1,
PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_RESTART_MASK
};
return transfer(buf, 2, nullptr, 0);
send_buf[0] = PCA9685_REG_MODE1;
int ret = transfer(&send_buf[0], 1, &recv_buf, 1);
if (ret != PX4_OK) {
return PX4_ERROR;
}
send_buf[1] = recv_buf & ~PCA9685_MODE1_SLEEP_MASK; // Clear sleep bit
ret |= transfer(&send_buf[0], 2, nullptr, 0);
px4_usleep(500); // wait for oscillator to stabilize
if (recv_buf & PCA9685_MODE1_RESTART_MASK) { // Check if reset bit is set
send_buf[1] |= PCA9685_MODE1_RESTART_MASK; // Set restart bit
ret |= transfer(&send_buf[0], 2, nullptr, 0);
}
ret |= transfer(&send_buf[0], 1, &recv_buf, 1);
if (ret != PX4_OK || recv_buf & (PCA9685_MODE1_RESTART_MASK | PCA9685_MODE1_SLEEP_MASK)) {
return PX4_ERROR;
}
return ret;
}
int PCA9685::probe()
{
int ret = I2C::probe();
for (int i = 0; i < 10; i++) {
uint8_t send_buf = PCA9685_REG_MODE1;
if (ret != PX4_OK) { return ret; }
if (transfer(&send_buf, 1, nullptr, 0) == PX4_OK) {
return PX4_OK;
}
uint8_t buf[2] = {0x00};
return transfer(buf, 2, buf, 1);
px4_usleep(10'000);
}
return PX4_ERROR;
}
int PCA9685::writePWM(uint8_t idx, const uint16_t *value, uint8_t num)

22
src/drivers/pca9685_pwm_out/PCA9685.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -95,13 +95,6 @@ public:
int init() override;
/*
* Write new PWM value to device
*
* *output: pulse width, us
*/
int updatePWM(const uint16_t *outputs, unsigned num_outputs);
/*
* Set PWM frequency to new value.
*
@ -150,11 +143,14 @@ public:
int wake();
/*
* If PCA9685 is put into sleep without clearing all the outputs,
* then the restart command will be available, and it can bring back PWM output without the
* need of updatePWM() call.
*/
int doRestart();
* Configure the PCA9685 device with necessary settings. e.g. MODE1 or MODE2
*/
int configure();
/*
* Verfy whether the registers of PCA9685 are in a consistent state
*/
int registers_check();
protected:
int probe() override;

189
src/drivers/pca9685_pwm_out/main.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2025 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -80,12 +80,15 @@ protected:
private:
perf_counter_t _cycle_perf;
perf_counter_t _comms_errors;
perf_counter_t _registers_invalid_reset;
perf_counter_t _registers_transfer_reset;
enum class STATE : uint8_t {
CONFIGURE,
INIT,
WAIT_FOR_OSC,
RUNNING
} state{STATE::INIT};
} _state{STATE::CONFIGURE};
PCA9685 *pca9685 = nullptr;
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@ -99,14 +102,22 @@ private:
float param_pwm_freq, previous_pwm_freq;
float param_schd_rate, previous_schd_rate;
bool param_update_failed = false;
uint32_t param_duty_mode;
static constexpr uint8_t _transfer_fails_threshold = 10;
uint8_t _register_transfer_fails = 0;
void Run() override;
int registers_check();
};
PCA9685Wrapper::PCA9685Wrapper() :
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")),
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors")),
_registers_invalid_reset(perf_alloc(PC_COUNT, MODULE_NAME": registers invalid reset")),
_registers_transfer_reset(perf_alloc(PC_COUNT, MODULE_NAME": registers transfer reset"))
{
}
@ -119,6 +130,9 @@ PCA9685Wrapper::~PCA9685Wrapper()
}
perf_free(_cycle_perf);
perf_free(_comms_errors);
perf_free(_registers_invalid_reset);
perf_free(_registers_transfer_reset);
}
int PCA9685Wrapper::init()
@ -139,7 +153,7 @@ int PCA9685Wrapper::init()
bool PCA9685Wrapper::updateOutputs(uint16_t *outputs, unsigned num_outputs,
unsigned num_control_groups_updated)
{
if (state != STATE::RUNNING) { return false; }
if (_state != STATE::RUNNING) { return false; }
uint16_t low_level_outputs[PCA9685_PWM_CHANNEL_COUNT] = {};
num_outputs = num_outputs > PCA9685_PWM_CHANNEL_COUNT ? PCA9685_PWM_CHANNEL_COUNT : num_outputs;
@ -154,7 +168,7 @@ bool PCA9685Wrapper::updateOutputs(uint16_t *outputs, unsigned num_outputs,
}
if (pca9685->updateRAW(low_level_outputs, num_outputs) != PX4_OK) {
PX4_ERR("Failed to write PWM to PCA9685");
perf_count(_comms_errors);
return false;
}
@ -176,69 +190,124 @@ void PCA9685Wrapper::Run()
return;
}
switch (state) {
case STATE::INIT:
updateParams();
pca9685->updateFreq(param_pwm_freq);
previous_pwm_freq = param_pwm_freq;
previous_schd_rate = param_schd_rate;
switch (_state) {
case STATE::CONFIGURE: {
int ret = pca9685->configure();
pca9685->wake();
state = STATE::WAIT_FOR_OSC;
ScheduleDelayed(500);
break;
if (ret == PX4_OK) {
_state = STATE::INIT;
ScheduleNow();
case STATE::WAIT_FOR_OSC: {
state = STATE::RUNNING;
ScheduleOnInterval(1000000 / param_schd_rate, 0);
}
break;
case STATE::RUNNING:
perf_begin(_cycle_perf);
_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();
// apply param updates
if ((float)fabs(previous_pwm_freq - param_pwm_freq) > 0.01f) {
previous_pwm_freq = param_pwm_freq;
ScheduleClear();
pca9685->sleep();
pca9685->updateFreq(param_pwm_freq);
pca9685->wake();
// update of PWM freq will always trigger scheduling change
previous_schd_rate = param_schd_rate;
state = STATE::WAIT_FOR_OSC;
ScheduleDelayed(500);
} else if ((float)fabs(previous_schd_rate - param_schd_rate) > 0.01f) {
// case when PWM freq not changed but scheduling rate does
previous_schd_rate = param_schd_rate;
ScheduleClear();
ScheduleOnInterval(1000000 / param_schd_rate, 1000000 / param_schd_rate);
} else {
perf_count(_comms_errors);
ScheduleDelayed(20_ms);
}
break;
}
_mixing_output.updateSubscriptions(false);
case STATE::INIT: {
updateParams();
int ret = pca9685->updateFreq(param_pwm_freq);
ret |= pca9685->wake();
perf_end(_cycle_perf);
break;
if (ret == PX4_OK) {
previous_pwm_freq = param_pwm_freq;
previous_schd_rate = param_schd_rate;
_state = STATE::RUNNING;
ScheduleOnInterval(1000000 / param_schd_rate, 0);
} else {
perf_count(_comms_errors);
_state = STATE::CONFIGURE;
ScheduleDelayed(20_ms);
}
break;
}
case STATE::RUNNING: {
perf_begin(_cycle_perf);
_mixing_output.update();
// check for parameter updates
if (_parameter_update_sub.updated() || param_update_failed) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
updateParams();
// apply param updates
if ((float)fabs(previous_pwm_freq - param_pwm_freq) > 0.01f) {
ScheduleClear();
int ret = pca9685->sleep();
ret |= pca9685->updateFreq(param_pwm_freq);
ret |= pca9685->wake();
if (ret == PX4_OK) {
// update of PWM freq will always trigger scheduling change
param_update_failed = false;
previous_schd_rate = param_schd_rate;
previous_pwm_freq = param_pwm_freq;
ScheduleOnInterval(1000000 / param_schd_rate, 0);
} else {
param_update_failed = true;
perf_count(_comms_errors);
ScheduleDelayed(20_ms);
break;
}
} else if ((float)fabs(previous_schd_rate - param_schd_rate) > 0.01f) {
// case when PWM freq not changed but scheduling rate does
previous_schd_rate = param_schd_rate;
ScheduleClear();
ScheduleOnInterval(1000000 / param_schd_rate, 1000000 / param_schd_rate);
}
}
if (registers_check() != PX4_OK) {
_state = STATE::CONFIGURE;
ScheduleClear();
ScheduleDelayed(20_ms);
}
_mixing_output.updateSubscriptions(false);
perf_end(_cycle_perf);
break;
}
}
}
int PCA9685Wrapper::registers_check()
{
int reg_ret = pca9685->registers_check();
if (reg_ret == -EIO) {
_register_transfer_fails++;
} else {
_register_transfer_fails = 0;
}
if (reg_ret == -EFAULT) {
perf_count(_registers_invalid_reset);
return PX4_ERROR;
}
if (_register_transfer_fails > _transfer_fails_threshold) {
perf_count(_registers_transfer_reset);
_register_transfer_fails = 0;
return PX4_ERROR;
}
return PX4_OK;
}
int PCA9685Wrapper::print_usage(const char *reason)
{
if (reason) {