Skip to content

Commit

Permalink
Merge branch 'can-ifaces' into apocanlypse
Browse files Browse the repository at this point in the history
  • Loading branch information
@PeterBowman
PeterBowman committed Aug 5, 2019
2 parents 1d062a6 + b87cc83 commit 8130c70
Show file tree
Hide file tree
Showing 10 changed files with 150 additions and 154 deletions.
7 changes: 7 additions & 0 deletions libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{

Expand Down Expand Up @@ -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);

Expand Down
4 changes: 3 additions & 1 deletion libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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();
Expand Down
38 changes: 20 additions & 18 deletions libraries/YarpPlugins/CanBusControlboard/ThreadImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down
6 changes: 3 additions & 3 deletions libraries/YarpPlugins/CanBusFake/FakeCanMessage.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,14 +72,14 @@ unsigned char * roboticslab::FakeCanMessage::getData()

unsigned char * roboticslab::FakeCanMessage::getPointer()
{
return (unsigned char *) message;
return reinterpret_cast<unsigned char *>(message);
}

// -----------------------------------------------------------------------------

const unsigned char * roboticslab::FakeCanMessage::getPointer() const
{
return (const unsigned char *) message;
return reinterpret_cast<const unsigned char *>(message);
}

// -----------------------------------------------------------------------------
Expand All @@ -88,7 +88,7 @@ void roboticslab::FakeCanMessage::setBuffer(unsigned char * buf)
{
if (buf != 0)
{
message = (struct fake_can_msg *) buf;
message = reinterpret_cast<struct fake_can_msg *>(buf);
}
}

Expand Down
4 changes: 2 additions & 2 deletions libraries/YarpPlugins/CanBusHico/CanBusHico.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
#include <utility>

#include <yarp/os/Bottle.h>
#include <yarp/os/Semaphore.h>
#include <yarp/os/Mutex.h>

#include <yarp/dev/DeviceDriver.h>
#include <yarp/dev/CanBusInterface.h>
Expand Down Expand Up @@ -133,7 +133,7 @@ class CanBusHico : public yarp::dev::DeviceDriver,
bool blockingMode;
bool allowPermissive;

yarp::os::Semaphore canBusReady;
yarp::os::Mutex canBusReady;

std::pair<bool, unsigned int> bitrateState;

Expand Down
6 changes: 3 additions & 3 deletions libraries/YarpPlugins/CanBusHico/HicoCanMessage.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,14 +72,14 @@ unsigned char * roboticslab::HicoCanMessage::getData()

unsigned char * roboticslab::HicoCanMessage::getPointer()
{
return (unsigned char *) message;
return reinterpret_cast<unsigned char *>(message);
}

// -----------------------------------------------------------------------------

const unsigned char * roboticslab::HicoCanMessage::getPointer() const
{
return (const unsigned char *) message;
return reinterpret_cast<const unsigned char *>(message);
}

// -----------------------------------------------------------------------------
Expand All @@ -88,7 +88,7 @@ void roboticslab::HicoCanMessage::setBuffer(unsigned char * buf)
{
if (buf != 0)
{
message = (struct can_msg *) buf;
message = reinterpret_cast<struct can_msg *>(buf);
}
}

Expand Down
41 changes: 9 additions & 32 deletions libraries/YarpPlugins/CanBusHico/ICanBusImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,8 @@

#include <string>

#include <yarp/os/LockGuard.h>

#include <ColorDebug.h>

// -----------------------------------------------------------------------------
Expand All @@ -26,36 +28,31 @@ 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;
}
}

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;
}

Expand All @@ -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)
{
Expand Down Expand Up @@ -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;
}

Expand All @@ -150,7 +142,7 @@ bool roboticslab::CanBusHico::canIdDelete(unsigned int id)
return false;
}

canBusReady.wait();
yarp::os::LockGuard lockGuard(canBusReady);

if (id == 0)
{
Expand All @@ -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;
}

Expand All @@ -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++)
{
Expand All @@ -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;
}

Expand All @@ -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;
}
}
Expand All @@ -255,8 +238,6 @@ bool roboticslab::CanBusHico::canRead(yarp::dev::CanBuffer & msgs, unsigned int
}
}

canBusReady.post();

return true;
}

Expand All @@ -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++)
{
Expand All @@ -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;
}

Expand All @@ -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;
}
}
Expand All @@ -321,8 +300,6 @@ bool roboticslab::CanBusHico::canWrite(const yarp::dev::CanBuffer & msgs, unsign
}
}

canBusReady.post();

return true;
}

Expand Down
Loading

0 comments on commit 8130c70

Please sign in to comment.