Implement custom factory, preset default flags

roboticslab-uc3m · Aug 5, 2019 · b87cc83 · b87cc83
1 parent 2133db1
commit b87cc83
Show file tree

Hide file tree

Showing 2 changed files with 40 additions and 11 deletions.
diff --git a/libraries/YarpPlugins/CanBusPeak/CanBusPeak.hpp b/libraries/YarpPlugins/CanBusPeak/CanBusPeak.hpp
@@ -29,14 +29,49 @@ namespace roboticslab
 {
 
 /**
- *
+ * @ingroup YarpPlugins
+ * @defgroup CanBusPeak
+ * @brief Contains roboticslab::CanBusPeak.
+ */
+
+/**
+ * @ingroup CanBusPeak
+ * @brief Custom buffer of PeakCanMessage instances.
+ */
+class ImplementPeakCanBufferFactory : public yarp::dev::ImplementCanBufferFactory<PeakCanMessage, struct pcanfd_msg>
+{
+public:
+    virtual yarp::dev::CanBuffer createBuffer(int elem)
+    {
+        yarp::dev::CanBuffer ret;
+        struct pcanfd_msg * storage = new pcanfd_msg[elem];
+        yarp::dev::CanMessage ** messages = new yarp::dev::CanMessage *[elem];
+        PeakCanMessage * tmp = new PeakCanMessage[elem];
+
+        std::memset(storage, 0, sizeof(struct pcanfd_msg) * elem);
+
+        for (int k = 0; k < elem; k++)
+        {
+            messages[k] = &tmp[k];
+            messages[k]->setBuffer(reinterpret_cast<unsigned char *>(&storage[k]));
+
+            // Changes wrt default implementation:
+            storage[k].type = PCANFD_TYPE_CAN20_MSG;
+            storage[k].flags = PCANFD_MSG_STD;
+        }
+
+        ret.resize(messages, elem);
+        return ret;
+    }
+};
+
+/**
  * @ingroup CanBusPeak
  * @brief Specifies the PeakCan behaviour and specifications.
- *
  */
 class CanBusPeak : public yarp::dev::DeviceDriver,
                    public yarp::dev::ICanBus,
-                   public yarp::dev::ImplementCanBufferFactory<PeakCanMessage, struct pcanfd_msg>
+                   public ImplementPeakCanBufferFactory
 {
 
 public:

diff --git a/libraries/YarpPlugins/CanBusPeak/ICanBusImpl.cpp b/libraries/YarpPlugins/CanBusPeak/ICanBusImpl.cpp
@@ -210,18 +210,12 @@ bool roboticslab::CanBusPeak::canWrite(const yarp::dev::CanBuffer & msgs, unsign
 
         // Point at first member of an internally defined array of pcanfd_msg structs.
 #if YARP_VERSION_MINOR >= 2 // note: remove #include <yarp/conf/version.h>
-        struct pcanfd_msg * pfdm = reinterpret_cast<struct pcanfd_msg *>(msgs.getPointer()[0]->getPointer());
+        const struct pcanfd_msg * pfdm = reinterpret_cast<const struct pcanfd_msg *>(msgs.getPointer()[0]->getPointer());
 #else
         yarp::dev::CanBuffer & msgs_not_const = const_cast<yarp::dev::CanBuffer &>(msgs);
-        struct pcanfd_msg * pfdm = reinterpret_cast<struct pcanfd_msg *>(msgs_not_const.getPointer()[0]->getPointer());
+        const struct pcanfd_msg * pfdm = reinterpret_cast<const struct pcanfd_msg *>(msgs_not_const.getPointer()[0]->getPointer());
 #endif
 
-        for (unsigned int i = 0; i < size; i++)
-        {
-            pfdm[i].type = PCANFD_TYPE_CAN20_MSG;
-            pfdm[i].flags = PCANFD_MSG_STD;
-        }
-
         if (blockingMode && txTimeoutMs > 0)
         {
             bool bufferReady;