111 lines
2.3 KiB
C++
111 lines
2.3 KiB
C++
#include "PressureMonitor.h"
|
|
#include <cmath>
|
|
|
|
PressureMonitor::PressureMonitor()
|
|
: m_tickCounter(0)
|
|
, m_sampleCount(0)
|
|
, m_bufIdx(0)
|
|
, m_totalTickCount(0)
|
|
, m_timeout(false)
|
|
{
|
|
for (int i = 0; i < BUF_SIZE; i++) {
|
|
m_buf[i] = 0.0f;
|
|
}
|
|
}
|
|
|
|
bool PressureMonitor::tick(float realtimePressure, float calibratedPressure)
|
|
{
|
|
m_totalTickCount++;
|
|
|
|
if (m_totalTickCount >= TIMEOUT_TICKS) {
|
|
m_timeout = true;
|
|
return true;
|
|
}
|
|
|
|
if (m_tickCounter != 0) {
|
|
m_tickCounter = (m_tickCounter + 1) % SAMPLE_INTERVAL_TICKS;
|
|
return false;
|
|
}
|
|
m_tickCounter = (m_tickCounter + 1) % SAMPLE_INTERVAL_TICKS;
|
|
|
|
if (calibratedPressure <= 0.0f) {
|
|
return false;
|
|
}
|
|
|
|
m_buf[m_bufIdx] = realtimePressure;
|
|
m_bufIdx = (m_bufIdx + 1) % BUF_SIZE;
|
|
m_sampleCount++;
|
|
|
|
if (m_sampleCount < BUF_SIZE) {
|
|
return false;
|
|
}
|
|
|
|
float rangeMin = calibratedPressure - 10.0f;
|
|
float rangeMax = calibratedPressure + 10.0f;
|
|
|
|
if (!checkFirst4InRange(rangeMin, rangeMax)) {
|
|
return false;
|
|
}
|
|
|
|
float avg = computeTrimmedMean();
|
|
return (avg >= rangeMin && avg <= rangeMax);
|
|
}
|
|
|
|
void PressureMonitor::reset()
|
|
{
|
|
m_tickCounter = 0;
|
|
m_sampleCount = 0;
|
|
m_bufIdx = 0;
|
|
m_totalTickCount = 0;
|
|
m_timeout = false;
|
|
for (int i = 0; i < BUF_SIZE; i++) {
|
|
m_buf[i] = 0.0f;
|
|
}
|
|
}
|
|
|
|
bool PressureMonitor::isCurrentlyInRange() const
|
|
{
|
|
return (m_sampleCount >= BUF_SIZE);
|
|
}
|
|
|
|
int PressureMonitor::stableTickCount() const
|
|
{
|
|
return m_sampleCount;
|
|
}
|
|
|
|
bool PressureMonitor::wasTimeout() const
|
|
{
|
|
return m_timeout;
|
|
}
|
|
|
|
const float* PressureMonitor::buffer() const
|
|
{
|
|
return m_buf;
|
|
}
|
|
|
|
bool PressureMonitor::checkFirst4InRange(float rangeMin, float rangeMax) const
|
|
{
|
|
for (int i = 0; i < 4; i++) {
|
|
float val = m_buf[(m_bufIdx + i) % BUF_SIZE];
|
|
if (val < rangeMin || val > rangeMax) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
float PressureMonitor::computeTrimmedMean() const
|
|
{
|
|
float minVal = m_buf[0];
|
|
float maxVal = m_buf[0];
|
|
float sum = 0.0f;
|
|
|
|
for (int i = 0; i < BUF_SIZE; i++) {
|
|
float val = m_buf[i];
|
|
sum += val;
|
|
if (val < minVal) minVal = val;
|
|
if (val > maxVal) maxVal = val;
|
|
}
|
|
|
|
return (sum - minVal - maxVal) / (float)(BUF_SIZE - 2);
|
|
} |