WaterTankController/TankMonitor.cpp at main · fredy314/WaterTankController · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
/*
 * WaterTankController project
 * Copyright (c) 2026 Fedir Vilhota <fredy31415@gmail.com>
 * This software is released under the MIT License.
 * See the LICENSE file in the project root for full license information.
 */
// TankMonitor.cpp
#include "TankMonitor.h" // Включаємо свій же заголовок
#include "Constants.h"

// Реалізація конструктора класу TankMonitor
TankMonitor::TankMonitor(Log& logRef)
    // Ініціалізація констант-членів класу за допомогою списку ініціалізації
    : _TANK_EMPTY_DISTANCE_CM(TANK_EMPTY_DISTANCE_CM),
      _TANK_FULL_DISTANCE_CM(TANK_FULL_DISTANCE_CM),
      _TANK_WIDTH_CM(TANK_WIDTH_CM),
      _TANK_LENGTH_CM(TANK_LENGTH_CM),
      _currentVolume(0.0), // Ініціалізація змінних стану
      _currentDistanceCm(0.0),
      _rollingVolumeIndex(0),
      _rollingVolumesCount(0),
      _lastVolumeRateUpdateMillis(0),
      _averagedCurrentRateLitersPerMin(0.0),
      _estimatedTimeMin(0),
      _log(logRef)
{
    // Ініціалізація буферів нулями
    for (int i = 0; i < _ROLLING_VOLUME_WINDOW_SIZE; i++) {
        _volumeReadingsForRate[i] = 0.0;
        _timestampsForRate[i] = 0;
    }
    // Ініціалізація таймера для першого виклику calculateRatesAndEstimates
    _lastVolumeRateUpdateMillis = millis();
}

// Реалізація методу оновлення відстані
void TankMonitor::setDistance(float distanceCm) {
    _currentDistanceCm = distanceCm;
}

// Реалізація методу розрахунку швидкості та оцінки часу
void TankMonitor::calculateRatesAndEstimates() {
    unsigned long currentMillis = millis();
    // Виконуємо розрахунки лише кожні _VOLUME_RATE_UPDATE_INTERVAL_MS (1 секунду)
    if (currentMillis - _lastVolumeRateUpdateMillis >= _VOLUME_RATE_UPDATE_INTERVAL_MS) {
        _lastVolumeRateUpdateMillis = currentMillis;
        // 1. Розрахунок поточного об'єму на основі останньої відстані
        float calculatedVolume = (_TANK_EMPTY_DISTANCE_CM - _currentDistanceCm) * _TANK_WIDTH_CM * _TANK_LENGTH_CM / 1000.0;
        if (calculatedVolume < 0) calculatedVolume = 0; // Об'єм не може бути від'ємним
        _currentVolume = calculatedVolume; // Оновлюємо приватний член класу

        // 2. Зберігаємо поточний об'єм та часову мітку у ковзний буфер
        _volumeReadingsForRate[_rollingVolumeIndex] = _currentVolume;
        _timestampsForRate[_rollingVolumeIndex] = currentMillis;
        _rollingVolumeIndex = (_rollingVolumeIndex + 1) % _ROLLING_VOLUME_WINDOW_SIZE; // Циклічний індекс
        if (_rollingVolumesCount < _ROLLING_VOLUME_WINDOW_SIZE) { // Лічильник заповнених елементів
            _rollingVolumesCount++;
        }
        // 3. Виконання лінійної регресії для визначення середньої швидкості
        _performLinearRegression();
        // 4. Розрахунок орієнтовного часу до повного/порожнього баку
        float totalTankVolume = (_TANK_EMPTY_DISTANCE_CM - _TANK_FULL_DISTANCE_CM) * _TANK_WIDTH_CM * _TANK_LENGTH_CM / 1000.0;
        if (totalTankVolume < 0) totalTankVolume = 0;
        float remainingVolumeToEmpty = _currentVolume;
        float remainingVolumeToFill = totalTankVolume - _currentVolume;
        _estimatedTimeMin = 0;
        // Якщо швидкість позитивна - бак наповнюється, якщо негативна - спорожнюється
        // Пороги узгоджені з _performLinearRegression (0.01 л/хв)
        if (_averagedCurrentRateLitersPerMin > 0.01) { // Наповнення
            if (remainingVolumeToFill > 0) {
                _estimatedTimeMin = remainingVolumeToFill / _averagedCurrentRateLitersPerMin;
            }
        } else if (_averagedCurrentRateLitersPerMin < -0.1) { // Спорожнення (з порогом для шуму)
            if (remainingVolumeToEmpty > 0 && _averagedCurrentRateLitersPerMin < 0) { // Запобігаємо діленню на нуль або додатне
                _estimatedTimeMin = remainingVolumeToEmpty / _averagedCurrentRateLitersPerMin;
            }
        }
        _log.add(_currentVolume);
    }
}

// Реалізація приватної функції для лінійної регресії
void TankMonitor::_performLinearRegression() {
    _averagedCurrentRateLitersPerMin = 0.0;
    if (_rollingVolumesCount >= 2) { // Потрібно мінімум дві точки для регресії
        double sumX = 0.0;
        double sumY = 0.0;
        double sumXY = 0.0;
        double sumX2 = 0.0;

        // Визначаємо стартовий індекс у циклічному буфері (найстаріша точка)
        int startIndex = (_rollingVolumeIndex - _rollingVolumesCount + _ROLLING_VOLUME_WINDOW_SIZE) % _ROLLING_VOLUME_WINDOW_SIZE;

        // Отримуємо базову часову мітку (найстаріша точка) для нормалізації
        unsigned long baseTimestamp = _timestampsForRate[startIndex];

        for (int j = 0; j < _rollingVolumesCount; j++) {
            int physicalIndex = (startIndex + j) % _ROLLING_VOLUME_WINDOW_SIZE;
            double currentY = _volumeReadingsForRate[physicalIndex]; // Об'єм (Y)
            // Використовуємо реальний час в секундах відносно базової мітки
            double currentX = (_timestampsForRate[physicalIndex] - baseTimestamp) / 1000.0; // мс -> сек
            sumX += currentX;
            sumY += currentY;
            sumXY += currentX * currentY;
            sumX2 += currentX * currentX;
        }

        double N = _rollingVolumesCount;
        double numerator = N * sumXY - sumX * sumY;
        double denominator = N * sumX2 - sumX * sumX;

        if (denominator != 0) {
            // Нахил (slope) = швидкість в л/сек
            double calculatedRateLitersPerSecond = numerator / denominator;
            double calculatedRateLitersPerMinute = calculatedRateLitersPerSecond * 60.0;

            // Пороги для фільтрації шуму
            if (calculatedRateLitersPerMinute > 0.01) {
                _averagedCurrentRateLitersPerMin = calculatedRateLitersPerMinute;
            } else if (calculatedRateLitersPerMinute < -0.01) {
                _averagedCurrentRateLitersPerMin = calculatedRateLitersPerMinute;
            } else {
                _averagedCurrentRateLitersPerMin = 0.0;
            }
        }
    }
}

// Реалізація методу для отримання даних буфера в CSV форматі
String TankMonitor::getVolumesCsv() const {
    String csvString = "";
    if (_rollingVolumesCount > 0) {
        int startIndex = (_rollingVolumeIndex - _rollingVolumesCount + _ROLLING_VOLUME_WINDOW_SIZE) % _ROLLING_VOLUME_WINDOW_SIZE;
        for (int j = 0; j < _rollingVolumesCount; j++) {
            int physicalIndex = (startIndex + j) % _ROLLING_VOLUME_WINDOW_SIZE;
            // Manual float formatting to 2 decimals
            float val = _volumeReadingsForRate[physicalIndex];
            int intPart = (int)val;
            int fracPart = (int)((val - intPart) * 100);
            if (fracPart < 0) fracPart = -fracPart;

            csvString += String(intPart) + "." + (fracPart < 10 ? "0" : "") + String(fracPart);

            if (j < _rollingVolumesCount - 1) {
                csvString += ",";
            }
        }
    }
    return csvString;
}