Commit e88569f7 authored by Thiago Santini's avatar Thiago Santini

Fixes Synchronizer bug

If higher priority trigger had been open, it's queue would never be empty.
Thus, if that triggers is turned off, next trigger would never kick in.
parent 971fb7de
...@@ -21,58 +21,50 @@ Synchronizer::Synchronizer(QObject* parent) ...@@ -21,58 +21,50 @@ Synchronizer::Synchronizer(QObject* parent)
this, SLOT(synchronize())); this, SLOT(synchronize()));
timer->start(userSynchronizeMs); timer->start(userSynchronizeMs);
} }
} emptyEyeData.timestamp = -1;
emptyFieldData.timestamp = -1;
Synchronizer::~Synchronizer()
{
} }
template <class T> template <class T>
void Synchronizer::updateList(std::list<T>& dataList, T& data) void Synchronizer::updateDeque(std::deque<T>& dataDeque, T& data)
{ {
dataList.emplace_back(data); dataDeque.emplace_back(data);
while (dataList.back().timestamp - dataList.front().timestamp > maxAgeMs) while (dataDeque.back().timestamp - dataDeque.front().timestamp > maxAgeMs)
dataList.pop_front(); dataDeque.pop_front();
updated = true; updated = true;
} }
void Synchronizer::newRightEyeData(EyeData eyeData) void Synchronizer::newRightEyeData(EyeData eyeData)
{ {
updateList(rEyeList, eyeData); updateDeque(rEyeDeque, eyeData);
synchronize(rEyeDeque.back().timestamp, Trigger::RIGHT_EYE);
if (!rEyeList.empty())
synchronize(eyeData.timestamp);
} }
void Synchronizer::newLeftEyeData(EyeData eyeData) void Synchronizer::newLeftEyeData(EyeData eyeData)
{ {
updateList(lEyeList, eyeData); updateDeque(lEyeDeque, eyeData);
synchronize(lEyeDeque.back().timestamp, Trigger::LEFT_EYE);
if (rEyeList.empty())
synchronize(eyeData.timestamp);
} }
void Synchronizer::newFieldData(FieldData fieldData) void Synchronizer::newFieldData(FieldData fieldData)
{ {
updateList(fieldList, fieldData); updateDeque(fieldDeque, fieldData);
synchronize(fieldDeque.back().timestamp, Trigger::FIELD_EYE);
if (rEyeList.empty() && lEyeList.empty())
synchronize(fieldData.timestamp);
} }
template <class T> template <class T>
T* Synchronizer::getClosestInTime(Timestamp timestamp, list<T>& dataList) T* Synchronizer::getClosestInTime(Timestamp timestamp, deque<T>& dataDeque)
{ {
T* closest = nullptr; T* closest = nullptr;
if (dataList.empty()) if (dataDeque.empty())
return closest; return closest;
unsigned int minimum = UINT_MAX; Timestamp minimum = std::numeric_limits<Timestamp>::max();
for (auto it = dataList.begin(); it != dataList.end(); ++it) { for (auto& data : dataDeque) {
unsigned int diff = abs(timestamp - it->timestamp); unsigned int diff = abs(timestamp - data.timestamp);
if (diff < minimum) { if (diff < minimum) {
minimum = diff; minimum = diff;
closest = &(*it); closest = &data;
} }
} }
...@@ -82,14 +74,31 @@ T* Synchronizer::getClosestInTime(Timestamp timestamp, list<T>& dataList) ...@@ -82,14 +74,31 @@ T* Synchronizer::getClosestInTime(Timestamp timestamp, list<T>& dataList)
return closest; return closest;
} }
void Synchronizer::synchronize(Timestamp timestamp) bool Synchronizer::shouldSynchronize(Timestamp timestamp, Trigger trigger)
{
switch (trigger) {
case Trigger::FIELD_EYE:
if (!lEyeDeque.empty())
if (timestamp - lEyeDeque.back().timestamp < maxAgeMs)
return false;
case Trigger::LEFT_EYE:
if (!rEyeDeque.empty())
if (timestamp - rEyeDeque.back().timestamp < maxAgeMs)
return false;
case Trigger::RIGHT_EYE:
default:
return true;
}
}
void Synchronizer::synchronize(Timestamp timestamp, Trigger trigger)
{ {
if (!updated) if (!updated)
return; return;
updated = false; updated = false;
if (timestamp == 0) if (!shouldSynchronize(timestamp, trigger))
timestamp = ns2ms(gTimer.nsecsElapsed()); return;
if (timestamp == maxTimestamp) // nothing recent was available if (timestamp == maxTimestamp) // nothing recent was available
return; return;
...@@ -100,15 +109,15 @@ void Synchronizer::synchronize(Timestamp timestamp) ...@@ -100,15 +109,15 @@ void Synchronizer::synchronize(Timestamp timestamp)
lastSynchronization = timestamp; lastSynchronization = timestamp;
FieldData* field = getClosestInTime<FieldData>(timestamp, fieldList); FieldData* field = getClosestInTime<FieldData>(timestamp, fieldDeque);
EyeData* lEye = getClosestInTime<EyeData>(timestamp, lEyeList); EyeData* lEye = getClosestInTime<EyeData>(timestamp, lEyeDeque);
EyeData* rEye = getClosestInTime<EyeData>(timestamp, rEyeList); EyeData* rEye = getClosestInTime<EyeData>(timestamp, rEyeDeque);
DataTuple dataTuple( DataTuple dataTuple(
timestamp, timestamp,
(lEye ? *lEye : EyeData()), (lEye ? *lEye : emptyEyeData),
(rEye ? *rEye : EyeData()), (rEye ? *rEye : emptyEyeData),
(field ? *field : FieldData())); (field ? *field : emptyFieldData));
emit newData(dataTuple); emit newData(dataTuple);
} }
#ifndef SYNCHRONIZER_H #ifndef SYNCHRONIZER_H
#define SYNCHRONIZER_H #define SYNCHRONIZER_H
#include <list> #include <deque>
#include <QObject> #include <QObject>
#include <QSize> #include <QSize>
...@@ -15,7 +15,6 @@ class Synchronizer : public QObject { ...@@ -15,7 +15,6 @@ class Synchronizer : public QObject {
public: public:
explicit Synchronizer(QObject* parent = nullptr); explicit Synchronizer(QObject* parent = nullptr);
~Synchronizer();
signals: signals:
void newData(DataTuple dataTuple); void newData(DataTuple dataTuple);
...@@ -26,22 +25,30 @@ public slots: ...@@ -26,22 +25,30 @@ public slots:
void newFieldData(FieldData fieldData); void newFieldData(FieldData fieldData);
private: private:
enum class Trigger {
RIGHT_EYE = 0,
LEFT_EYE = 1,
FIELD_EYE = 2,
};
unsigned int maxAgeMs; unsigned int maxAgeMs;
unsigned int safeGuardMs; unsigned int safeGuardMs;
std::list<EyeData> lEyeList; std::deque<EyeData> lEyeDeque;
std::list<EyeData> rEyeList; std::deque<EyeData> rEyeDeque;
std::list<FieldData> fieldList; std::deque<FieldData> fieldDeque;
EyeData emptyEyeData;
FieldData emptyFieldData;
bool updated; bool updated;
Timestamp lastSynchronization; Timestamp lastSynchronization;
QTimer* timer; QTimer* timer;
template <class T> template <class T>
T* getClosestInTime(Timestamp timestamp, std::list<T>& dataList); T* getClosestInTime(Timestamp timestamp, std::deque<T>& dataDeque);
template <class T> template <class T>
void updateList(std::list<T>& dataList, T& data); void updateDeque(std::deque<T>& dataDeque, T& data);
bool shouldSynchronize(Timestamp timestamp, Trigger trigger);
private slots: void synchronize(Timestamp timestamp, Trigger trigger);
void synchronize(Timestamp timestamp = 0);
}; };
#endif // SYNCHRONIZER_H #endif // SYNCHRONIZER_H
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment