T* RingBuffer<T>::writeOneBuffer()
{
pthread_mutex_lock(&_lock);
int futWritePos = (getWritePos() + 1) % getNumBufers();
while (futWritePos == getReadPos())
pthread_cond_wait(&_wrcond, &_lock);
T *buf = _buffer + getWritePos()*getBufSize();
pthread_mutex_unlock(&_lock);
return buf;
}
int Buffer::vnprintf(size_t size, const char* format, va_list args)
{
int pos = getWritePos();
if(!(m_dataLength - m_writePos < size) || enlargeData(size)){
char* dst = reinterpret_cast<char*>(m_data + m_writePos);
vsnprintf(dst, size, format, args);
dst[size - 1] = '\0'; //No matter if truncated or not, this
//should be safe.
int written = strlen(dst) + 1;
m_writePos += written;
if(m_validBytes < m_writePos){
m_validBytes = m_writePos;
}
}
checkIntegrity();
return getWritePos() - pos;
}
void RingBuffer<T>::updateWritePos() {
pthread_mutex_lock(&_lock);
_writePos = (getWritePos() + 1) % getNumBufers();
pthread_cond_signal(&_rdcond);
pthread_mutex_unlock(&_lock);
}
T* RingBuffer<T>::readOneBuffer()
{
pthread_mutex_lock(&_lock);
while (getReadPos() == getWritePos())
pthread_cond_wait(&_rdcond, &_lock);
T *buf = _buffer + getReadPos()*getBufSize();
pthread_mutex_unlock(&_lock);
return buf;
}
void HardwareSensorCameraFirewire::preloadTask(void)
{ try {
struct timeval ts, *pts = &ts;
int r;
//bool emptied_buffers = false;
//double date = 0.;
while(true)
{
// acquire the image
#ifdef HAVE_VIAM
int buff_write = getWritePos();
//if (!emptied_buffers) date = kernel::Clock::getTime();
r = viam_oneshot(handle, bank, &(bufferImage[buff_write]), &pts, 1);
//if (!emptied_buffers) { date = kernel::Clock::getTime()-date; if (date < 0.004) continue; else emptied_buffers = true; }
bufferSpecPtr[buff_write]->arrival = kernel::Clock::getTime();
bufferSpecPtr[buff_write]->timestamp = ts.tv_sec + ts.tv_usec*1e-6;
last_timestamp = bufferSpecPtr[buff_write]->timestamp;
#endif
incWritePos();
condition.setAndNotify(1);
}
} catch (kernel::Exception &e) { std::cout << e.what(); throw e; } }
