Perform CAN initialization in monitor thread

roboticslab-uc3m · Oct 7, 2023 · 0836160 · 0836160
1 parent 8bc3388
commit 0836160
Show file tree

Hide file tree

Showing 4 changed files with 43 additions and 25 deletions.
diff --git 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
@@ -73,26 +73,25 @@ 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;
     }
     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
@@ -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
@@ -6,6 +6,7 @@
 #include <cstdint>
 
 #include <array>
+#include <atomic>
 #include <mutex>
 #include <string>
 
@@ -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<std::int16_t, 3> buffer {}; // zero-initialize
     std::array<std::int16_t, 6> raw {}; // zero-initialize
 
+    std::atomic<yarp::dev::MAS_status> 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};