From 0836160a0f0580298b4b0a0976b3ef0ccd0f7e05 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Tue, 26 Sep 2023 15:37:06 +0200 Subject: [PATCH] Perform CAN initialization in monitor thread --- .../YarpPlugins/Jr3Mbed/DeviceDriverImpl.cpp | 47 +++++++++++++------ .../YarpPlugins/Jr3Mbed/ICanBusSharerImpl.cpp | 11 ++--- .../ISixAxisForceTorqueSensorsImpl.cpp | 4 +- libraries/YarpPlugins/Jr3Mbed/Jr3Mbed.hpp | 6 ++- 4 files changed, 43 insertions(+), 25 deletions(-) diff --git a/libraries/YarpPlugins/Jr3Mbed/DeviceDriverImpl.cpp b/libraries/YarpPlugins/Jr3Mbed/DeviceDriverImpl.cpp index fc5533242..a025c19f2 100644 --- a/libraries/YarpPlugins/Jr3Mbed/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/Jr3Mbed/DeviceDriverImpl.cpp @@ -129,29 +129,46 @@ bool Jr3Mbed::open(yarp::os::Searchable & config) status = yarp::dev::MAS_WAITING_FOR_FIRST_READ; - monitorThread = new yarp::os::Timer(yarp::os::TimerSettings(monitorPeriod), [this](const auto & event) + monitorThread = new yarp::os::Timer(monitorPeriod, [this](const auto & event) { - std::lock_guard lock(rxMutex); - - double elapsed = event.currentReal - timestamp; - - if (elapsed < event.lastDuration) + if (isBooting) { - if (status != yarp::dev::MAS_OK) // either timeout or awaiting first read + status = yarp::dev::MAS_WAITING_FOR_FIRST_READ; + + if (!initialize()) { - yCIInfo(JR3M, id()) << "Sensor is responding"; + // perhaps it did initialize correctly, but CAN comms timed out; + // otherwise, we could have used MAS_ERROR here instead + status = yarp::dev::MAS_TIMEOUT; } - status = yarp::dev::MAS_OK; - } - else if (status == yarp::dev::MAS_OK) // timeout! - { - yCIWarning(JR3M, id()) << "Sensor has timed out, last data was received" << elapsed << "seconds ago"; - status = yarp::dev::MAS_TIMEOUT; + isBooting = false; } else { - // still waiting for first read + mtx.lock(); + auto localTimestamp = timestamp; + mtx.unlock(); + + auto elapsed = event.currentReal - localTimestamp; + + if (elapsed < event.lastDuration) // we have received data in time + { + if (status != yarp::dev::MAS_OK) // either timeout or awaiting first read + { + yCIInfo(JR3M, id()) << "Sensor is responding"; + status = yarp::dev::MAS_OK; + } + } + else if (status == yarp::dev::MAS_OK) // timeout! + { + yCIWarning(JR3M, id()) << "Sensor has timed out, last data was received" << elapsed << "seconds ago"; + status = yarp::dev::MAS_TIMEOUT; + } + else + { + // still in timeout state or waiting for first read + } } return true; diff --git a/libraries/YarpPlugins/Jr3Mbed/ICanBusSharerImpl.cpp b/libraries/YarpPlugins/Jr3Mbed/ICanBusSharerImpl.cpp index 1d442343e..2bd2d5368 100644 --- a/libraries/YarpPlugins/Jr3Mbed/ICanBusSharerImpl.cpp +++ b/libraries/YarpPlugins/Jr3Mbed/ICanBusSharerImpl.cpp @@ -73,16 +73,16 @@ bool Jr3Mbed::notifyMessage(const can_message & message) case JR3_BOOTUP: { yCIInfo(JR3M, id()) << "Bootup message received"; - std::lock_guard lock(rxMutex); - status = yarp::dev::MAS_WAITING_FOR_FIRST_READ; - return initialize(); + isBooting = true; + // can't block here, let the monitor thread call the initialization routine + return true; } case JR3_ACK: return ackStateObserver->notify(); case JR3_GET_FORCES: { auto [forces, counter] = parseData(message); - std::lock_guard lock(rxMutex); + std::lock_guard lock(mtx); buffer = forces; integrityCounter = counter; return true; @@ -90,9 +90,8 @@ bool Jr3Mbed::notifyMessage(const can_message & message) case JR3_GET_MOMENTS: { auto [moments, counter] = parseData(message); - std::lock_guard lock(rxMutex); - if (counter == integrityCounter) + if (std::lock_guard lock(mtx); counter == integrityCounter) { std::copy(buffer.cbegin(), buffer.cend(), raw.begin()); std::copy(moments.cbegin(), moments.cend(), raw.begin() + 3); diff --git a/libraries/YarpPlugins/Jr3Mbed/ISixAxisForceTorqueSensorsImpl.cpp b/libraries/YarpPlugins/Jr3Mbed/ISixAxisForceTorqueSensorsImpl.cpp index ff630c803..da52aacd6 100644 --- a/libraries/YarpPlugins/Jr3Mbed/ISixAxisForceTorqueSensorsImpl.cpp +++ b/libraries/YarpPlugins/Jr3Mbed/ISixAxisForceTorqueSensorsImpl.cpp @@ -18,7 +18,6 @@ std::size_t Jr3Mbed::getNrOfSixAxisForceTorqueSensors() const yarp::dev::MAS_status Jr3Mbed::getSixAxisForceTorqueSensorStatus(std::size_t sens_index) const { - std::lock_guard lock(rxMutex); return status; } @@ -41,7 +40,8 @@ bool Jr3Mbed::getSixAxisForceTorqueSensorFrameName(std::size_t sens_index, std:: bool Jr3Mbed::getSixAxisForceTorqueSensorMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const { - std::lock_guard lock(rxMutex); + out.resize(raw.size()); + std::lock_guard lock(mtx); std::transform(raw.cbegin(), raw.cend(), scales.cbegin(), out.begin(), std::multiplies<>{}); timestamp = this->timestamp; return true; diff --git a/libraries/YarpPlugins/Jr3Mbed/Jr3Mbed.hpp b/libraries/YarpPlugins/Jr3Mbed/Jr3Mbed.hpp index a03eb071e..281728c1c 100644 --- a/libraries/YarpPlugins/Jr3Mbed/Jr3Mbed.hpp +++ b/libraries/YarpPlugins/Jr3Mbed/Jr3Mbed.hpp @@ -6,6 +6,7 @@ #include #include +#include #include #include @@ -99,13 +100,14 @@ class Jr3Mbed : public yarp::dev::DeviceDriver, ICanSenderDelegate * sender {nullptr}; StateObserver * ackStateObserver {nullptr}; - mutable std::mutex rxMutex; + mutable std::mutex mtx; std::array buffer {}; // zero-initialize std::array raw {}; // zero-initialize + std::atomic status {yarp::dev::MAS_UNKNOWN}; + std::atomic_bool isBooting {false}; double timestamp {0.0}; - yarp::dev::MAS_status status {yarp::dev::MAS_UNKNOWN}; std::uint16_t integrityCounter {0}; yarp::os::Timer * monitorThread {nullptr};