Improvements to handle arbitrary ID's. Each filtered Id has its own data...

Improvements to handle arbitrary ID's. Each filtered Id has its own data queue, and all the others are handled by a common data queue.

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
      */

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