From 976c994156f703d1f24b7042186148886d9e4860 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Roman=20Dvo=C5=99=C3=A1k?= Date: Mon, 21 Feb 2022 22:47:16 +0100 Subject: [PATCH] Extend the PCF8583 driver to support multiple instances (#19232) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add some restart events into pcf8583 driver Co-authored-by: Vít Hanousek --- ROMFS/px4fmu_common/init.d/rc.sensors | 7 ++ src/drivers/rpm/pcf8583/PCF8583.cpp | 115 +++++++++++++++++++---- src/drivers/rpm/pcf8583/PCF8583.hpp | 15 ++- src/drivers/rpm/pcf8583/parameters.c | 25 +++-- src/drivers/rpm/pcf8583/pcf8583_main.cpp | 12 ++- src/modules/logger/logged_topics.cpp | 2 +- src/modules/mavlink/streams/RAW_RPM.hpp | 22 +++-- 7 files changed, 150 insertions(+), 48 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 811695a6b5..4f89a005fe 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/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 diff --git a/src/drivers/rpm/pcf8583/PCF8583.cpp b/src/drivers/rpm/pcf8583/PCF8583.cpp index bc5d54d19b..8ab28995b0 100644 --- a/src/drivers/rpm/pcf8583/PCF8583.cpp +++ b/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; + } + + //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; } - float indicated_rpm = (float)diffCount / _param_pcf8583_magnet.get() / ((float)diffTime / 1000000) * 60.f; + // 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); } diff --git a/src/drivers/rpm/pcf8583/PCF8583.hpp b/src/drivers/rpm/pcf8583/PCF8583.hpp index 34c19c2d43..453befe407 100644 --- a/src/drivers/rpm/pcf8583/PCF8583.hpp +++ b/src/drivers/rpm/pcf8583/PCF8583.hpp @@ -48,6 +48,7 @@ #include #include #include +#include #include #include @@ -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_pub{ORB_ID(rpm)}; + uORB::PublicationMulti _rpm_pub{ORB_ID(rpm)}; DEFINE_PARAMETERS( (ParamInt) _param_pcf8583_pool, diff --git a/src/drivers/rpm/pcf8583/parameters.c b/src/drivers/rpm/pcf8583/parameters.c index 1ed06b50fb..bf2d41b6fd 100644 --- a/src/drivers/rpm/pcf8583/parameters.c +++ b/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 * diff --git a/src/drivers/rpm/pcf8583/pcf8583_main.cpp b/src/drivers/rpm/pcf8583/pcf8583_main.cpp index c52c779254..be584d7969 100644 --- a/src/drivers/rpm/pcf8583/pcf8583_main.cpp +++ b/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); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 69e7495495..a878d38ee2 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/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) diff --git a/src/modules/mavlink/streams/RAW_RPM.hpp b/src/modules/mavlink/streams/RAW_RPM.hpp index ffa4001cb0..9d2088123f 100644 --- a/src/modules/mavlink/streams/RAW_RPM.hpp +++ b/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_subs{ORB_ID::rpm}; bool send() override { - rpm_s rpm; + bool updated = false; - if (_rpm_sub.update(&rpm)) { - mavlink_raw_rpm_t msg{}; + for (int i = 0; i < _rpm_subs.size(); i++) { + rpm_s rpm; - msg.frequency = rpm.indicated_frequency_rpm; + if (_rpm_subs[i].update(&rpm)) { + mavlink_raw_rpm_t msg{}; - mavlink_msg_raw_rpm_send_struct(_mavlink->get_channel(), &msg); + msg.index = i; + msg.frequency = rpm.indicated_frequency_rpm; - return true; + mavlink_msg_raw_rpm_send_struct(_mavlink->get_channel(), &msg); + updated = true; + } } - return false; + return updated; } };