From 0c8c400dae6bc5135de425894f3a928cbbc2cb42 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Mon, 6 May 2019 18:43:45 +0200 Subject: [PATCH 1/6] Parameterize and increase RX buffer length --- .../CanBusControlboard/CanBusControlboard.hpp | 7 ++++ .../CanBusControlboard/DeviceDriverImpl.cpp | 4 +- .../CanBusControlboard/ThreadImpl.cpp | 38 ++++++++++--------- 3 files changed, 30 insertions(+), 19 deletions(-) diff --git a/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp b/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp index 340805bf0..7c57499cc 100644 --- a/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp +++ b/libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp @@ -39,6 +39,9 @@ #define DEFAULT_LIN_INTERP_BUFFER_SIZE 1 #define DEFAULT_LIN_INTERP_MODE "pt" +#define DEFAULT_CAN_RX_BUFFER_SIZE 500 +#define DEFAULT_CAN_TX_BUFFER_SIZE 500 + namespace roboticslab { @@ -875,11 +878,15 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, std::map< int, int > idxFromCanId; + PositionDirectThread * posdThread; int linInterpPeriodMs; int linInterpBufferSize; std::string linInterpMode; + int canRxBufferSize; + int canTxBufferSize; + /** A helper function to display CAN messages. */ std::string msgToStr(const yarp::dev::CanMessage& message); diff --git a/libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp b/libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp index 9ed98ca91..31ad1b88d 100644 --- a/libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp @@ -13,6 +13,8 @@ bool roboticslab::CanBusControlboard::open(yarp::os::Searchable& config) std::string mode = config.check("mode",yarp::os::Value("position"),"control mode on startup (position/velocity)").asString(); int timeCuiWait = config.check("waitEncoder", yarp::os::Value(DEFAULT_TIME_TO_WAIT_CUI), "CUI timeout (seconds)").asInt32(); std::string canBusType = config.check("canBusType", yarp::os::Value(DEFAULT_CAN_BUS), "CAN bus device name").asString(); + canRxBufferSize = config.check("canBusRxBufferSize", yarp::os::Value(DEFAULT_CAN_RX_BUFFER_SIZE), "CAN bus RX buffer size").asInt(); + canTxBufferSize = config.check("canBusTxBufferSize", yarp::os::Value(DEFAULT_CAN_TX_BUFFER_SIZE), "CAN bus TX buffer size").asInt(); linInterpPeriodMs = config.check("linInterpPeriodMs", yarp::os::Value(DEFAULT_LIN_INTERP_PERIOD_MS), "linear interpolation mode period (milliseconds)").asInt32(); linInterpBufferSize = config.check("linInterpBufferSize", yarp::os::Value(DEFAULT_LIN_INTERP_BUFFER_SIZE), "linear interpolation mode buffer size").asInt32(); @@ -55,7 +57,7 @@ bool roboticslab::CanBusControlboard::open(yarp::os::Searchable& config) return false; } - canInputBuffer = iCanBufferFactory->createBuffer(1); + canInputBuffer = iCanBufferFactory->createBuffer(canRxBufferSize); //-- Start the reading thread (required for checkMotionDoneRaw). this->Thread::start(); diff --git a/libraries/YarpPlugins/CanBusControlboard/ThreadImpl.cpp b/libraries/YarpPlugins/CanBusControlboard/ThreadImpl.cpp index d13fd3dfd..919fd5990 100644 --- a/libraries/YarpPlugins/CanBusControlboard/ThreadImpl.cpp +++ b/libraries/YarpPlugins/CanBusControlboard/ThreadImpl.cpp @@ -20,36 +20,38 @@ void roboticslab::CanBusControlboard::run() unsigned int read; - //-- Blocks with timeout until one message arrives, returns false on errors. - bool ok = iCanBus->canRead(canInputBuffer, 1, &read, true); + //-- Blocks with timeout until a message arrives, returns false on errors. + bool ok = iCanBus->canRead(canInputBuffer, canRxBufferSize, &read, true); //-- All debugging messages should be contained in canRead, so just loop again. if( !ok || read == 0 ) continue; - const yarp::dev::CanMessage &msg = canInputBuffer[0]; - int canId = msg.getId() & 0x7F; + for (int i = 0; i < read; i++) + { + const yarp::dev::CanMessage &msg = canInputBuffer[i]; + int canId = msg.getId() & 0x7F; - //-- Commenting next line as way too verbose, happens all the time. - //CD_DEBUG("Read ok. %s\n", msgToStr(msg).c_str()); + //-- Commenting next line as way too verbose, happens all the time. + //CD_DEBUG("Read ok. %s\n", msgToStr(msg).c_str()); - std::map< int, int >::iterator idxFromCanIdFound = idxFromCanId.find(canId); + std::map< int, int >::iterator idxFromCanIdFound = idxFromCanId.find(canId); - if( idxFromCanIdFound == idxFromCanId.end() ) //-- Can ID not found - { - //-- Intercept 700h 0 msg that just indicates presence. - if( msg.getId() - canId == 0x700 ) + if( idxFromCanIdFound == idxFromCanId.end() ) //-- Can ID not found { - CD_SUCCESS("Device indicating presence. %s\n", msgToStr(msg).c_str()); + //-- Intercept 700h 0 msg that just indicates presence. + if( msg.getId() - canId == 0x700 ) + { + CD_SUCCESS("Device indicating presence. %s\n", msgToStr(msg).c_str()); + continue; + } + + //CD_ERROR("Read CAN message from unknown device!!! %s\n", msgToStr(msg).c_str()); // --Commented this line to avoid filling the screen with error messages continue; } - //CD_ERROR("Read CAN message from unknown device!!! %s\n", msgToStr(msg).c_str()); // --Commented this line to avoid filling the screen with error messages - continue; + //CD_DEBUG("idxFromCanIdFound->second: %d\n",idxFromCanIdFound->second); + iCanBusSharer[ idxFromCanIdFound->second ]->interpretMessage(msg); //-- Check if false? } - - //CD_DEBUG("idxFromCanIdFound->second: %d\n",idxFromCanIdFound->second); - iCanBusSharer[ idxFromCanIdFound->second ]->interpretMessage(msg); //-- Check if false? - } //-- ends: while ( ! this->isStopping() ). CD_INFO("Stopping CanBusControlboard reading thread run.\n"); From 485758196ef61c4f2ecdba7986670db725eeb214 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Tue, 7 May 2019 01:08:13 +0200 Subject: [PATCH 2/6] Adopt C++-style reinterpret casts --- libraries/YarpPlugins/CanBusFake/FakeCanMessage.cpp | 6 +++--- libraries/YarpPlugins/CanBusHico/HicoCanMessage.cpp | 6 +++--- libraries/YarpPlugins/CanBusPeak/PeakCanMessage.cpp | 6 +++--- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/libraries/YarpPlugins/CanBusFake/FakeCanMessage.cpp b/libraries/YarpPlugins/CanBusFake/FakeCanMessage.cpp index 3a3b1845e..002beeee5 100644 --- a/libraries/YarpPlugins/CanBusFake/FakeCanMessage.cpp +++ b/libraries/YarpPlugins/CanBusFake/FakeCanMessage.cpp @@ -72,14 +72,14 @@ unsigned char * roboticslab::FakeCanMessage::getData() unsigned char * roboticslab::FakeCanMessage::getPointer() { - return (unsigned char *) message; + return reinterpret_cast(message); } // ----------------------------------------------------------------------------- const unsigned char * roboticslab::FakeCanMessage::getPointer() const { - return (const unsigned char *) message; + return reinterpret_cast(message); } // ----------------------------------------------------------------------------- @@ -88,7 +88,7 @@ void roboticslab::FakeCanMessage::setBuffer(unsigned char * buf) { if (buf != 0) { - message = (struct fake_can_msg *) buf; + message = reinterpret_cast(buf); } } diff --git a/libraries/YarpPlugins/CanBusHico/HicoCanMessage.cpp b/libraries/YarpPlugins/CanBusHico/HicoCanMessage.cpp index d2b1b26d5..d3dc4b055 100644 --- a/libraries/YarpPlugins/CanBusHico/HicoCanMessage.cpp +++ b/libraries/YarpPlugins/CanBusHico/HicoCanMessage.cpp @@ -72,14 +72,14 @@ unsigned char * roboticslab::HicoCanMessage::getData() unsigned char * roboticslab::HicoCanMessage::getPointer() { - return (unsigned char *) message; + return reinterpret_cast(message); } // ----------------------------------------------------------------------------- const unsigned char * roboticslab::HicoCanMessage::getPointer() const { - return (const unsigned char *) message; + return reinterpret_cast(message); } // ----------------------------------------------------------------------------- @@ -88,7 +88,7 @@ void roboticslab::HicoCanMessage::setBuffer(unsigned char * buf) { if (buf != 0) { - message = (struct can_msg *) buf; + message = reinterpret_cast(buf); } } diff --git a/libraries/YarpPlugins/CanBusPeak/PeakCanMessage.cpp b/libraries/YarpPlugins/CanBusPeak/PeakCanMessage.cpp index 7e2450c53..1290fc0a4 100644 --- a/libraries/YarpPlugins/CanBusPeak/PeakCanMessage.cpp +++ b/libraries/YarpPlugins/CanBusPeak/PeakCanMessage.cpp @@ -72,14 +72,14 @@ unsigned char * roboticslab::PeakCanMessage::getData() unsigned char * roboticslab::PeakCanMessage::getPointer() { - return (unsigned char *)message; + return reinterpret_cast(message); } // ----------------------------------------------------------------------------- const unsigned char * roboticslab::PeakCanMessage::getPointer() const { - return (const unsigned char *)message; + return reinterpret_cast(message); } // ----------------------------------------------------------------------------- @@ -88,7 +88,7 @@ void roboticslab::PeakCanMessage::setBuffer(unsigned char * buf) { if (buf != 0) { - message = (struct pcanfd_msg *)buf; + message = reinterpret_cast(buf); } } From eb4f5e050aaef4c1f6d123b054daf4a12d444227 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Tue, 7 May 2019 12:50:21 +0200 Subject: [PATCH 3/6] Avoid making copies of incoming/outgoing messages --- .../YarpPlugins/CanBusPeak/ICanBusImpl.cpp | 48 ++++++------------- 1 file changed, 15 insertions(+), 33 deletions(-) diff --git a/libraries/YarpPlugins/CanBusPeak/ICanBusImpl.cpp b/libraries/YarpPlugins/CanBusPeak/ICanBusImpl.cpp index 5bc7ac31b..a745b66c8 100644 --- a/libraries/YarpPlugins/CanBusPeak/ICanBusImpl.cpp +++ b/libraries/YarpPlugins/CanBusPeak/ICanBusImpl.cpp @@ -157,8 +157,6 @@ bool roboticslab::CanBusPeak::canRead(yarp::dev::CanBuffer & msgs, unsigned int return false; } - struct pcanfd_msg * pfdm = new struct pcanfd_msg[size]; - canBusReady.wait(); if (blockingMode && rxTimeoutMs > 0) @@ -168,7 +166,6 @@ bool roboticslab::CanBusPeak::canRead(yarp::dev::CanBuffer & msgs, unsigned int if (!waitUntilTimeout(READ, &bufferReady)) { canBusReady.post(); CD_ERROR("waitUntilTimeout() failed.\n"); - delete[] pfdm; return false; } @@ -176,11 +173,12 @@ bool roboticslab::CanBusPeak::canRead(yarp::dev::CanBuffer & msgs, unsigned int { canBusReady.post(); *read = 0; - delete[] pfdm; return true; } } + // Point at first member of an internally defined array of pcandf_msg structs. + struct pcanfd_msg * pfdm = reinterpret_cast(msgs.getPointer()[0]->getPointer()); int res = pcanfd_recv_msgs_list(fileDescriptor, size, pfdm); canBusReady.post(); @@ -188,29 +186,18 @@ bool roboticslab::CanBusPeak::canRead(yarp::dev::CanBuffer & msgs, unsigned int if (!blockingMode && res == -EWOULDBLOCK) { *read = 0; - delete[] pfdm; return true; } else if (res < 0) { CD_ERROR("Unable to read messages: %s.\n", std::strerror(-res)); - delete[] pfdm; return false; } - - *read = res; - - for (unsigned int i = 0; i < res; i++) + else { - yarp::dev::CanMessage & msg = msgs[i]; - std::memcpy(msg.getData(), pfdm[i].data, pfdm[i].data_len); - msg.setLen(pfdm[i].data_len); - msg.setId(pfdm[i].id); + *read = res; + return true; } - - delete[] pfdm; - - return true; } // ----------------------------------------------------------------------------- @@ -223,20 +210,18 @@ bool roboticslab::CanBusPeak::canWrite(const yarp::dev::CanBuffer & msgs, unsign return false; } - struct pcanfd_msg * pfdm = new struct pcanfd_msg[size]; + canBusReady.wait(); + + // Point at first member of an internally defined array of pcandf_msg structs. + yarp::dev::CanBuffer & msgs_not_const = const_cast(msgs); + struct pcanfd_msg * pfdm = reinterpret_cast(msgs_not_const.getPointer()[0]->getPointer()); for (unsigned int i = 0; i < size; i++) { - const yarp::dev::CanMessage & msg = msgs[i]; - std::memcpy(pfdm[i].data, msg.getData(), msg.getLen()); - pfdm[i].data_len = msg.getLen(); - pfdm[i].id = msg.getId(); pfdm[i].type = PCANFD_TYPE_CAN20_MSG; pfdm[i].flags = PCANFD_MSG_STD; } - canBusReady.wait(); - if (blockingMode && txTimeoutMs > 0) { bool bufferReady; @@ -244,7 +229,6 @@ bool roboticslab::CanBusPeak::canWrite(const yarp::dev::CanBuffer & msgs, unsign if (!waitUntilTimeout(WRITE, &bufferReady)) { canBusReady.post(); CD_ERROR("waitUntilTimeout() failed.\n"); - delete[] pfdm; return false; } @@ -252,7 +236,6 @@ bool roboticslab::CanBusPeak::canWrite(const yarp::dev::CanBuffer & msgs, unsign { canBusReady.post(); *sent = 0; - delete[] pfdm; return true; } } @@ -261,8 +244,6 @@ bool roboticslab::CanBusPeak::canWrite(const yarp::dev::CanBuffer & msgs, unsign canBusReady.post(); - delete[] pfdm; - if (!blockingMode && res == -EWOULDBLOCK) { *sent = 0; @@ -273,10 +254,11 @@ bool roboticslab::CanBusPeak::canWrite(const yarp::dev::CanBuffer & msgs, unsign CD_ERROR("Unable to send messages: %s.\n", std::strerror(-res)); return false; } - - *sent = res; - - return true; + else + { + *sent = res; + return true; + } } // ----------------------------------------------------------------------------- From d91e4ca5bd23cb8c3e109704e539e9d050a5378d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Mon, 5 Aug 2019 13:01:08 +0200 Subject: [PATCH 4/6] Adopt YARP's mutex lock guard abstraction --- .../YarpPlugins/CanBusHico/CanBusHico.hpp | 4 +- .../YarpPlugins/CanBusHico/ICanBusImpl.cpp | 41 ++----- .../YarpPlugins/CanBusPeak/CanBusPeak.hpp | 4 +- .../YarpPlugins/CanBusPeak/ICanBusImpl.cpp | 110 ++++++++---------- 4 files changed, 64 insertions(+), 95 deletions(-) diff --git a/libraries/YarpPlugins/CanBusHico/CanBusHico.hpp b/libraries/YarpPlugins/CanBusHico/CanBusHico.hpp index 0d69747d1..feea4494d 100644 --- a/libraries/YarpPlugins/CanBusHico/CanBusHico.hpp +++ b/libraries/YarpPlugins/CanBusHico/CanBusHico.hpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include #include @@ -133,7 +133,7 @@ class CanBusHico : public yarp::dev::DeviceDriver, bool blockingMode; bool allowPermissive; - yarp::os::Semaphore canBusReady; + yarp::os::Mutex canBusReady; std::pair bitrateState; diff --git a/libraries/YarpPlugins/CanBusHico/ICanBusImpl.cpp b/libraries/YarpPlugins/CanBusHico/ICanBusImpl.cpp index 27d2409cc..40a02b287 100644 --- a/libraries/YarpPlugins/CanBusHico/ICanBusImpl.cpp +++ b/libraries/YarpPlugins/CanBusHico/ICanBusImpl.cpp @@ -10,6 +10,8 @@ #include +#include + #include // ----------------------------------------------------------------------------- @@ -26,20 +28,18 @@ bool roboticslab::CanBusHico::canSetBaudRate(unsigned int rate) return false; } - canBusReady.wait(); + yarp::os::LockGuard lockGuard(canBusReady); if (bitrateState.first) { if (bitrateState.second == id) { CD_WARNING("Bitrate already set.\n"); - canBusReady.post(); return true; } else { CD_ERROR("Bitrate already set to a different value: %d.\n", bitrateState.second); - canBusReady.post(); return false; } } @@ -47,15 +47,12 @@ bool roboticslab::CanBusHico::canSetBaudRate(unsigned int rate) if (::ioctl(fileDescriptor, IOC_SET_BITRATE, &id) == -1) { CD_ERROR("Could not set bitrate: %s.\n", std::strerror(errno)); - canBusReady.post(); return false; } bitrateState.first = true; bitrateState.second = id; - canBusReady.post(); - return true; } @@ -67,9 +64,9 @@ bool roboticslab::CanBusHico::canGetBaudRate(unsigned int * rate) unsigned int id; - canBusReady.wait(); + canBusReady.lock(); int ret = ::ioctl(fileDescriptor, IOC_GET_BITRATE, &id); - canBusReady.post(); + canBusReady.unlock(); if (ret == -1) { @@ -104,31 +101,26 @@ bool roboticslab::CanBusHico::canIdAdd(unsigned int id) return false; } - canBusReady.wait(); + yarp::os::LockGuard lockGuard(canBusReady); if (filterManager->hasId(id)) { CD_WARNING("Filter for ID %d is already active.\n", id); - canBusReady.post(); return true; } if (!filterManager->insertId(id)) { CD_ERROR("Could not set filter: %s.\n", std::strerror(errno)); - canBusReady.post(); return false; } if (!filterManager->isValid()) { CD_WARNING("Hardware limit was hit, not all requested filters are enabled.\n"); - canBusReady.post(); return true; } - canBusReady.post(); - return true; } @@ -150,7 +142,7 @@ bool roboticslab::CanBusHico::canIdDelete(unsigned int id) return false; } - canBusReady.wait(); + yarp::os::LockGuard lockGuard(canBusReady); if (id == 0) { @@ -159,37 +151,30 @@ bool roboticslab::CanBusHico::canIdDelete(unsigned int id) if (!filterManager->clearFilters(true)) { CD_ERROR("Unable to clear accceptance filters: %s.\n", std::strerror(errno)); - canBusReady.post(); return false; } - canBusReady.post(); return true; } if (!filterManager->hasId(id)) { CD_WARNING("Filter for ID %d not found, doing nothing.\n", id); - canBusReady.post(); return true; } if (!filterManager->eraseId(id)) { CD_ERROR("Could not remove filter: %s.\n", std::strerror(errno)); - canBusReady.post(); return false; } if (!filterManager->isValid()) { CD_WARNING("Hardware limit was hit, not all requested filters are enabled.\n"); - canBusReady.post(); return false; } - canBusReady.post(); - return true; } @@ -205,7 +190,7 @@ bool roboticslab::CanBusHico::canRead(yarp::dev::CanBuffer & msgs, unsigned int *read = 0; - canBusReady.wait(); + yarp::os::LockGuard lockGuard(canBusReady); for (unsigned int i = 0; i < size; i++) { @@ -216,7 +201,6 @@ bool roboticslab::CanBusHico::canRead(yarp::dev::CanBuffer & msgs, unsigned int if (!waitUntilTimeout(READ, &bufferReady)) { CD_ERROR("waitUntilTimeout() failed.\n"); - canBusReady.post(); return false; } @@ -241,7 +225,6 @@ bool roboticslab::CanBusHico::canRead(yarp::dev::CanBuffer & msgs, unsigned int else { CD_ERROR("read() error: %s.\n", std::strerror(errno)); - canBusReady.post(); return false; } } @@ -255,8 +238,6 @@ bool roboticslab::CanBusHico::canRead(yarp::dev::CanBuffer & msgs, unsigned int } } - canBusReady.post(); - return true; } @@ -272,7 +253,7 @@ bool roboticslab::CanBusHico::canWrite(const yarp::dev::CanBuffer & msgs, unsign *sent = 0; - canBusReady.wait(); + yarp::os::LockGuard lockGuard(canBusReady); for (unsigned int i = 0; i < size; i++) { @@ -283,7 +264,6 @@ bool roboticslab::CanBusHico::canWrite(const yarp::dev::CanBuffer & msgs, unsign if (!waitUntilTimeout(WRITE, &bufferReady)) { CD_ERROR("waitUntilTimeout() failed.\n"); - canBusReady.post(); return false; } @@ -307,7 +287,6 @@ bool roboticslab::CanBusHico::canWrite(const yarp::dev::CanBuffer & msgs, unsign else { CD_ERROR("%s.\n", std::strerror(errno)); - canBusReady.post(); return false; } } @@ -321,8 +300,6 @@ bool roboticslab::CanBusHico::canWrite(const yarp::dev::CanBuffer & msgs, unsign } } - canBusReady.post(); - return true; } diff --git a/libraries/YarpPlugins/CanBusPeak/CanBusPeak.hpp b/libraries/YarpPlugins/CanBusPeak/CanBusPeak.hpp index 14f691a3d..8b5e37dfe 100644 --- a/libraries/YarpPlugins/CanBusPeak/CanBusPeak.hpp +++ b/libraries/YarpPlugins/CanBusPeak/CanBusPeak.hpp @@ -7,7 +7,7 @@ #include -#include +#include #include #include @@ -89,7 +89,7 @@ class CanBusPeak : public yarp::dev::DeviceDriver, bool blockingMode; bool allowPermissive; - mutable yarp::os::Semaphore canBusReady; + mutable yarp::os::Mutex canBusReady; std::set activeFilters; }; diff --git a/libraries/YarpPlugins/CanBusPeak/ICanBusImpl.cpp b/libraries/YarpPlugins/CanBusPeak/ICanBusImpl.cpp index a745b66c8..5e0b72fd6 100644 --- a/libraries/YarpPlugins/CanBusPeak/ICanBusImpl.cpp +++ b/libraries/YarpPlugins/CanBusPeak/ICanBusImpl.cpp @@ -5,6 +5,8 @@ #include // std::memset, std::memcpy, std::strerror #include // error codes +#include + #include #include @@ -19,9 +21,9 @@ bool roboticslab::CanBusPeak::canSetBaudRate(unsigned int rate) std::memset(&pfdi, '\0', sizeof(pfdi)); pfdi.nominal.bitrate = rate; - canBusReady.wait(); + canBusReady.lock(); int res = pcanfd_set_init(fileDescriptor, &pfdi); - canBusReady.post(); + canBusReady.unlock(); if (res < 0) { @@ -38,9 +40,9 @@ bool roboticslab::CanBusPeak::canGetBaudRate(unsigned int * rate) { struct pcanfd_init pfdi; - canBusReady.wait(); + canBusReady.lock(); int res = pcanfd_get_init(fileDescriptor, &pfdi); - canBusReady.post(); + canBusReady.unlock(); if (res < 0) { @@ -65,12 +67,11 @@ bool roboticslab::CanBusPeak::canIdAdd(unsigned int id) { CD_DEBUG("(%d)\n", id); - canBusReady.wait(); + yarp::os::LockGuard lockGuard(canBusReady); if (activeFilters.find(id) != activeFilters.end()) { CD_WARNING("Filter for id %d already set.\n", id); - canBusReady.post(); return true; } @@ -86,12 +87,9 @@ bool roboticslab::CanBusPeak::canIdAdd(unsigned int id) { CD_ERROR("pcanfd_set_option() failed (%s).\n", std::strerror(-res)); activeFilters.erase(id); - canBusReady.post(); return false; } - canBusReady.post(); - return true; } @@ -101,7 +99,7 @@ bool roboticslab::CanBusPeak::canIdDelete(unsigned int id) { CD_DEBUG("(%d)\n", id); - canBusReady.wait(); + yarp::os::LockGuard lockGuard(canBusReady); if (id == 0) { @@ -112,19 +110,16 @@ bool roboticslab::CanBusPeak::canIdDelete(unsigned int id) if (res < 0) { CD_ERROR("Unable to clear accceptance filters (%s).\n", std::strerror(-res)); - canBusReady.post(); return false; } activeFilters.clear(); - canBusReady.post(); return true; } if (activeFilters.erase(id) == 0) { CD_WARNING("Filter for id %d missing or already deleted.\n", id); - canBusReady.post(); return true; } @@ -138,12 +133,9 @@ bool roboticslab::CanBusPeak::canIdDelete(unsigned int id) { CD_ERROR("pcanfd_set_option() failed (%s).\n", std::strerror(-res)); activeFilters.insert(id); - canBusReady.post(); return false; } - canBusReady.post(); - return true; } @@ -157,31 +149,31 @@ bool roboticslab::CanBusPeak::canRead(yarp::dev::CanBuffer & msgs, unsigned int return false; } - canBusReady.wait(); + int res; - if (blockingMode && rxTimeoutMs > 0) { - bool bufferReady; + yarp::os::LockGuard lockGuard(canBusReady); - if (!waitUntilTimeout(READ, &bufferReady)) { - canBusReady.post(); - CD_ERROR("waitUntilTimeout() failed.\n"); - return false; - } - - if (!bufferReady) + if (blockingMode && rxTimeoutMs > 0) { - canBusReady.post(); - *read = 0; - return true; + bool bufferReady; + + if (!waitUntilTimeout(READ, &bufferReady)) { + CD_ERROR("waitUntilTimeout() failed.\n"); + return false; + } + + if (!bufferReady) + { + *read = 0; + return true; + } } - } - // Point at first member of an internally defined array of pcandf_msg structs. - struct pcanfd_msg * pfdm = reinterpret_cast(msgs.getPointer()[0]->getPointer()); - int res = pcanfd_recv_msgs_list(fileDescriptor, size, pfdm); - - canBusReady.post(); + // Point at first member of an internally defined array of pcanfd_msg structs. + struct pcanfd_msg * pfdm = reinterpret_cast(msgs.getPointer()[0]->getPointer()); + res = pcanfd_recv_msgs_list(fileDescriptor, size, pfdm); + } if (!blockingMode && res == -EWOULDBLOCK) { @@ -210,39 +202,39 @@ bool roboticslab::CanBusPeak::canWrite(const yarp::dev::CanBuffer & msgs, unsign return false; } - canBusReady.wait(); + int res; - // Point at first member of an internally defined array of pcandf_msg structs. - yarp::dev::CanBuffer & msgs_not_const = const_cast(msgs); - struct pcanfd_msg * pfdm = reinterpret_cast(msgs_not_const.getPointer()[0]->getPointer()); - - for (unsigned int i = 0; i < size; i++) { - pfdm[i].type = PCANFD_TYPE_CAN20_MSG; - pfdm[i].flags = PCANFD_MSG_STD; - } + yarp::os::LockGuard lockGuard(canBusReady); - if (blockingMode && txTimeoutMs > 0) - { - bool bufferReady; + // Point at first member of an internally defined array of pcanfd_msg structs. + yarp::dev::CanBuffer & msgs_not_const = const_cast(msgs); + struct pcanfd_msg * pfdm = reinterpret_cast(msgs_not_const.getPointer()[0]->getPointer()); - if (!waitUntilTimeout(WRITE, &bufferReady)) { - canBusReady.post(); - CD_ERROR("waitUntilTimeout() failed.\n"); - return false; + for (unsigned int i = 0; i < size; i++) + { + pfdm[i].type = PCANFD_TYPE_CAN20_MSG; + pfdm[i].flags = PCANFD_MSG_STD; } - if (!bufferReady) + if (blockingMode && txTimeoutMs > 0) { - canBusReady.post(); - *sent = 0; - return true; + bool bufferReady; + + if (!waitUntilTimeout(WRITE, &bufferReady)) { + CD_ERROR("waitUntilTimeout() failed.\n"); + return false; + } + + if (!bufferReady) + { + *sent = 0; + return true; + } } - } - int res = pcanfd_send_msgs_list(fileDescriptor, size, pfdm); - - canBusReady.post(); + res = pcanfd_send_msgs_list(fileDescriptor, size, pfdm); + } if (!blockingMode && res == -EWOULDBLOCK) { From 2133db176220f5bd1d0c8ebdd35b72cf2b1ea518 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Mon, 5 Aug 2019 13:31:40 +0200 Subject: [PATCH 5/6] Use const signature of ICanBuffer::getPointer https://github.com/robotology/yarp/pull/2015 --- libraries/YarpPlugins/CanBusPeak/ICanBusImpl.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/YarpPlugins/CanBusPeak/ICanBusImpl.cpp b/libraries/YarpPlugins/CanBusPeak/ICanBusImpl.cpp index 5e0b72fd6..6935a0503 100644 --- a/libraries/YarpPlugins/CanBusPeak/ICanBusImpl.cpp +++ b/libraries/YarpPlugins/CanBusPeak/ICanBusImpl.cpp @@ -5,6 +5,7 @@ #include // std::memset, std::memcpy, std::strerror #include // error codes +#include #include #include @@ -208,8 +209,12 @@ bool roboticslab::CanBusPeak::canWrite(const yarp::dev::CanBuffer & msgs, unsign yarp::os::LockGuard lockGuard(canBusReady); // Point at first member of an internally defined array of pcanfd_msg structs. +#if YARP_VERSION_MINOR >= 2 // note: remove #include + struct pcanfd_msg * pfdm = reinterpret_cast(msgs.getPointer()[0]->getPointer()); +#else yarp::dev::CanBuffer & msgs_not_const = const_cast(msgs); struct pcanfd_msg * pfdm = reinterpret_cast(msgs_not_const.getPointer()[0]->getPointer()); +#endif for (unsigned int i = 0; i < size; i++) { From b87cc839f18e46076a1315715aa2b7f6d77c61f6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Mon, 5 Aug 2019 13:49:14 +0200 Subject: [PATCH 6/6] Implement custom factory, preset default flags --- .../YarpPlugins/CanBusPeak/CanBusPeak.hpp | 41 +++++++++++++++++-- .../YarpPlugins/CanBusPeak/ICanBusImpl.cpp | 10 +---- 2 files changed, 40 insertions(+), 11 deletions(-) diff --git a/libraries/YarpPlugins/CanBusPeak/CanBusPeak.hpp b/libraries/YarpPlugins/CanBusPeak/CanBusPeak.hpp index 8b5e37dfe..112f5d8cb 100644 --- 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 +{ +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(&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 + public ImplementPeakCanBufferFactory { public: diff --git a/libraries/YarpPlugins/CanBusPeak/ICanBusImpl.cpp b/libraries/YarpPlugins/CanBusPeak/ICanBusImpl.cpp index 6935a0503..339b4be5e 100644 --- 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 - struct pcanfd_msg * pfdm = reinterpret_cast(msgs.getPointer()[0]->getPointer()); + const struct pcanfd_msg * pfdm = reinterpret_cast(msgs.getPointer()[0]->getPointer()); #else yarp::dev::CanBuffer & msgs_not_const = const_cast(msgs); - struct pcanfd_msg * pfdm = reinterpret_cast(msgs_not_const.getPointer()[0]->getPointer()); + const struct pcanfd_msg * pfdm = reinterpret_cast(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;