90 lines
1.7 KiB
C++
90 lines
1.7 KiB
C++
/*
|
|
* STMicroelectronics Change ODR Timestamp Stack Class
|
|
*
|
|
* Copyright 2015-2016 STMicroelectronics Inc.
|
|
* Author: Denis Ciocca - <denis.ciocca@st.com>
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License").
|
|
*/
|
|
|
|
#include "ChangeODRTimestampStack.h"
|
|
|
|
ChangeODRTimestampStack::ChangeODRTimestampStack()
|
|
{
|
|
pthread_mutex_init(&data_mutex, NULL);
|
|
|
|
elements_available = 0;
|
|
}
|
|
|
|
ChangeODRTimestampStack::~ChangeODRTimestampStack()
|
|
{
|
|
|
|
}
|
|
|
|
int ChangeODRTimestampStack::writeElement(int64_t timestamp, int64_t newpollrate)
|
|
{
|
|
pthread_mutex_lock(&data_mutex);
|
|
|
|
if (elements_available >= ST_ODR_STACK_MAX_ELEMENTS) {
|
|
pthread_mutex_unlock(&data_mutex);
|
|
return -ENOMEM;
|
|
}
|
|
|
|
timestamps[elements_available] = timestamp;
|
|
new_pollrate[elements_available] = newpollrate;
|
|
elements_available++;
|
|
|
|
pthread_mutex_unlock(&data_mutex);
|
|
|
|
return 0;
|
|
}
|
|
|
|
int64_t ChangeODRTimestampStack::readLastElement(int64_t *newpollrate)
|
|
{
|
|
int64_t latest_change_odr_timestamp;
|
|
|
|
pthread_mutex_lock(&data_mutex);
|
|
|
|
if (elements_available == 0) {
|
|
pthread_mutex_unlock(&data_mutex);
|
|
return (int64_t)(int)(-EIO);
|
|
}
|
|
|
|
latest_change_odr_timestamp = timestamps[0];
|
|
*newpollrate = new_pollrate[0];
|
|
|
|
pthread_mutex_unlock(&data_mutex);
|
|
|
|
return latest_change_odr_timestamp;
|
|
}
|
|
|
|
void ChangeODRTimestampStack::removeLastElement()
|
|
{
|
|
int i;
|
|
|
|
pthread_mutex_lock(&data_mutex);
|
|
|
|
if (elements_available == 0) {
|
|
pthread_mutex_unlock(&data_mutex);
|
|
return;
|
|
}
|
|
|
|
for (i = 0; i < (int)elements_available - 1; i++) {
|
|
timestamps[i] = timestamps[i + 1];
|
|
new_pollrate[i] = new_pollrate[i + 1];
|
|
}
|
|
|
|
elements_available--;
|
|
|
|
pthread_mutex_unlock(&data_mutex);
|
|
}
|
|
|
|
void ChangeODRTimestampStack::resetBuffer()
|
|
{
|
|
pthread_mutex_lock(&data_mutex);
|
|
|
|
elements_available = 0;
|
|
|
|
pthread_mutex_unlock(&data_mutex);
|
|
}
|