drivers: PCA9685 robustness & logging improvements

Co-authored-by: Phil-Engljaehringer <philipp.engljahringer@auterion.com>

src/drivers/pca9685_pwm_out/PCA9685.cpp

@@ -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();
+	return I2C::init();
-
+}
-	if (ret != PX4_OK) { return ret; }
 
+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);
+	int ret = transfer(buf, 2, nullptr, 0);
-
-	if (OK != ret) {
-		PX4_ERR("init: i2c::transfer returned %d", ret);
-		return ret;
-	}
 
 #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
+	ret |= transfer(buf, 2, nullptr, 0);
-
-	if (OK != ret) {
-		PX4_ERR("init: i2c::transfer returned %d", ret);
-		return ret;
-	}
-
 #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) {
+int PCA9685::registers_check()
-		PX4_ERR("init: i2c::transfer returned %d", ret);
+{
-		return ret;
+	/* 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] = {
+	uint8_t send_buf[2];
-		PCA9685_REG_MODE1,
+	uint8_t recv_buf;
-		PCA9685_DEFAULT_MODE1_CFG
-	};
-	return transfer(buf, 2, nullptr, 0);
-}
 
-int PCA9685::doRestart()
+	send_buf[0] = PCA9685_REG_MODE1;
-{
+	int ret = transfer(&send_buf[0], 1, &recv_buf, 1);
-	uint8_t buf[2] = {
+
-		PCA9685_REG_MODE1,
+	if (ret != PX4_OK) {
-		PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_RESTART_MASK
+		return PX4_ERROR;
-	};
+	}
-	return transfer(buf, 2, nullptr, 0);
+
+	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};
+		px4_usleep(10'000);
-	return transfer(buf, 2, buf, 1);
+	}
+
+	return PX4_ERROR;
 }
 
 int PCA9685::writePWM(uint8_t idx, const uint16_t *value, uint8_t num)

src/drivers/pca9685_pwm_out/PCA9685.h

@@ -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,
+	* Configure the PCA9685 device with necessary settings. e.g. MODE1 or MODE2
-	 * then the restart command will be available, and it can bring back PWM output without the
+	*/
-	 * need of updatePWM() call.
+	int configure();
-	 */
+
-	int doRestart();
+	/*
+	* Verfy whether the registers of PCA9685 are in a consistent state
+	*/
+	int registers_check();
 
 protected:
 	int probe() override;

src/drivers/pca9685_pwm_out/main.cpp

@@ -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) {
+	switch (_state) {
-	case STATE::INIT:
+	case STATE::CONFIGURE: {
-		updateParams();
+			int ret = pca9685->configure();
-		pca9685->updateFreq(param_pwm_freq);
-		previous_pwm_freq = param_pwm_freq;
-		previous_schd_rate = param_schd_rate;
 
-		pca9685->wake();
+			if (ret == PX4_OK) {
-		state = STATE::WAIT_FOR_OSC;
+				_state = STATE::INIT;
-		ScheduleDelayed(500);
+				ScheduleNow();
-		break;
 
-	case STATE::WAIT_FOR_OSC: {
+			} else {
-			state = STATE::RUNNING;
+				perf_count(_comms_errors);
-			ScheduleOnInterval(1000000 / param_schd_rate, 0);
+				ScheduleDelayed(20_ms);
-		}
-		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);
 			}
+
+			break;
 		}
 
-		_mixing_output.updateSubscriptions(false);
+	case STATE::INIT: {
+			updateParams();
+			int ret = pca9685->updateFreq(param_pwm_freq);
+			ret |= pca9685->wake();
 
-		perf_end(_cycle_perf);
+			if (ret == PX4_OK) {
-		break;
+				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) {