diff --git a/include/can/can.h b/include/can/can.h
index 57251edcbbe8cead6f980e9f6f8cb78c4090e895..b0dcd4e689979079ec4d38db4b62297a9e7c880c 100755
--- a/include/can/can.h
+++ b/include/can/can.h
@@ -240,11 +240,7 @@ class CCAN : protected CComm
     /** 
      * \brief
      */
-    unsigned int get_num_data(unsigned int can_id);
-    /**
-     * \brief
-     */
-    void request_frame(unsigned int can_id, int len);
+    unsigned int get_num_data(unsigned int can_id=-1);
     /** 
      * \brief
      */
diff --git a/src/can/can.cpp b/src/can/can.cpp
index 02a3402150e545dffbc64b0f9246bd301191445e..5c1ebf5c39180dc0ed640458c780b6f69aab4b31 100755
--- a/src/can/can.cpp
+++ b/src/can/can.cpp
@@ -163,7 +163,7 @@ void *CCAN::can_thread(void *param)
   std::list<std::string> events;
   CCAN *can=(CCAN *)param;
   unsigned int event_id,num;
-  bool end=false;
+  bool end=false,assigned;
 
   events.push_back(can->finish_can_thread_event_id);
   events.push_back(can->get_rx_event_id());
@@ -190,16 +190,28 @@ void *CCAN::can_thread(void *param)
         else
         {
           //push the new frame into the queue
-          can->rx_frames.push(new_frame);
-          can->can_data_map[new_frame.can_id]->write(new_frame.data,new_frame.can_dlc);
-          //for (unsigned int i=0; i<new_frame.can_dlc; i++)
-          //  can->can_data_map[new_frame.can_id].push_back(new_frame.data[i]);      
-          // activate the event
-          can->event_server->set_event(can->new_frame_event_id);
-          can->event_server->set_event((std::to_string(new_frame.can_id) + "_new_data_event_id"));
+          assigned=false;
+          for(int i=0;i<can->num_filters;i++)
+          {
+            if(can->can_data_map.find(new_frame.can_id&can->rx_filters[i].can_mask)!=can->can_data_map.end())
+            {
+              can->can_data_map[new_frame.can_id&can->rx_filters[i].can_mask]->write(new_frame.data,new_frame.can_dlc);
+              //for (unsigned int i=0; i<new_frame.can_dlc; i++)
+              //  can->can_data_map[new_frame.can_id].push_back(new_frame.data[i]);      
+              // activate the event
+              if(!can->event_server->event_is_set(std::to_string(new_frame.can_id) + "_new_data_event_id"))
+                can->event_server->set_event((std::to_string(new_frame.can_id) + "_new_data_event_id"));
+              assigned=true;
+              break;
+            }
+          }
+          if(!assigned)
+          {
+            can->rx_frames.push(new_frame);
+            can->event_server->set_event(can->new_frame_event_id);
+          }
         }
-        num=((CComm *)can)->get_num_data();
-        //num-=sizeof(struct can_frame);
+        num-=sizeof(struct can_frame);
       }
       can->can_access.exit();
     }
@@ -280,16 +292,43 @@ void CCAN::write(unsigned int can_id, unsigned char *data, int len)
 
 void CCAN::read(unsigned int can_id, unsigned char *data, int len)
 {
+  int num=0;
+  struct can_frame frame;
+
   this->can_access.enter();
-  if(!this->can_data_map[can_id]->is_empty())
+  if(can_id!=(unsigned int)-1)
   {
-    this->can_data_map[can_id]->read(data,len);
+    if(this->can_data_map.find(can_id)==this->can_data_map.end())
+    {
+      this->can_access.exit();
+      throw CCommException(_HERE_,"Invalid CAN id",this->comm_id);
+    }
+    if(!this->can_data_map[can_id]->is_empty())
+    {
+      this->can_data_map[can_id]->read(data,len);
+    }
+    else
+    {
+      /* handle exceptions */
+      this->can_access.exit();    
+      throw CCommException(_HERE_,"No data available",this->comm_id);
+    }
   }
   else
   {
-    /* handle exceptions */
-    this->can_access.exit();    
-    throw CCommException(_HERE_,"No data available",this->comm_id);
+    while(len>0)
+    {
+      if(this->rx_frames.size()==0)
+      {
+        this->can_access.exit();    
+        throw CCommException(_HERE_,"No data available",this->comm_id);
+      }
+      frame=this->rx_frames.front();
+      this->rx_frames.pop();
+      memcpy(&data[num*sizeof(struct can_frame)],&frame,sizeof(struct can_frame));
+      num++;
+      len-=sizeof(struct can_frame);
+    }
   }
   this->can_access.exit();
 }
@@ -298,10 +337,15 @@ unsigned int CCAN::get_num_data(unsigned int can_id)
 {
   unsigned int num_data;
 
-  if(this->can_data_map.find(can_id)==this->can_data_map.end())
-    throw CCommException(_HERE_,"Invalid can ID",this->comm_id);
+  if(can_id!=(unsigned int)-1)
+  {
+    if(this->can_data_map.find(can_id)==this->can_data_map.end())
+      throw CCommException(_HERE_,"Invalid can ID",this->comm_id);
  
-  num_data = this->can_data_map[can_id]->get_num_data();
+    num_data = this->can_data_map[can_id]->get_num_data();
+  }
+  else
+    num_data = this->rx_frames.size()*sizeof(struct can_frame);
   
   return num_data;
 }