diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index b12e433527fce6c419903e208b6f9d218dff2195..49b4503f6273ee7bcf57f66d8bb82f2285ee215a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -35,9 +35,9 @@ ELSE(FTDI_INCLUDE_DIR AND FTDI_LIBRARY) ENDIF(FTDI_INCLUDE_DIR AND FTDI_LIBRARY) # edit the following line to add all the source code files of the library -SET(sources ./comm.cpp ./cqueue.cpp ./commexceptions.cpp ./serial/rs232.cpp ./serial/rs232exceptions.cpp ./sockets/socket.cpp ./sockets/socketclient.cpp ./sockets/socketserver.cpp ./sockets/socketudp.cpp ./sockets/socketexceptions.cpp ./can/can.cpp) +SET(sources ./comm.cpp ./cqueue.cpp ./commexceptions.cpp ./serial/rs232.cpp ./serial/rs232exceptions.cpp ./sockets/socket.cpp ./sockets/socketclient.cpp ./sockets/socketserver.cpp ./sockets/socketudp.cpp ./sockets/socketexceptions.cpp ./can/can.cpp ./can/virtual_can.cpp) # edit the following line to add all the header files of the library -SET(headers ./comm.h ./cqueue.h ./commexceptions.h ./serial/rs232.h ./serial/rs232exceptions.h ./sockets/socket.h ./sockets/socketclient.h ./sockets/socketserver.h ./sockets/socketudp.h ./sockets/socketexceptions.h ./can/can.h) +SET(headers ./comm.h ./cqueue.h ./commexceptions.h ./serial/rs232.h ./serial/rs232exceptions.h ./sockets/socket.h ./sockets/socketclient.h ./sockets/socketserver.h ./sockets/socketudp.h ./sockets/socketexceptions.h ./can/can.h ./can/virtual_can.h) IF(BUILD_FTDI) SET(sources ${sources} ./usb_ftdi/ftdiserver.cpp ./usb_ftdi/ftdimodule.cpp ./usb_ftdi/ftdiexceptions.cpp) diff --git a/src/can/can.cpp b/src/can/can.cpp index ca8d68aefc930793d4f26dfa2c7af8169433d947..292ca74fd01cbdff2d9d04fe3ecd2b6c4eaab603 100755 --- a/src/can/can.cpp +++ b/src/can/can.cpp @@ -40,7 +40,6 @@ void CCAN::hard_open(void *comm_dev) throw CCommException(_HERE_,"impossible to create the socket-CAN",this->get_id()); } addr.can_family = AF_CAN; - std::cout << *can_dev << std::endl; strcpy(ifr.ifr_name, can_dev->c_str()); ioctl(this->can_socket_fd, SIOCGIFINDEX, &ifr); addr.can_ifindex = ifr.ifr_ifindex; diff --git a/src/can/virtual_can.cpp b/src/can/virtual_can.cpp new file mode 100755 index 0000000000000000000000000000000000000000..22e0f9732f8a82944946b3ffd99a23a008b1edaf --- /dev/null +++ b/src/can/virtual_can.cpp @@ -0,0 +1,333 @@ +#include "virtual_can.h" +#include "commexceptions.h" +#include <string.h> +#include <iostream> +#include <math.h> +#include <errno.h> +#include <sys/time.h> + +CVirtualCAN::CVirtualCAN(const std::string &comm_id) : CComm(comm_id) +{ + this->dump_filename=""; + 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"; + this->thread_server->create_thread(this->can_thread_id); + this->thread_server->attach_thread(this->can_thread_id,this->can_thread,this); + // initialize the event atributes + this->event_server=CEventServer::instance(); + this->new_frame_event_id=comm_id + "_new_frame_event_id"; + this->event_server->create_event(this->new_frame_event_id); + this->frame_error_event_id=comm_id + "_frame_error_event_id"; + this->event_server->create_event(this->frame_error_event_id); + this->data_requested_event_id=comm_id + "_data_requested_event_id"; + this->event_server->create_event(this->data_requested_event_id); + this->finish_can_thread_event_id=comm_id + "_finish_can_thread_event_id"; + this->event_server->create_event(this->finish_can_thread_event_id); +} + +void CVirtualCAN::hard_open(void *comm_dev) +{ + std::string *dump_filename=(std::string *)comm_dev; + + this->dump_file.exceptions(std::ifstream::failbit|std::ifstream::eofbit);// enable exceptions + try{ + this->dump_file.open(dump_filename->c_str(),std::ifstream::in);// try to open the file for reading + this->dump_filename=*dump_filename; + }catch(std::ifstream::failure &e){ + throw CCommException(_HERE_,"Impossible to open the desired dump file",this->comm_id); + } +} + +void CVirtualCAN::hard_config(void *config) +{ + // start the can thread + this->thread_server->start_thread(this->can_thread_id); +} + +int CVirtualCAN::hard_read(unsigned char *data, int len) +{ + struct can_frame *frame=(struct can_frame *)data; + char line[1024]; + bool valid_id=true; + unsigned int i; + + do{ + valid_id=true; + // read a line from the file + try{ + this->dump_file.getline(line,1024); + }catch(std::ifstream::failure &e){ + /* close and reopen the file */ + this->dump_file.close(); + this->dump_file.open(this->dump_filename.c_str(),std::ifstream::in); + this->dump_file.getline(line,1024); + } + if((sscanf(line," slcan0 %x [%hhd] %hhx %hhx %hhx %hhx %hhx %hhx %hhx %hhx",&frame->can_id,&frame->can_dlc,&frame->data[0],&frame->data[1],&frame->data[2], + &frame->data[3],&frame->data[4],&frame->data[5],&frame->data[6],&frame->data[7]))!=10) + { + throw CCommException(_HERE_,"Invalid file format.",this->comm_id); + } + // filter the incomming packet + for(i=0;i<this->num_filters;i++) + { + if((frame->can_id&this->rx_filters[i].can_mask)!=this->rx_filters[i].can_id) + valid_id=false; + } + }while(!valid_id); + + return len; +} + +int CVirtualCAN::hard_write(unsigned char *data, int len) +{ + return len; +} + +int CVirtualCAN::hard_get_num_data(void) +{ + return sizeof(struct can_frame);// return the length of a full packet +} + +int CVirtualCAN::hard_wait_comm_event(void) +{ + usleep(1000);// publish can packets at 1KHz + return 1; +} + +void CVirtualCAN::hard_close(void) +{ + this->dump_file.close(); +} + +void *CVirtualCAN::can_thread(void *param) +{ + struct can_frame new_frame; + std::list<std::string> events; + CVirtualCAN *can=(CVirtualCAN *)param; + unsigned int event_id,num; + bool end=false; + + events.push_back(can->finish_can_thread_event_id); + events.push_back(can->get_rx_event_id()); + while(!end) + { + event_id=can->event_server->wait_first(events); + if(event_id==0) + end=true; + else + { + can->can_access.enter(); + // process the incomming data + num=can->get_num_data(); + while(num>=sizeof(struct can_frame))// there is a whole frame in the input buffer + { + can->read((unsigned char *)&new_frame,sizeof(struct can_frame)); + if(new_frame.can_id&CAN_RTR_FLAG) + { + // save the requested can_id + can->req_can_id=new_frame.can_id; + // activate the event + can->event_server->set_event(can->data_requested_event_id); + } + else + { + //puch the new frame into the queue + can->rx_frames.push(new_frame); + // activate the event + can->event_server->set_event(can->new_frame_event_id); + } + num-=sizeof(struct can_frame); + } + can->can_access.exit(); + } + } + + pthread_exit(NULL); +} + +void CVirtualCAN::open(const std::string &dump_filename) +{ + CComm::open((void *)&dump_filename); + this->config(); +} + +std::string CVirtualCAN::get_new_frame_event_id(void) +{ + return this->new_frame_event_id; +} + +std::string CVirtualCAN::get_frame_error_event_id(void) +{ + return this->frame_error_event_id; +} + +std::string CVirtualCAN::get_data_requested_event_id(void) +{ + return this->data_requested_event_id; +} + +unsigned int CVirtualCAN::get_requested_can_id(void) +{ + return this->req_can_id; +} + +unsigned long CVirtualCAN::get_last_rx_timestamp(void) +{ + struct timeval tv; + + gettimeofday (&tv, NULL); + + return tv.tv_sec*1000000+tv.tv_usec; +} + +void CVirtualCAN::write_frame(unsigned int can_id, unsigned char *data, int len) +{ + int num_frames=ceil((double)len/8.0),i,j; + struct can_frame frame; + + // check the identifier format + if((can_id&(~CAN_SFF_MASK))!=0)// it's an extended identifier + can_id=can_id|CAN_EFF_FLAG; + + for(i=0;i<num_frames;i++) + { + // generate all the frames + frame.can_id=can_id; + if((len-8*i)<8) + frame.can_dlc=len-8*i; + else + frame.can_dlc=8; + for(j=0;j<frame.can_dlc;j++) + frame.data[j]=data[i*8+j]; + // send the frames + this->write((unsigned char *)&frame,sizeof(struct can_frame)); + } +} + +void CVirtualCAN::read_frame(unsigned int *can_id, unsigned char *data, int *len) +{ + struct can_frame frame; + int i=0; + + this->can_access.enter(); + if(!this->rx_frames.empty()) + { + frame=this->rx_frames.front(); + this->rx_frames.pop(); + *can_id=frame.can_id; + *len=frame.can_dlc; //COMMENT: Do we need a cast ? frame.can_dlc is __u8 type variable (see /usr/include/linux/can.h) + for(i=0;i<frame.can_dlc;i++) + data[i]=frame.data[i]; + } + else + { + /* handle exceptions */ + this->can_access.exit(); + throw CCommException(_HERE_,"No data available",this->comm_id); + } + this->can_access.exit(); +} + +void CVirtualCAN::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); + if(this->rx_filters!=NULL) + 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++; +} + +void CVirtualCAN::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); + if(this->rx_filters!=NULL) + 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++; +} + +void CVirtualCAN::clear_id_filters(void) +{ + if(this->rx_filters!=NULL) + { + delete this->rx_filters; + this->rx_filters=NULL; + this->num_filters=0; + } +} + +void CVirtualCAN::close(void) +{ + CComm::close(); +} + +CVirtualCAN::~CVirtualCAN() +{ + // close the can device + CComm::close(); + // free all the resources + if(this->finish_can_thread_event_id!="") + { + this->event_server->set_event(this->finish_can_thread_event_id); + this->thread_server->end_thread(this->can_thread_id); + this->thread_server->detach_thread(this->can_thread_id); + this->thread_server->delete_thread(this->can_thread_id); + this->can_thread_id=""; + this->event_server->delete_event(this->finish_can_thread_event_id); + this->finish_can_thread_event_id=""; + } + if(this->new_frame_event_id!="") + { + this->event_server->delete_event(this->new_frame_event_id); + this->new_frame_event_id=""; + } + if(this->frame_error_event_id!="") + { + this->event_server->delete_event(this->frame_error_event_id); + this->frame_error_event_id=""; + } + if(this->data_requested_event_id!="") + { + this->event_server->delete_event(this->data_requested_event_id); + this->data_requested_event_id=""; + } +} + diff --git a/src/can/virtual_can.h b/src/can/virtual_can.h new file mode 100755 index 0000000000000000000000000000000000000000..83283affb55ab9420f4724686d3f6fa37b5422c0 --- /dev/null +++ b/src/can/virtual_can.h @@ -0,0 +1,262 @@ +#ifndef _VIRTUAL_CAN_H +#define _VIRTUAL_CAN_H + +#include "comm.h" +#include "mutex.h" + +#include <sys/types.h> +#include <sys/socket.h> +#include <sys/ioctl.h> +#include <sys/uio.h> +#include <net/if.h> +#include <linux/can.h> +#include <linux/can/bcm.h> +#include <linux/can/raw.h> + +#include "threadserver.h" +#include "eventserver.h" +#include <queue> + +#include <fstream> + +/** + * \brief CAN driver + * + */ + +class CVirtualCAN : protected CComm +{ + private: + // can device identifier + std::string dump_filename; + std::ifstream dump_file; + unsigned int req_can_id; + // thread attributes + CThreadServer *thread_server; + std::string can_thread_id; + // event attributes + CEventServer *event_server; + std::string new_frame_event_id;// public event + 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 + // internal received frame buffer + std::queue<struct can_frame> rx_frames; + // reception can_id filters + struct can_filter *rx_filters; + unsigned int num_filters; + CMutex can_access; + protected: + /** + * \brief Function to actually open the device + * + * This function is called automatically when the base class open() function + * is called. By default, it initializes the socket file descriptor using the + * socket() system call. But, if the object has been created by the + * create_socket() function, this function does nothing. + * + * This class does not need any parameter since the socket is created by + * default using a TCP/IP protocol and the AF_INET socket family. So the + * argument passed to this function is ignored and may be NULL. + * + * This function can throw any CSocketException object exception or the generic + * CCommException class. + * + * \param comm_dev this parameter is ignored in this case and can be set to NULL. + * It is only keeped for backward compatiblity with the CComm + * class. + */ + virtual void hard_open(void *comm_dev=NULL); + /** + * \brief Function to actually configure the device + * + * This function is called automatically when the base class config() function + * is called. In this case this function does nothing, since all the necessay + * parameters are provide to the open() function. However, it is necessary to + * call the config() function to successfully change the internal state of the + * communication device. + * + * The provided parameter in this case can be NULL since no configuration is + * needed. This function can throw any CCommException object exception or else + * any exception class defined by the inherited class. + * + * \param config This parameter is not used since no configuration is required. + * It is only keeped for backward compatiblity with the CComm + * class. + */ + virtual void hard_config(void *config=NULL); + /** + * \brief Function to actually read from the device + * + * This function is automatically called when the new data received event is + * activated. The read() function from the base class gets data from the + * internal queue, so this function is not used. It must try to read the + * ammount of data specified and store it in the data buffer provided without + * blocking. Also, it must return the number of bytes actually read from the + * devicve, since they may be different than the desired value. + * + * In case of any error, this function throws a CSocketException exception. + * + * \param data a reference to the buffer where the received data must be + * copied. The necessary memory for this buffer must be allocated before + * calling this function and have enough size to store all the desired data. + * If this buffer is not initialized, the function throws an exception. + * + * \param len a positive interger that indicates the number of byte to be + * read from the communication device. This value must be at most the length + * of the data buffer provided to the function. + * + * \return an integer with the number of bytes actually read. These number + * coincide with the desired number if there is enough data in the internal + * queue, but it could be smaller if not. + */ + virtual int hard_read(unsigned char *data, int len); + /** + * \brief Hard write function + * + * This function is automatically called when the base class write() function + * is called. It must try to write the desired ammount of data to the communication + * device without blocking. Also it must return the number of bytes actually + * written to the communication device since they may be different from the + * desired value. + * + * In case of any error, this function throws a CSocketException exception. + * + * \param data a reference to the buffer with the data must be send to the + * device. The necessary memory for this buffer must be allocated before + * calling this function and have enough size to store all the desired data. + * If this buffer is not initialized, the function throws an exception. + * + * \param len a positive interger that indicates the number of byte to be + * written to the communication device. This value must be at most the length + * of the data buffer provided to the function. + * + * \return an integer with the number of bytes actually written. These number + * coincide with the desired number if there is enough data in the internal + * queue, but it could be smaller if not. + */ + virtual int hard_write(unsigned char *data, int len); + /** + * \brief Function to actually get the number of bytes availables + * + * This function is called when the new data received event is activated. + * It must get the number of data bytes available from the communication + * device and return its value without blocking. + * + * In case of any error, this function throws a CSocketException exception. + * + * \return an integer with the number of bytes available in the receive queue. + * This value can be 0 if there is no data available, but there is ni upper + * limit in its value. + */ + virtual int hard_get_num_data(void); + /** + * \brief Function to actually wait for a given communication event + * + * This function is called in the internal communciation thread to wait for + * any event on the communuication device. It must check for any event on the + * device (reception, end of transmission or error) and return the + * corresponding identifier. When an event is activated, this function must + * return to allow the base class to handle it, and the it is called again + * to wait for the next event. + * + * This function can throw any CCommException object exception or else any + * exception class defined by the inherited class. + * + * \return -1 if there has been any error or else the identifier of the + * communciation event: + * + * - 1 for the new data received event + * - 2 for the error event + */ + virtual int hard_wait_comm_event(void); + /** + * \brief Hard close function + * + * This function is called when the base class close() funciton is called. It + * must free the device handle initialized by the open() function and also free + * any other resource allocated by the inherited class. + * + * This function can throw any CCommException object exception or else any + * exception class defined by the inherited class. + */ + virtual void hard_close(void); + /** + * \brief + */ + static void *can_thread(void *param); + public: + /** + * \brief default constructor + * + * This constructor creates a new CSocket object. It does not open a physical + * socket, it only allocates the necessary resources to use it. + * + * \param comm_id A null terminated string which identifies the + * communications device. This string is used to created a + * unique identifier for all the threads and events of the + * class. + */ + CVirtualCAN(const std::string &comm_id); + /** + * \brief + */ + void open(const std::string &dump_filename); + /** + * \brief + */ + std::string get_new_frame_event_id(void); + /** + * \brief + */ + std::string get_frame_error_event_id(void); + /** + * \brief + */ + std::string get_data_requested_event_id(void); + /** + * \brief + */ + unsigned int get_requested_can_id(void); + /** + * \brief + */ + unsigned long get_last_rx_timestamp(void); + /** + * \brief + */ + void write_frame(unsigned int can_id, unsigned char *data, int len); + /** + * \brief + */ + void read_frame(unsigned int *can_id, unsigned char *data, int *len); + /** + * \brief + */ + 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 + */ + void clear_id_filters(void); + /** + * \brief + */ + void close(void); + /** + * \brief Destructor + * + * This destructor does nothing. The base class destructor is the one in + * charge of freeing all the allocated resources. + */ + virtual ~CVirtualCAN(); +}; + +#endif diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 0f50c9af54d2a55148ed1fa5894f88cfa03af4bb..6829ae5e74bb0d2427f8a417b72b8be3d2637040 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -16,6 +16,12 @@ ADD_EXECUTABLE(test_can_rx test_can_rx.cpp) # edit the following line to add the necessary libraries TARGET_LINK_LIBRARIES(test_can_rx ${IRIUTILS_LIBRARY} comm pthread) +# edit the following line to add the source code for the example and the name of the executable +ADD_EXECUTABLE(test_virtual_can_rx test_virtual_can_rx.cpp) + +# edit the following line to add the necessary libraries +TARGET_LINK_LIBRARIES(test_virtual_can_rx ${IRIUTILS_LIBRARY} comm pthread) + # edit the following line to add the source code for the example and the name of the executable ADD_EXECUTABLE(test_candump test_candump.cpp) diff --git a/src/examples/test_virtual_can_rx.cpp b/src/examples/test_virtual_can_rx.cpp new file mode 100755 index 0000000000000000000000000000000000000000..cb0943c2c05f3ceced3b428b6c959bba6d1e31d1 --- /dev/null +++ b/src/examples/test_virtual_can_rx.cpp @@ -0,0 +1,47 @@ +#include "eventserver.h" +#include "threadserver.h" +#include "commexceptions.h" +#include "virtual_can.h" +#include <stdio.h> +#include <unistd.h> +#include <string> +#include <iostream> + +#include <linux/can/raw.h> +#include <linux/can.h> + +const std::string dump_file="/home/shernand/Work/pas/Sensors/radar/radar_308_idiada/candumpobjects.txt"; + +/** + * \example test_can.cpp + * + */ +int main(int argc,char *argv[]) +{ + CEventServer *event_server=CEventServer::instance(); + std::list<std::string> events; + CVirtualCAN can_port("can_port"); + unsigned char data[8]; + unsigned int can_id; + int i=0,len; + + events.push_back(can_port.get_new_frame_event_id()); + try{ + can_port.open(dump_file); + can_port.add_id_filter(0x010,0x0F0,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; + std::cout << "data: "; + for(i=0;i<len;i++) + std::cout << "0x" << std::hex << (int)data[i] << ","; + std::cout << std::endl; + } + }catch(CCommException &e){ + std::cout << e.what() << std::endl; + } +}