diff --git a/src/can/can.cpp b/src/can/can.cpp
index 920f70f76e0ac1628ea19db9c9ebd09b13a9d417..5755a5cba7442762889487932483113a333972d2 100755
--- a/src/can/can.cpp
+++ b/src/can/can.cpp
@@ -9,6 +9,8 @@ CCAN::CCAN(const std::string &comm_id) : CComm(comm_id)
{
this->can_socket_fd=-1;
this->req_can_id=-1;
+ this->rx_filters=NULL;
+ this->num_filters=0;
// initialize the thread attributes
this->thread_server=CThreadServer::instance();
this->can_thread_id=comm_id + "_can_thread";
@@ -254,6 +256,19 @@ unsigned int CCAN::get_requested_can_id(void)
return this->req_can_id;
}
+unsigned long CCAN::get_last_rx_timestamp(void)
+{
+ struct timeval tv;
+
+ if(ioctl(this->can_socket_fd,SIOCGSTAMP,&tv)==-1)
+ {
+ /* handle exceptions */
+ throw CCommException(_HERE_,"Impossible to get the last time stamp",this->comm_id);
+ }
+
+ return tv.tv_sec*1000000+tv.tv_usec;
+}
+
void CCAN::write_frame(unsigned int can_id, unsigned char *data, int len)
{
int num_frames=ceil((double)len/8.0),i,j;
@@ -300,9 +315,64 @@ void CCAN::read_frame(unsigned int *can_id, unsigned char *data, int *len)
this->can_access.exit();
}
-void CCAN::add_id_filter(unsigned short int start_id, unsigned short int end_id)
+void CCAN::add_id_filter(unsigned short int can_id,bool invert)
{
+ struct can_filter *new_filters;
+ new_filters=new struct can_filter[this->num_filters+1];
+ memcpy(new_filters,this->rx_filters,sizeof(struct can_filter)*this->num_filters);
+ delete[] this->rx_filters;
+ this->rx_filters=new_filters;
+ // update the new filter
+ if((can_id&(~CAN_SFF_MASK))!=0)// it's an extended identifier
+ {
+ this->rx_filters[this->num_filters].can_id=can_id|CAN_EFF_FLAG;
+ this->rx_filters[this->num_filters].can_mask=CAN_EFF_MASK;
+ }
+ else
+ {
+ this->rx_filters[this->num_filters].can_id=can_id;
+ this->rx_filters[this->num_filters].can_mask=CAN_SFF_MASK;
+ }
+ if(invert)
+ this->rx_filters[this->num_filters].can_id|=CAN_INV_FILTER;
+ // set up the new filters
+ this->num_filters++;
+ if(setsockopt(this->can_socket_fd,SOL_CAN_RAW,CAN_RAW_FILTER,this->rx_filters,sizeof(struct can_filter)*this->num_filters)==-1)
+ {
+ /* handle exceptions */
+ throw CCommException(_HERE_,"Error while setting up the filters",this->comm_id);
+ }
+}
+
+void CCAN::add_id_filter(unsigned short int can_id,unsigned short int mask,bool invert)
+{
+ struct can_filter *new_filters;
+
+ new_filters=new struct can_filter[this->num_filters+1];
+ memcpy(new_filters,this->rx_filters,sizeof(struct can_filter)*this->num_filters);
+ delete[] this->rx_filters;
+ this->rx_filters=new_filters;
+ // update the new filter
+ if((can_id&(~CAN_SFF_MASK))!=0)// it's an extended identifier
+ {
+ this->rx_filters[this->num_filters].can_id=can_id|CAN_EFF_FLAG;
+ this->rx_filters[this->num_filters].can_mask=mask;
+ }
+ else
+ {
+ this->rx_filters[this->num_filters].can_id=can_id;
+ this->rx_filters[this->num_filters].can_mask=mask;
+ }
+ if(invert)
+ this->rx_filters[this->num_filters].can_id|=CAN_INV_FILTER;
+ // set up the new filters
+ this->num_filters++;
+ if(setsockopt(this->can_socket_fd,SOL_CAN_RAW,CAN_RAW_FILTER,this->rx_filters,sizeof(struct can_filter)*this->num_filters)==-1)
+ {
+ /* handle exceptions */
+ throw CCommException(_HERE_,"Error while setting up the filters",this->comm_id);
+ }
}
void CCAN::clear_id_filters(void)
diff --git a/src/can/can.h b/src/can/can.h
index 6cbd314c51c12b6f62799d4e5dbf4ca1f48b349f..b695ae44825f33698e53b36cea0cda32d78c8500 100755
--- a/src/can/can.h
+++ b/src/can/can.h
@@ -11,6 +11,7 @@
#include <net/if.h>
#include <linux/can.h>
#include <linux/can/bcm.h>
+#include <linux/can/raw.h>
#include "threadserver.h"
#include "eventserver.h"
@@ -36,8 +37,11 @@ class CCAN : protected CComm
std::string frame_error_event_id;// public event
std::string data_requested_event_id;// public event
std::string finish_can_thread_event_id;// private event
- // intranal received frame buffer
+ // internal received frame buffer
std::queue<struct can_frame> rx_frames;
+ // reception can_id filters
+ struct can_filter *rx_filters;
+ int num_filters;
CMutex can_access;
protected:
/**
@@ -211,6 +215,10 @@ class CCAN : protected CComm
* \brief
*/
unsigned int get_requested_can_id(void);
+ /**
+ * \brief
+ */
+ unsigned long get_last_rx_timestamp(void);
/**
* \brief
*/
@@ -222,7 +230,15 @@ class CCAN : protected CComm
/**
* \brief
*/
- void add_id_filter(unsigned short int start_id, unsigned short int end_id=-1);
+ void request_frame(unsigned int can_id, int len);
+ /**
+ * \brief
+ */
+ void add_id_filter(unsigned short int can_id,bool invert);
+ /**
+ * \brief
+ */
+ void add_id_filter(unsigned short int can_id, unsigned short int mask,bool invert);
/**
* \brief
*/
diff --git a/src/examples/test_can.cpp b/src/examples/test_can.cpp
index 9fdb13c795704c37778843dc87251e7983eba67e..75e735682e48f1615296913482e314007c5bd01e 100755
--- a/src/examples/test_can.cpp
+++ b/src/examples/test_can.cpp
@@ -34,9 +34,9 @@ int main(int argc,char *argv[])
data[7]=0x08;
data[8]=0x09;
data[9]=0x0A;
- for(i=0;i<5;i++)
+ for(i=0;i<10;i++)
{
- can_port.write_frame(0x200,data,10);
+ can_port.write_frame(0x1FB+i,data,10);
sleep(1);
}
}catch(CCommException &e){
diff --git a/src/examples/test_can_rx.cpp b/src/examples/test_can_rx.cpp
index ba541bcf1adb917397456f1280e39e24ac8ae463..b067f5fc618d6c2af5f290d643cc45e308111fdf 100755
--- a/src/examples/test_can_rx.cpp
+++ b/src/examples/test_can_rx.cpp
@@ -28,9 +28,11 @@ int main(int argc,char *argv[])
events.push_back(can_port.get_new_frame_event_id());
try{
can_port.open(can_dev);
+ can_port.add_id_filter(0x200,0xFF0,false);
while(1)
{
event_server->wait_all(events);
+ std::cout << "[" << std::dec << can_port.get_last_rx_timestamp() << "]" << std::endl;
can_port.read_frame(&can_id,data,&len);
std::cout << "can id: 0x" << std::hex << can_id << std::endl;
std::cout << "length: " << len << std::endl;