diff --git a/include/model_car_driver_base.h b/include/model_car_driver_base.h
index 7ac2ff26342b3511698195d01a9b0b54d72de891..d207d9217fbe24641f80f9939a1d7c015ea2b07c 100644
--- a/include/model_car_driver_base.h
+++ b/include/model_car_driver_base.h
@@ -44,6 +44,9 @@ class CModelCarDriverBase
uint id;
uint sw_version;
+ tCRCUnion crc_union;
+ TDataUnion data_union;
+ THeader header;
/* event attributes */
CEventServer *event_server;
@@ -59,6 +62,7 @@ class CModelCarDriverBase
CMutex data_mutex;
static void *data_thread(void *param);
+ bool process_byte(unsigned char byte);
bool get_id(uint8_t & id);
void send_request(uint8_t id, uint8_t data_length, uint8_t *data);
sm_state process_data(THeader & header, TDataUnion & data_union, tCRCUnion & crc_union);
diff --git a/src/model_car_batteries.cpp b/src/model_car_batteries.cpp
index 7d4f52cef21218d5d04d81c69a16092cf1fcf754..f32a37762c14492c8133bc121bb28f549eefd51c 100644
--- a/src/model_car_batteries.cpp
+++ b/src/model_car_batteries.cpp
@@ -3,6 +3,7 @@
CModelCarBatteries::CModelCarBatteries(std::string name) : CModelCarDriverBase(name, ARDUINO_CENTER_MEASUREMENT)
{
TVoltageData v;
+ v.voltage=0;
m_bat_values.insert(std::make_pair(ID_ARD_SENS_VOLT_ACTUATOR, v));
m_bat_values.insert(std::make_pair(ID_ARD_SENS_VOLT_ACTUATOR_CELL1, v));
m_bat_values.insert(std::make_pair(ID_ARD_SENS_VOLT_ACTUATOR_CELL2, v));
diff --git a/src/model_car_driver_base.cpp b/src/model_car_driver_base.cpp
index e0dcbd0be2dbc4100f36bec3a5562e918f46f682..b25c970c1df38176908c0da5928a8cb7a2bffe2e 100644
--- a/src/model_car_driver_base.cpp
+++ b/src/model_car_driver_base.cpp
@@ -42,16 +42,6 @@ void CModelCarDriverBase::open()
{
std::vector<std::string> dev_list;
- /*
- dev_list.push_back("/dev/ttyACM0");
- dev_list.push_back("/dev/ttyACM1");
- dev_list.push_back("/dev/ttyACM2");
- dev_list.push_back("/dev/ttyACM3");
- */
-
- //override first device to lookup:
- //dev_list.push_back("/dev/ttyACM3");
-
this->close();
std::cout << "CModelCarDriverBase::open(): scanning ttyACM* devices in /sys/class/tty directory" << std::endl;
@@ -138,9 +128,6 @@ bool CModelCarDriverBase::open_dev(std::string &serial_dev)
void CModelCarDriverBase::close()
{
- //std::cout << "CModelCarDriverBase::close(): stopping" << std::endl;
-
- //this->event_server->reset_event(this->new_info_event_id);
if(this->thread_server->get_thread_state(this->data_thread_id)==starting ||
this->thread_server->get_thread_state(this->data_thread_id)==active)
{
@@ -149,9 +136,7 @@ void CModelCarDriverBase::close()
this->event_server->reset_event(this->finish_thread_event_id);
}
else
- {
std::cout << "CModelCarDriverBase::close(): data thread already ended" << std::endl;
- }
this->port_mutex.enter();
if(this->serial_port!=NULL)
@@ -161,11 +146,8 @@ void CModelCarDriverBase::close()
this->serial_port=NULL;
}
else
- {
std::cout << "CModelCarDriverBase::close(): serial port already closed" << std::endl;
- }
this->port_mutex.exit();
- //std::cout << "CModelCarDriverBase::close(): stopped" << std::endl;
}
void CModelCarDriverBase::delete_thread_and_events()
@@ -183,20 +165,18 @@ void *CModelCarDriverBase::data_thread(void *param)
{
CModelCarDriverBase *driver=(CModelCarDriverBase *)param;
bool end=false;
+
+ unsigned char * frame_data=NULL;
+ unsigned int num;
+ std::list<std::string> events;
+ events.push_back(driver->serial_port->get_rx_event_id());
while(!end)
{
try
{
- static sm_state state=WAIT_START;
- unsigned char * frame_data=NULL;
- unsigned int num;
- std::list<std::string> events;
- events.push_back(driver->serial_port->get_rx_event_id());
-
driver->port_mutex.enter();
- bool empty = (num=driver->serial_port->get_num_data())==0;
- if(empty)
+ if(driver->serial_port->get_num_data()==0)
{
driver->port_mutex.exit();
driver->event_server->wait_all(events,1000);
@@ -208,163 +188,17 @@ void *CModelCarDriverBase::data_thread(void *param)
driver->serial_port->read(frame_data,num);
driver->port_mutex.exit();
- bool escaped=false;
- int ts_count=0;
- int data_count=0;
- int crc_count=0;
- tTimeStampUnion ts_union;
- tCRCUnion crc_union;
-
- TDataUnion data_union;
- THeader header;
-
//driver->cout_data(frame_data,num,"frame_data");
for(unsigned int i=0; i<num; i++)
{
- //std::cout << "state=" << state << std::endl;
- //std::cout << "frame[" << i << "]=" << hex(frame_data[i]) << std::endl;
-
- if(frame_data[i]==ESCAPE_BYTE && !escaped)
+ bool frame_ready = driver->process_byte(frame_data[i]);
+ if(frame_ready)
+ driver->process_data(driver->header, driver->data_union, driver->crc_union);
+ else
{
- //std::cout << " esc_byte!" << std::endl;
- escaped=true;
- continue;
- }
- else if(frame_data[i]==START_BYTE && !escaped && state!=WAIT_START)
- {
- std::cout << "CModelCarDriverBase::data_thread: WARNING, found unexpected START_BYTE" << std::endl;
- state=READ_ID;
- }
- else if(frame_data[i]==END_BYTE && !escaped && state!=READ_END)
- {
- std::cout << "CModelCarDriverBase::data_thread: WARNING, found unexpected END_BYTE" << std::endl;
- state=WAIT_START;
- }
-
-
- //std::cout << " state=" << state << std::endl;
- switch(state)
- {
- case WAIT_START:
- if(frame_data[i]==START_BYTE)
- {
- if(!escaped)
- {
- //std::cout << " found start byte! " << hex(frame_data[i]) << std::endl;
- state=READ_ID;
- }
- else
- {
- escaped=false;
- state=WAIT_START;
- }
- }
- else
- state=WAIT_START;
- break;
-
- case READ_ID:
- if(escaped)
- escaped=false;
- header.id = frame_data[i];
- //std::cout << " found id="<< unsigned(header.id) << std::endl;
- state=READ_DATA_LENGTH;
- break;
-
- case READ_DATA_LENGTH:
- if(escaped)
- escaped=false;
- header.data_length = frame_data[i];
- //std::cout << " data length="<< unsigned(header.data_length) << std::endl;
- ts_count=0;
- state=READ_TIMESTAMP;
- break;
- case READ_TIMESTAMP:
- if(escaped)
- escaped=false;
- ts_union.raw[ts_count] = frame_data[i];
- //std::cout << " reading timestamp " << ts_count+1 << "/4: "<< hex(ts_union.raw[ts_count]) << std::endl;
- if(ts_count<3)
- {
- ts_count++;
- state=READ_TIMESTAMP;
- }
- else
- {
- header.time_stamp = ts_union.time_stamp;
- //std::cout << " time stamp="<< unsigned(header.time_stamp) << std::endl;
- data_count=0;
- state=READ_DATA;
- }
- break;
-
- case READ_DATA:
- if(escaped)
- escaped=false;
- data_union.raw[data_count] = frame_data[i];
- //std::cout << " reading data " << data_count+1 << "/" << unsigned(header.data_length) << ": " << hex(data_union.raw[data_count]) << std::endl;
- if(data_count<header.data_length-1)
- {
- data_count++;
- state=READ_DATA;
- }
- else
- {
- /*
- std::cout << " data=";
- for(unsigned int ii=0; ii<header.data_length; ii++)
- {
- std::cout << hex(data_union.raw[ii]) << ",";
- }
- std::cout << std::endl;
- */
-
- crc_count=0;
- state=READ_CRC;
- }
- break;
-
- case READ_CRC:
- if(escaped)
- escaped=false;
- crc_union.raw[crc_count] = frame_data[i];
- //std::cout << " reading crc " << crc_count+1 << "/2: " << hex(crc_union.raw[crc_count]) << std::endl;
- if(crc_count<1)
- {
- crc_count++;
- state=READ_CRC;
- }
- else
- {
- //std::cout << " crc="<< hex(((crc_union.crc >> (8*0)) & 0xff)) << "," << hex(((crc_union.crc >> (8*1)) & 0xff))<< std::endl;
- state=READ_END;
- }
- break;
-
- case READ_END:
- if(frame_data[i]==END_BYTE && !escaped)
- {
- //std::cout << " found end byte! " << unsigned(frame_data[i]) << std::endl;
- state = driver->process_data(header, data_union, crc_union);
- }
- else
- {
- if(escaped)
- escaped=false;
- std::cout << "CModelCarDriverBase::data_thread: error, no END BYTE found" << std::endl;
- state=WAIT_START;
- }
- break;
- }
-
- //enf of buffer
- if(i==num-1)
- {
- if(state!=WAIT_START)
- {
- std::cout<< "CModelCarDriverBase::data_thread: buffer ended and frame not fully read, state=" << state << std::endl;
- }
+ if(i==num-1)
+ std::cout<< "CModelCarDriverBase::data_thread: buffer ended and frame not fully read" << std::endl;
}
}
if(frame_data!=NULL)
@@ -381,11 +215,129 @@ void *CModelCarDriverBase::data_thread(void *param)
if(driver->event_server->event_is_set(driver->finish_thread_event_id))
end=true;
-
}
pthread_exit(NULL);
}
+
+
+bool CModelCarDriverBase::process_byte(unsigned char byte)
+{
+ bool frame_ready=false;
+
+ static sm_state state=WAIT_START;
+ static bool escaped =false;
+ static int ts_count =0;
+ static int data_count=0;
+ static int crc_count =0;
+ tTimeStampUnion ts_union;
+
+ if(byte==ESCAPE_BYTE && !escaped)
+ escaped=true;
+ else
+ {
+ if(byte==START_BYTE && !escaped && state!=WAIT_START)
+ {
+ std::cout << "CModelCarDriverBase::data_thread: WARNING, found unexpected START_BYTE" << std::endl;
+ state=READ_ID;
+ }
+ else if(byte==END_BYTE && !escaped && state!=READ_END)
+ {
+ std::cout << "CModelCarDriverBase::data_thread: WARNING, found unexpected END_BYTE" << std::endl;
+ state=WAIT_START;
+ }
+
+ switch(state)
+ {
+ case WAIT_START:
+ if(byte==START_BYTE)
+ {
+ if(!escaped)
+ state=READ_ID;
+ else
+ {
+ escaped=false;
+ state=WAIT_START;
+ }
+ }
+ else
+ state=WAIT_START;
+ break;
+ case READ_ID:
+ if(escaped)
+ escaped=false;
+ header.id = byte;
+ state=READ_DATA_LENGTH;
+ break;
+ case READ_DATA_LENGTH:
+ if(escaped)
+ escaped=false;
+ header.data_length = byte;
+ ts_count=0;
+ state=READ_TIMESTAMP;
+ break;
+ case READ_TIMESTAMP:
+ if(escaped)
+ escaped=false;
+ ts_union.raw[ts_count] = byte;
+ if(ts_count<3)
+ {
+ ts_count++;
+ state=READ_TIMESTAMP;
+ }
+ else
+ {
+ header.time_stamp = ts_union.time_stamp;
+ data_count=0;
+ state=READ_DATA;
+ }
+ break;
+ case READ_DATA:
+ if(escaped)
+ escaped=false;
+ data_union.raw[data_count] = byte;
+ if(data_count<header.data_length-1)
+ {
+ data_count++;
+ state=READ_DATA;
+ }
+ else
+ {
+ crc_count=0;
+ state=READ_CRC;
+ }
+ break;
+ case READ_CRC:
+ if(escaped)
+ escaped=false;
+ crc_union.raw[crc_count] = byte;
+ if(crc_count<1)
+ {
+ crc_count++;
+ state=READ_CRC;
+ }
+ else
+ state=READ_END;
+ break;
+ case READ_END:
+ if(byte==END_BYTE && !escaped)
+ {
+ frame_ready=true;
+ state=WAIT_START;
+ }
+ else
+ {
+ if(escaped)
+ escaped=false;
+ std::cout << "CModelCarDriverBase::data_thread: error, no END BYTE found" << std::endl;
+ state=WAIT_START;
+ }
+ break;
+ }
+ }
+ return frame_ready;
+}
+
sm_state CModelCarDriverBase::process_data(THeader & header, TDataUnion & data_union, tCRCUnion & crc_union)
{
sm_state state;
@@ -394,28 +346,17 @@ sm_state CModelCarDriverBase::process_data(THeader & header, TDataUnion & data_u
TInfoData info_data;
TErrorData error_data;
- //CHECK_CRC
-
unsigned int frame_size = sizeof(THeader)+header.data_length;
single_frame = new unsigned char[frame_size];
memcpy(single_frame, &header, sizeof(THeader));
memcpy(&single_frame[sizeof(THeader)], &data_union, header.data_length);
- //std::cout << " crc frame=";
- // for(unsigned int ii=0; ii<frame_size; ii++)
- // {
- // std::cout << hex(single_frame[ii]) << ",";
- // }
- // std::cout << std::endl;
-
calculated_crc = this->fletcher16(single_frame, frame_size);
if(single_frame!=NULL)
delete []single_frame;
- //std::cout << " calculated_crc="<< hex(((calculated_crc >> (8*0)) & 0xff)) << "," << hex(((calculated_crc >> (8*1)) & 0xff))<< std::endl;
- //calculated_crc = 0xff << 8 | 0xff;
if (calculated_crc != crc_union.crc)
{
std::cout << "CModelCarDriverBase: error, frame CRC invalid. Skipping frame" << std::endl;
@@ -423,10 +364,6 @@ sm_state CModelCarDriverBase::process_data(THeader & header, TDataUnion & data_u
}
else
{
- //std::cout << " valid CRC!" << std::endl;
- //state=PROCESS_DATA;
- //std::cout << "header.id=" << (unsigned int)header.id << std::endl;
- //PROCESS_DATA
switch(header.id)
{
case ID_ARD_SENSOR_INFO:
@@ -484,7 +421,6 @@ sm_state CModelCarDriverBase::process_data(THeader & header, TDataUnion & data_u
}
break;
default:
- //std::cout << "CModelCarDriverBase::data_thread: process data frame" << std::endl;
this->data_mutex.enter();
this->process_data_frame(header.id, header.time_stamp, data_union);
if(!this->event_server->event_is_set(this->new_data_event_id))
@@ -504,16 +440,14 @@ bool CModelCarDriverBase::get_id(uint8_t & id)
std::list<std::string> events;
events.push_back(this->new_info_event_id);
- //std::cout << "CModelCarDriverBase: waiting info frame" << std::endl;
try
{
this->event_server->wait_all(events,1000);
}
catch(CEventTimeoutException &e)
{
- std::cout << e.what() << std::endl;
+ std::cout << e.what() << std::endl;
}
- //std::cout << "CModelCarDriverBase: received info frame" << std::endl;
ok=true;
id=this->id;
@@ -522,9 +456,6 @@ bool CModelCarDriverBase::get_id(uint8_t & id)
void CModelCarDriverBase::send_request(uint8_t id, uint8_t data_length, uint8_t *data)
{
- //std::cout << "CModelCarDriverBase::send_request()" <<std::endl;
- //std::cout << "CModelCarDriverBase::send_request(); id: " << hex(id) << " data_length: " << hex(data_length) <<std::endl;
-
unsigned int req_frame_size = sizeof(uint8_t)+sizeof(uint8_t)+sizeof(tTimeStampUnion)+data_length;
unsigned char *frame;
frame = new unsigned char[req_frame_size];
@@ -537,12 +468,9 @@ void CModelCarDriverBase::send_request(uint8_t id, uint8_t data_length, uint8_t
memcpy(frame+2*sizeof(uint8_t), &ts, sizeof(tTimeStampUnion));
for(unsigned int i=0; i<data_length; i++)
- {
memcpy(frame+2*sizeof(uint8_t)+sizeof(tTimeStampUnion), &data[i], sizeof(uint8_t));
- }
uint16_t calculated_crc = fletcher16(frame, uint8_t(req_frame_size));
- //std::cout << " calculated_crc="<< hex(((calculated_crc >> (8*0)) & 0xff)) << "," << hex(((calculated_crc >> (8*1)) & 0xff))<< std::endl;
unsigned char *frame_with_crc;
frame_with_crc = new unsigned char[req_frame_size+sizeof(uint16_t)];
@@ -552,33 +480,13 @@ void CModelCarDriverBase::send_request(uint8_t id, uint8_t data_length, uint8_t
req_frame_size+=sizeof(uint16_t);
- /*
- std::cout << "CModelCarDriverBase::send_request(): DEBUG: frame_with_crc = ";
- for(unsigned int i=0; i<req_frame_size; i++)
- {
- std::cout << hex(frame_with_crc[i]) << "," << std::dec;
- }
- std::cout << std::endl;
- */
-
delete []frame;
uint8_t *stuffed_frame=NULL;
uint8_t stuffed_frame_size=0;
this->stuff_frame(frame_with_crc,req_frame_size,&stuffed_frame,stuffed_frame_size);
- //std::cout << "stuffed_frame_size = " << (unsigned int)stuffed_frame_size << std::endl;
delete []frame_with_crc;
- /*
- std::cout << "CModelCarDriverBase::send_request(): DEBUG: sending frame = ";
- for(unsigned int i=0; i<stuffed_frame_size; i++)
- {
- std::cout << hex(stuffed_frame[i]) << "," << std::dec;
- }
- std::cout << std::endl;
- */
-
-
this->port_mutex.enter();
this->serial_port->write(stuffed_frame,stuffed_frame_size);
this->port_mutex.exit();
@@ -595,8 +503,6 @@ void CModelCarDriverBase::stuff_frame(uint8_t const *frame_in, uint8_t const siz
}
size_out = (unsigned int)size_in+esc_num+2;
- //std::cout << " size_out= "<<(unsigned int)size_out << std::endl;
- //*frame_out = new unsigned char[(unsigned int)size_out];
*frame_out = new uint8_t[size_out];
std::fill(*frame_out, *frame_out + (unsigned int)size_out, 0x00);
@@ -606,7 +512,6 @@ void CModelCarDriverBase::stuff_frame(uint8_t const *frame_in, uint8_t const siz
(*frame_out)[(unsigned int)size_out-1]=END_BYTE;
for(unsigned int i=0; i<(unsigned int)size_in; i++)
{
- //std::cout << "i,ii=" << i << ","<< ii << std::endl;
if( (frame_in[i]==START_BYTE) || (frame_in[i]==END_BYTE) || (frame_in[i]==ESCAPE_BYTE ) )
{
(*frame_out)[ii]=ESCAPE_BYTE;
@@ -620,15 +525,6 @@ void CModelCarDriverBase::stuff_frame(uint8_t const *frame_in, uint8_t const siz
ii++;
}
}
-
- /*
- std::cout << "CModelCarDriverBase::stuff_frame(): DEBUG: frame_out = ";
- for(unsigned int i=0; i<(unsigned int)size_out; i++)
- {
- std::cout << hex((*frame_out)[i]) << "," << std::dec;
- }
- std::cout << std::endl;
- */
}
uint16_t CModelCarDriverBase::fletcher16(uint8_t const *data, uint8_t bytes)
@@ -662,9 +558,7 @@ void CModelCarDriverBase::cout_data(unsigned char * data, unsigned int size, std
{
std::cout << "CModelCarDriverBase::cout_data: " << name << "= ";
for(unsigned int i=0; i<size; i++)
- {
std::cout << hex(data[i]) << "," << std::dec;
- }
std::cout << std::endl;
}
diff --git a/src/model_car_ultrasounds.cpp b/src/model_car_ultrasounds.cpp
index 68abaf8333e325a6894d07a30ac46d109ee59ac0..f1968d0dca04fe2a1c8046acf66b6faec4c0054f 100644
--- a/src/model_car_ultrasounds.cpp
+++ b/src/model_car_ultrasounds.cpp
@@ -3,6 +3,7 @@
CModelCarUltrasounds::CModelCarUltrasounds(std::string name) : CModelCarDriverBase(name, ARDUINO_REAR_US)
{
TUltrasoundData us_data;
+ us_data.distance=0;
m_uss_values.insert(std::make_pair(ID_ARD_SENS_US_SIDE_RIGHT, us_data));
m_uss_values.insert(std::make_pair(ID_ARD_SENS_US_REAR_CENTER_RIGHT, us_data));
m_uss_values.insert(std::make_pair(ID_ARD_SENS_US_REAR_CENTER, us_data));