[WIP]

roboticslab-uc3m · Sep 26, 2023 · cc361a6 · cc361a6
1 parent cb8bbf9
commit cc361a6
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 10 deletions.
diff --git a/libraries/YarpPlugins/Jr3Mbed/DeviceDriverImpl.cpp b/libraries/YarpPlugins/Jr3Mbed/DeviceDriverImpl.cpp
@@ -48,6 +48,12 @@ bool Jr3Mbed::open(yarp::os::Searchable & config)
         return false;
     }
 
+    if (filter < 0.0)
+    {
+        yCError(JR3M) << "Illegal filter value:" << filter;
+        return false;
+    }
+
     yarp::dev::DeviceDriver::setId("ID" + std::to_string(canId));
 
     auto ackTimeout = jr3Group.check("ackTimeout", yarp::os::Value(DEFAULT_ACK_TIMEOUT), "CAN acknowledge timeout (seconds)").asFloat64();
@@ -125,29 +131,37 @@ 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);
+            rxMutex.lock();
+            auto localTimestamp = timestamp;
+            auto localStatus = status;
+            rxMutex.unlock();
 
-            double elapsed = event.currentReal - timestamp;
+            auto elapsed = event.currentReal - localTimestamp;
 
-            if (elapsed < event.lastDuration)
+            if (elapsed < event.lastDuration) // we have received data in time
             {
-                if (status != yarp::dev::MAS_OK) // either timeout or awaiting first read
+                if (localStatus != yarp::dev::MAS_OK) // either timeout or awaiting first read
                 {
                     yCIInfo(JR3M, id()) << "Sensor is responding";
-                }
 
-                status = yarp::dev::MAS_OK;
+                    if (localStatus != yarp::dev::MAS_WAITING_FOR_FIRST_READ || initialize())
+                    {
+                        std::lock_guard lock(rxMutex);
+                        status = yarp::dev::MAS_OK;
+                    }
+                }
             }
-            else if (status == yarp::dev::MAS_OK) // timeout!
+            else if (localStatus == yarp::dev::MAS_OK) // timeout!
             {
                 yCIWarning(JR3M, id()) << "Sensor has timed out, last data was received" << elapsed << "seconds ago";
+                std::lock_guard lock(rxMutex);
                 status = yarp::dev::MAS_TIMEOUT;
             }
             else
             {
-                // still waiting for first read
+                // 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
@@ -72,7 +72,8 @@ bool Jr3Mbed::notifyMessage(const can_message & message)
         yCIInfo(JR3M, id()) << "Bootup message received";
         std::lock_guard lock(rxMutex);
         status = yarp::dev::MAS_WAITING_FOR_FIRST_READ;
-        return initialize();
+        // can't block here, let the monitor thread call the initialization routine
+        return true;
     }
     case JR3_ACK:
         return ackStateObserver->notify();