diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 4358a0628181ac7372db7a43ba58268f703576ed..ab896393d923110ab91b576c873b128314f1dd71 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -33,9 +33,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 ./commexceptions.cpp ./serial/rs232.cpp ./serial/rs232exceptions.cpp ./sockets/socket.cpp ./sockets/socketclient.cpp ./sockets/socketserver.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/socketexceptions.cpp ./can/can.cpp)
# edit the following line to add all the header files of the library
-SET(headers ./comm.h ./commexceptions.h ./serial/rs232.h ./serial/rs232exceptions.h ./sockets/socket.h ./sockets/socketclient.h ./sockets/socketserver.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/socketexceptions.h ./can/can.h)
IF(BUILD_FTDI)
SET(sources ${sources} ./usb_ftdi/ftdiserver.cpp ./usb_ftdi/ftdimodule.cpp ./usb_ftdi/ftdiexceptions.cpp)
diff --git a/src/comm.cpp b/src/comm.cpp
index 643753534fdb643b299f1acff51f2f0aecf3282e..51c8dc83035b1bf0eaee39c972151db8de02f792 100644
--- a/src/comm.cpp
+++ b/src/comm.cpp
@@ -105,8 +105,7 @@ void CComm::config(void *config)
int CComm::read(unsigned char *data,int len)
{
- int num_available=0;
- int i=0;
+ int num_read=0;
if(data==NULL)
{
@@ -118,17 +117,12 @@ int CComm::read(unsigned char *data,int len)
if(this->state==configured || this->state==sending)
{
this->access_comm.enter();
- num_available=this->receive_queue.size();
- if(len>num_available)
+ num_read=this->receive_queue.read(data,len);
+ if(len>num_read)
{
- len=num_available;
+ len=num_read;
this->event_server->reset_event(this->rx_event_id);
}
- for(i=0;i<len;i++)
- {
- data[i]=*this->receive_queue.begin();
- this->receive_queue.pop_front();
- }
this->access_comm.exit();
return len;
}
@@ -186,7 +180,7 @@ unsigned int CComm::get_num_data(void)
if(this->state==configured || this->state==sending)
{
this->access_comm.enter();
- num=this->receive_queue.size();
+ num=this->receive_queue.get_num_data();
this->access_comm.exit();
}
@@ -230,7 +224,7 @@ void CComm::close(void)
throw;
}
/* flush the data queues */
- this->receive_queue.erase(this->receive_queue.begin(),this->receive_queue.end());
+ this->receive_queue.flush();
/* change the current state */
this->state=created;
this->access_comm.exit();
@@ -271,7 +265,7 @@ void *CComm::comm_thread(void *param)
void CComm::on_receive(void)
{
unsigned char *data=NULL;
- int num=0,num_read=0,i=0;
+ int num=0,num_read=0;
if((num=this->hard_get_num_data())==-1)
{
@@ -295,8 +289,7 @@ void CComm::on_receive(void)
}
else
{
- for(i=0;i<num;i++)
- this->receive_queue.push_back(data[i]);
+ this->receive_queue.write(data,num);
if(!this->event_server->event_is_set(this->rx_event_id))
{
this->event_server->set_event(this->rx_event_id);
diff --git a/src/comm.h b/src/comm.h
index 27919ea21ab0c9262bb31f44d478aff242c2d047..6346018d761aa04158d22ab54442909672e3a3cc 100644
--- a/src/comm.h
+++ b/src/comm.h
@@ -5,6 +5,7 @@
#include "eventserver.h"
#include "threadserver.h"
#include "mutex.h"
+#include "cqueue.h"
typedef enum {created,configured,opened,sending} comm_states;
@@ -135,7 +136,7 @@ class CComm
* function get_num_data() returns the number of bytes in this queue. By default,
* this queue is empty.
*/
- std::list<unsigned char> receive_queue;
+ CQueue receive_queue;
/**
* \brief current state of the communication device
*
diff --git a/src/cqueue.cpp b/src/cqueue.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..10b83eb05789c2ffaebe3340efb93262c809869b
--- /dev/null
+++ b/src/cqueue.cpp
@@ -0,0 +1,118 @@
+#include "cqueue.h"
+
+CQueue::CQueue()
+{
+ this->size=QUEUE_DEFAULT_SIZE;
+ this->data=new unsigned char[this->size];
+ this->num_data=0;
+ this->read_pointer=0;
+ this->write_pointer=0;
+}
+
+CQueue::CQueue(int size)
+{
+ this->size=size;
+ this->data=new unsigned char[this->size];
+ this->num_data=0;
+ this->read_pointer=0;
+ this->write_pointer=0;
+}
+
+void CQueue::resize(int new_size)
+{
+}
+
+int CQueue::read(unsigned char *data,int len)
+{
+ int num_data_to_end=0;
+
+ this->queue_access.enter();
+ if(len>this->num_data)
+ len=this->num_data;
+ num_data_to_end=this->size-this->read_pointer;
+ if(num_data_to_end<len)
+ {
+ memcpy(data,&this->data[read_pointer],num_data_to_end);
+ memcpy(&data[num_data_to_end],this->data,len-num_data_to_end);
+ this->read_pointer=len-num_data_to_end;
+ this->num_data-=len;
+ }
+ else
+ {
+ memcpy(data,&this->data[this->read_pointer],len);
+ this->read_pointer+=len;
+ this->num_data-=len;
+ }
+ this->queue_access.exit();
+
+ return len;
+}
+
+int CQueue::write(unsigned char *data,int len)
+{
+ int num_data_to_end=0;
+
+ this->queue_access.enter();
+ if(len>(this->size-this->num_data))
+ len=this->size-this->num_data;
+ num_data_to_end=this->size-this->write_pointer;
+ if(num_data_to_end<len)
+ {
+ memcpy(&this->data[this->write_pointer],data,num_data_to_end);
+ memcpy(this->data,&data[num_data_to_end],len-num_data_to_end);
+ this->write_pointer=len-num_data_to_end;
+ this->num_data+=len;
+ }
+ else
+ {
+ memcpy(&this->data[this->write_pointer],data,len);
+ this->write_pointer+=len;
+ this->num_data+=len;
+ }
+ this->queue_access.exit();
+
+ return len;
+}
+
+int CQueue::get_num_data(void)
+{
+ return this->num_data;
+}
+
+bool CQueue::is_empty(void)
+{
+ if(this->num_data==0)
+ return true;
+ else
+ return false;
+}
+
+bool CQueue::is_full(void)
+{
+ if(this->num_data==this->size)
+ return true;
+ else
+ return false;
+}
+
+void CQueue::flush(void)
+{
+ this->queue_access.enter();
+ this->read_pointer=0;
+ this->write_pointer=0;
+ this->num_data=0;
+ this->queue_access.exit();
+}
+
+CQueue::~CQueue()
+{
+ if(this->data!=NULL)
+ {
+ delete[] this->data;
+ this->data=NULL;
+ }
+ this->num_data=0;
+ this->read_pointer=0;
+ this->write_pointer=0;
+}
+
diff --git a/src/cqueue.h b/src/cqueue.h
new file mode 100755
index 0000000000000000000000000000000000000000..02ce135d325e768610f089bb15b31a79687f7502
--- /dev/null
+++ b/src/cqueue.h
@@ -0,0 +1,31 @@
+#ifndef _CQUEUE_H
+#define _CQUEUE_H
+
+#include <string.h>
+#include "mutex.h"
+
+#define QUEUE_DEFAULT_SIZE 1024
+
+class CQueue
+{
+ private:
+ int size;
+ int num_data;
+ int read_pointer;
+ int write_pointer;
+ unsigned char *data;
+ CMutex queue_access;
+ public:
+ CQueue();
+ CQueue(int size);
+ void resize(int new_size);
+ int read(unsigned char *data,int len);
+ int write(unsigned char *data,int len);
+ int get_num_data(void);
+ bool is_empty(void);
+ bool is_full(void);
+ void flush(void);
+ ~CQueue();
+};
+
+#endif