Arbitration lost errors are now configurable: either report as error …

…or ignore and throw warning
ros-industrial · Feb 6, 2018 · b996bc9 · b996bc9
1 parent cd89f5f
commit b996bc9
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 9 deletions.
diff --git a/socketcan_bridge/src/socketcan_bridge_node.cpp b/socketcan_bridge/src/socketcan_bridge_node.cpp
@@ -38,10 +38,13 @@ int main(int argc, char *argv[])
 
   ros::NodeHandle nh(""), nh_param("~");
 
+  bool lostArbIsError;
   std::string can_device;
   nh_param.param<std::string>("can_device", can_device, "can0");
+  nh_param.param<bool>("lost_arbitration_is_error", lostArbIsError, true);
 
   boost::shared_ptr<can::ThreadedSocketCANInterface> driver = boost::make_shared<can::ThreadedSocketCANInterface> ();
+  driver->arbitrationLostIsError_ = lostArbIsError;
 
   if (!driver->init(can_device, 0))  // initialize device at can_device, 0 for no loopback.
   {

diff --git a/socketcan_bridge/src/socketcan_to_topic_node.cpp b/socketcan_bridge/src/socketcan_to_topic_node.cpp
@@ -39,10 +39,13 @@ int main(int argc, char *argv[])
 
   ros::NodeHandle nh(""), nh_param("~");
 
+  bool lostArbIsError;
   std::string can_device;
   nh_param.param<std::string>("can_device", can_device, "can0");
+  nh_param.param<bool>("lost_arbitration_is_error", lostArbIsError, true);
 
   boost::shared_ptr<can::ThreadedSocketCANInterface> driver = boost::make_shared<can::ThreadedSocketCANInterface> ();
+  driver->arbitrationLostIsError_ = lostArbIsError;
 
   if (!driver->init(can_device, 0))  // initialize device at can_device, 0 for no loopback.
   {

diff --git a/socketcan_bridge/src/topic_to_socketcan_node.cpp b/socketcan_bridge/src/topic_to_socketcan_node.cpp
@@ -39,10 +39,13 @@ int main(int argc, char *argv[])
 
   ros::NodeHandle nh(""), nh_param("~");
 
+  bool lostArbIsError;
   std::string can_device;
   nh_param.param<std::string>("can_device", can_device, "can0");
+  nh_param.param<bool>("lost_arbitration_is_error", lostArbIsError, true);
 
   boost::shared_ptr<can::ThreadedSocketCANInterface> driver = boost::make_shared<can::ThreadedSocketCANInterface> ();
+  driver->arbitrationLostIsError_ = lostArbIsError;
 
   if (!driver->init(can_device, 0))  // initialize device at can_device, 0 for no loopback.
   {

diff --git a/socketcan_interface/include/socketcan_interface/socketcan.h b/socketcan_interface/include/socketcan_interface/socketcan.h
@@ -20,11 +20,12 @@ namespace can {
 class SocketCANInterface : public AsioDriver<boost::asio::posix::stream_descriptor> {
     bool loopback_;
     int sc_;
-public:    
+public:
+    bool arbitrationLostIsError_;
     SocketCANInterface()
-    : loopback_(false), sc_(-1)
+    : loopback_(false), sc_(-1), arbitrationLostIsError_(true)
     {}
-    
+
     virtual bool doesLoopBack() const{
         return loopback_;
     }
@@ -53,7 +54,7 @@ class SocketCANInterface : public AsioDriver<boost::asio::posix::stream_descript
             }
             can_err_mask_t err_mask =
                 ( CAN_ERR_TX_TIMEOUT   /* TX timeout (by netdevice driver) */
-                //CAN_ERR_LOSTARB      /* lost arbitration    / data[0]  (Not a fatal error)  */
+                | CAN_ERR_LOSTARB      /* lost arbitration    / data[0]    */
                 | CAN_ERR_CRTL         /* controller problems / data[1]    */
                 | CAN_ERR_PROT         /* protocol violations / data[2..3] */
                 | CAN_ERR_TRX          /* transceiver status  / data[4]    */
@@ -160,7 +161,6 @@ class SocketCANInterface : public AsioDriver<boost::asio::posix::stream_descript
 protected:
     std::string device_;
     can_frame frame_;
-
     virtual void triggerReadSome(){
         boost::mutex::scoped_lock lock(send_mutex_);
         socket_.async_read_some(boost::asio::buffer(&frame_, sizeof(frame_)), boost::bind( &SocketCANInterface::readFrame,this, boost::asio::placeholders::error));
@@ -200,10 +200,14 @@ class SocketCANInterface : public AsioDriver<boost::asio::posix::stream_descript
                 input_.id = frame_.can_id & CAN_EFF_MASK;
                 input_.is_error = 1;
 
-                LOG("error: " << input_.id);
-                setInternalError(input_.id);
-                setNotReady();
-
+                bool errorIsArbitLost = (frame_.can_id & CAN_ERR_LOSTARB);
+                if(!errorIsArbitLost || (errorIsArbitLost && arbitrationLostIsError_)){
+                    LOG("error: " << input_.id);
+                    setInternalError(input_.id);
+                    setNotReady();
+                }else{
+                    LOG("warning: " << input_.id << ", arbitration lost but ignored");
+                }
             }else{
                 input_.is_extended = (frame_.can_id & CAN_EFF_FLAG) ? 1 :0;
                 input_.id = frame_.can_id & (input_.is_extended ? CAN_EFF_MASK : CAN_SFF_MASK);