diff --git a/include/model_car_driver_base.h b/include/model_car_driver_base.h index b42af69808c3710b994d72089f0be579a9faec56..31fd8a94b93bd31f3976255781d7277e0193f7c5 100644 --- a/include/model_car_driver_base.h +++ b/include/model_car_driver_base.h @@ -43,7 +43,8 @@ class CModelCarDriverBase uint8_t detected_id; uint16_t sw_version; uint32_t rx_timeout; - uint32_t timestamp; + uint32_t timestamp_lsb; + uint32_t timestamp_msb; /* event attributes */ std::string finish_thread_event_id; @@ -66,7 +67,7 @@ class CModelCarDriverBase void process_data(THeader & header, TDataUnion & data_union, TCRCUnion & crc_union); virtual void process_data_frame(uint8_t id,TDataUnion data_union)=0; uint16_t fletcher16(uint8_t *data, uint8_t bytes); - uint32_t get_last_timestamp(void); + uint64_t get_last_timestamp(void); public: CModelCarDriverBase(std::string name, ARDUINO_ID arduino_id); void open(void); diff --git a/src/model_car_driver_base.cpp b/src/model_car_driver_base.cpp index 0ef6a8f18afbbfaf12502adab87337ae7dcad35d..57d0ac724ab25603b1683df6dd1a3edbfc8958c1 100644 --- a/src/model_car_driver_base.cpp +++ b/src/model_car_driver_base.cpp @@ -13,7 +13,8 @@ CModelCarDriverBase::CModelCarDriverBase(std::string name, ARDUINO_ID arduino_id this->detected_id=-1; this->sw_version=-1; this->rx_timeout=DEFAULT_TIMEOUT; - this->timestamp=0; + this->timestamp_lsb=0; + this->timestamp_msb=0; /* init events */ this->event_server=CEventServer::instance(); @@ -247,8 +248,10 @@ bool CModelCarDriverBase::process_byte(THeader &header, TDataUnion &data_union, return frame_ready; } -void CModelCarDriverBase::process_data(THeader & header, TDataUnion & data_union, TCRCUnion & crc_union) +void CModelCarDriverBase::process_data(THeader &header, TDataUnion &data_union, TCRCUnion &crc_union) { + static uint32_t last_timestamp; + static bool first=true; uint8_t * single_frame=NULL; uint16_t calculated_crc; uint8_t frame_size; @@ -261,6 +264,19 @@ void CModelCarDriverBase::process_data(THeader & header, TDataUnion & data_union calculated_crc = this->fletcher16(single_frame, frame_size); delete[] single_frame; + if(first) + { + first=false; + last_timestamp=header.time_stamp; + } + else + { + this->timestamp_lsb=header.time_stamp; + if(header.time_stamp<last_timestamp)//32 bit overflow + this->timestamp_msb++; + last_timestamp=header.time_stamp; + } + if (calculated_crc != crc_union.crc) std::cout << "CModelCarDriverBase: error, frame CRC invalid. Skipping frame" << std::endl; else @@ -321,7 +337,6 @@ void CModelCarDriverBase::process_data(THeader & header, TDataUnion & data_union break; default: this->data_mutex.enter(); - this->timestamp=header.time_stamp; this->process_data_frame(header.id, data_union); this->data_mutex.exit(); break; @@ -498,9 +513,9 @@ double CModelCarDriverBase::get_rx_timeout(void) return this->rx_timeout; } -uint32_t CModelCarDriverBase::get_last_timestamp(void) +uint64_t CModelCarDriverBase::get_last_timestamp(void) { - return this->timestamp; + return ((uint64_t)this->timestamp_msb<<32)+this->timestamp_lsb; } std::string CModelCarDriverBase::get_new_data_event_id(void)