Extend the PCF8583 driver to support multiple instances (#19232)

* Add some restart events into pcf8583 driver

Co-authored-by: Vít Hanousek <vithanousek@seznam.cz>

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -155,6 +155,13 @@ then
 	irlock start -X
 fi
 
+# PCF8583 counter (RPM sensor)
+if param compare -s SENS_EN_PCF8583 1
+then
+	pcf8583 start -X
+	pcf8583 start -X -a 0x51
+fi
+
 # probe for optional external I2C devices
 if param compare SENS_EXT_I2C_PRB 1
 then

src/drivers/rpm/pcf8583/PCF8583.cpp

@@ -51,41 +51,83 @@ int PCF8583::init()
 		  _param_pcf8583_reset.get(),
 		  _param_pcf8583_magnet.get());
 
-	// set counter mode
-	setRegister(0x00, 0b00100000);
-
-	// start measurement
-	resetCounter();
-
-	_rpm_pub.advertise();
-
+	initCounter();
 	ScheduleOnInterval(_param_pcf8583_pool.get());
+	_rpm_pub.advertise();
 
 	return PX4_OK;
 }
 
 int PCF8583::probe()
 {
+	setRegister(0x00, 0b00100000);
+
+	uint8_t s = readRegister(0x00);
+	PX4_DEBUG("status register: %" PRId8 " fail_count: %" PRId8, s, _tranfer_fail_count);
+
+	// PCF8583 contains free RAM registers
+	// This checks if I2C devices contains this RAM memory registers
+	// Some values are stored into this registers
+	// then it is vertified that the entered values fit.
+	setRegister(0x04, 10);
+	setRegister(0x05, 10);
+	setRegister(0x06, 10);
+	setRegister(0x0c, 5);
+	setRegister(0x0d, 5);
+	setRegister(0x0e, 5);
+	uint32_t tmp{0};
+
+	// check values stored in free RAM parts
+	tmp += readRegister(0x04);
+	tmp += readRegister(0x05);
+	tmp += readRegister(0x06);
+	tmp += readRegister(0x0c);
+	tmp += readRegister(0x0d);
+	tmp += readRegister(0x0e);
+
+	if (tmp != 45) {
+		return PX4_ERROR;
+	}
+
 	return PX4_OK;
 }
 
-int PCF8583::getCounter()
+void PCF8583::initCounter()
+{
+	// set counter mode
+	_tranfer_fail_count = 0;
+	setRegister(0x00, 0b00100000);
+	resetCounter();
+
+}
+
+uint32_t PCF8583::getCounter()
 {
+	// Counter value is stored in 9 words
+	// in 3 register as BCD value
 	uint8_t a = readRegister(0x01);
 	uint8_t b = readRegister(0x02);
 	uint8_t c = readRegister(0x03);
 
-	return int((a & 0x0f) * 1 + ((a & 0xf0) >> 4) * 10 + (b & 0x0f) * 100 + ((b & 0xf0) >> 4) * 1000 +
-		   (c & 0x0f) * 10000 + ((c & 0xf0) >> 4) * 1000000);
+	return uint32_t(
+		       hiWord(a) * 1u + loWord(a) * 10u
+		       + hiWord(b) * 100u + loWord(b) * 1000u
+		       + hiWord(c) * 10000u + loWord(c) * 1000000u);
 }
 
 void PCF8583::resetCounter()
 {
+	_last_measurement_time = hrt_absolute_time();
 	setRegister(0x01, 0x00);
 	setRegister(0x02, 0x00);
 	setRegister(0x03, 0x00);
+	_count = 0;
+	_last_reset_time = _last_measurement_time;
+	_reset_count ++;
 }
 
+
+// Configure PCF8583 driver into counting mode
 void PCF8583::setRegister(uint8_t reg, uint8_t value)
 {
 	uint8_t buff[2];
@@ -93,8 +135,13 @@ void PCF8583::setRegister(uint8_t reg, uint8_t value)
 	buff[1] = value;
 	int ret = transfer(buff, 2, nullptr, 0);
 
+	if (reg == 0x00) {
+		_last_config_register_content = value;
+	}
+
 	if (PX4_OK != ret) {
 		PX4_DEBUG("setRegister : i2c::transfer returned %d", ret);
+		_tranfer_fail_count++;
 	}
 }
 
@@ -105,6 +152,7 @@ uint8_t PCF8583::readRegister(uint8_t reg)
 
 	if (PX4_OK != ret) {
 		PX4_DEBUG("readRegister : i2c::transfer returned %d", ret);
+		_tranfer_fail_count++;
 	}
 
 	return rcv;
@@ -113,34 +161,61 @@ uint8_t PCF8583::readRegister(uint8_t reg)
 void PCF8583::RunImpl()
 {
 	// read sensor and compute frequency
-	int oldcount = _count;
-	uint64_t oldtime = _last_measurement_time;
+	int32_t oldcount = _count;
+
+	int32_t diffTime = hrt_elapsed_time(&_last_measurement_time);
+
+	// check if delay is enought
+	if (diffTime < _param_pcf8583_pool.get() / 2) {
+		PX4_ERR("pcf8583 loop called too early");
+		return;
+	}
 
 	_count = getCounter();
 	_last_measurement_time = hrt_absolute_time();
 
-	int diffCount = _count - oldcount;
-	uint64_t diffTime = _last_measurement_time - oldtime;
+	int32_t diffCount = _count - oldcount;
 
-	if (_param_pcf8583_reset.get() < _count + diffCount) {
+	// check if there is enought space in counter
+	// Otherwise, reset counter
+	if (diffCount > (999999 - oldcount)) {
+		PX4_ERR("pcf8583 RPM register overflow");
 		resetCounter();
-		_last_measurement_time = hrt_absolute_time();
-		_count = 0;
+		return;
 	}
 
-	float indicated_rpm = (float)diffCount / _param_pcf8583_magnet.get() / ((float)diffTime / 1000000) * 60.f;
+	//check if device failed or reset
+	uint8_t s = readRegister(0x00);
+
+	if (_tranfer_fail_count > 0 || s != 0b00100000 || diffCount < 0) {
+		PX4_ERR("pcf8583 RPM sensor restart: fail count %d, status: %d, diffCount: %ld",
+			_tranfer_fail_count, s, diffCount);
+		initCounter();
+		return;
+	}
+
+	// Calculate RPM and accuracy estimation
+	float indicated_rpm = (((float)diffCount / _param_pcf8583_magnet.get()) / ((float)diffTime / 1000000.f)) * 60.f;
 	float estimated_accurancy = 1 / (float)_param_pcf8583_magnet.get() / ((float)diffTime / 1000000) * 60.f;
 
-	// publish
+	// publish data to uorb
 	rpm_s msg{};
 	msg.indicated_frequency_rpm = indicated_rpm;
 	msg.estimated_accurancy_rpm = estimated_accurancy;
 	msg.timestamp = hrt_absolute_time();
 	_rpm_pub.publish(msg);
+
+	//check counter range
+	if (_param_pcf8583_reset.get() < diffCount + (int)_count) {
+		resetCounter();
+	}
 }
 
 void PCF8583::print_status()
 {
 	I2CSPIDriverBase::print_status();
 	PX4_INFO("poll interval:  %" PRId32 " us", _param_pcf8583_pool.get());
+	PX4_INFO("Last reset %.3fs ago, Count of resets: %d", (double)(hrt_absolute_time() - _last_reset_time) / 1000000.0,
+		 _reset_count);
+	PX4_INFO("Last count %ld", _count);
 }

src/drivers/rpm/pcf8583/PCF8583.hpp

@@ -48,6 +48,7 @@
 #include <px4_platform_common/i2c_spi_buses.h>
 #include <drivers/device/i2c.h>
 #include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/rpm.h>
 #include <drivers/drv_hrt.h>
 
@@ -71,16 +72,24 @@ private:
 
 	int  probe() override;
 
-	int            getCounter();
+	void           initCounter();
+	uint32_t       getCounter();
 	void           resetCounter();
 
 	uint8_t        readRegister(uint8_t reg);
 	void           setRegister(uint8_t reg, uint8_t value);
 
-	int            _count{0};
+	uint8_t        hiWord(uint8_t in) { return (in & 0x0fu); }
+	uint8_t        loWord(uint8_t in) { return ((in & 0xf0u) >> 4); }
+
+	uint32_t       _count{0};
+	uint16_t       _reset_count{0};
 	hrt_abstime    _last_measurement_time{0};
+	hrt_abstime    _last_reset_time{0};
+	int            _tranfer_fail_count{0};
+	uint8_t        _last_config_register_content{0x00};
 
-	uORB::Publication<rpm_s> _rpm_pub{ORB_ID(rpm)};
+	uORB::PublicationMulti<rpm_s> _rpm_pub{ORB_ID(rpm)};
 
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::PCF8583_POOL>) _param_pcf8583_pool,

src/drivers/rpm/pcf8583/parameters.c

@@ -31,6 +31,21 @@
  *
  ****************************************************************************/
 
+/**
+* PCF8583 eneable driver
+*
+* Run PCF8583 driver automatically
+*
+* @reboot_required true
+* @min 0
+* @max 1
+* @group Sensors
+* @value 0 Disabled
+* @value 1 Eneabled
+*/
+PARAM_DEFINE_INT32(SENS_EN_PCF8583, 0);
+
+
 /**
  * PCF8583 rotorfreq (i2c) pool interval
  *
@@ -42,16 +57,6 @@
  */
 PARAM_DEFINE_INT32(PCF8583_POOL, 1000000);
 
-/**
- * PCF8583 rotorfreq (i2c) i2c address
- *
- * @reboot_required true
- * @group Sensors
- * @value 80 Address 0x50 (80)
- * @value 81 Address 0x51 (81)
- */
-PARAM_DEFINE_INT32(PCF8583_ADDR, 80);
-
 /**
  * PCF8583 rotorfreq (i2c) pulse reset value
  *

src/drivers/rpm/pcf8583/pcf8583_main.cpp

@@ -40,9 +40,11 @@ void
 PCF8583::print_usage()
 {
 	PRINT_MODULE_USAGE_NAME("pcf8583", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("rpm_sensor");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
-	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(80);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x50);
+	PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG();
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
@@ -50,11 +52,11 @@ extern "C" __EXPORT int pcf8583_main(int argc, char *argv[])
 {
 	using ThisDriver = PCF8583;
 	BusCLIArguments cli{true, false};
-	cli.default_i2c_frequency = 400000;
+	cli.default_i2c_frequency = 100000;
 
-	int32_t addr{80};
-	param_get(param_find("PCF8583_ADDR"), &addr);
-	cli.i2c_address = addr;
+	//int32_t addr{0x50};
+	cli.i2c_address = 0x50;
+	cli.support_keep_running = true;
 
 	const char *verb = cli.parseDefaultArguments(argc, argv);
 

src/modules/logger/logged_topics.cpp

@@ -83,7 +83,6 @@ void LoggedTopics::add_default_topics()
 	add_topic("position_setpoint_triplet", 200);
 	add_optional_topic("px4io_status");
 	add_topic("radio_status");
-	add_optional_topic("rpm", 500);
 	add_topic("rtl_time_estimate", 1000);
 	add_topic("safety");
 	add_topic("sensor_combined");
@@ -123,6 +122,7 @@ void LoggedTopics::add_default_topics()
 	add_topic_multi("control_allocator_status", 200, 2);
 	add_optional_topic_multi("rate_ctrl_status", 200, 2);
 	add_topic_multi("sensor_hygrometer", 500, 4);
+	add_optional_topic_multi("rpm", 200);
 	add_optional_topic_multi("telemetry_status", 1000, 4);
 
 	// EKF multi topics (currently max 9 estimators)

src/modules/mavlink/streams/RAW_RPM.hpp

@@ -49,29 +49,33 @@ public:
 
 	unsigned get_size() override
 	{
-		return _rpm_sub.advertised() ? (MAVLINK_MSG_ID_RAW_RPM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+		return _rpm_subs.advertised_count() * (MAVLINK_MSG_ID_RAW_RPM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
 	}
 
 private:
 	explicit MavlinkStreamRawRpm(Mavlink *mavlink) : MavlinkStream(mavlink) {}
 
-	uORB::Subscription _rpm_sub{ORB_ID(rpm)};
+	uORB::SubscriptionMultiArray<rpm_s> _rpm_subs{ORB_ID::rpm};
 
 	bool send() override
 	{
+		bool updated = false;
+
+		for (int i = 0; i < _rpm_subs.size(); i++) {
 			rpm_s rpm;
 
-		if (_rpm_sub.update(&rpm)) {
+			if (_rpm_subs[i].update(&rpm)) {
 				mavlink_raw_rpm_t msg{};
 
+				msg.index = i;
 				msg.frequency = rpm.indicated_frequency_rpm;
 
 				mavlink_msg_raw_rpm_send_struct(_mavlink->get_channel(), &msg);
-
-			return true;
+				updated = true;
+			}
 		}
 
-		return false;
+		return updated;
 	}
 };