1#![no_std]
2#![forbid(unsafe_code)]
3#![deny(warnings, clippy::all)]
4
5use micromath::F32Ext;
6
7#[derive(Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash, Debug)]
8pub enum AlgorithmType {
9 Voc,
10 Nox,
11}
12
13#[derive(Debug)]
14pub struct GasIndexAlgorithm {
15 state: GasIndexAlgorithmParams,
16}
17
18impl GasIndexAlgorithm {
19 pub const fn new_uninitialized(algorithm_type: AlgorithmType) -> Self {
21 Self {
22 state: GasIndexAlgorithmParams::new_uninit(algorithm_type),
23 }
24 }
25
26 pub fn new(algorithm_type: AlgorithmType, sampling_interval: f32) -> Self {
27 let mut s = Self::new_uninitialized(algorithm_type);
28 s.init_with_sampling_interval(sampling_interval);
29 s
30 }
31
32 pub fn init_with_sampling_interval(&mut self, sampling_interval: f32) {
33 self.state
34 .init_with_sampling_interval(self.state.algorithm_type, sampling_interval);
35 }
36
37 pub fn process(&mut self, sraw: i32) -> i32 {
42 self.state.process(sraw)
43 }
44}
45
46#[derive(Copy, Clone, PartialEq, PartialOrd, Debug)]
47pub struct GasIndexAlgorithmParams {
48 pub algorithm_type: AlgorithmType,
49 pub sampling_interval: f32,
50 pub index_offset: f32,
51 pub sraw_minimum: i32,
52 pub gating_max_duration_minutes: f32,
53 pub init_duration_mean: f32,
54 pub init_duration_variance: f32,
55 pub gating_threshold: f32,
56 pub index_gain: f32,
57 pub tau_mean_hours: f32,
58 pub tau_variance_hours: f32,
59 pub sraw_std_initial: f32,
60 pub uptime: f32,
61 pub sraw: f32,
62 pub gas_index: f32,
63 pub mean_variance_estimator_initialized: bool,
64 pub mean_variance_estimator_mean: f32,
65 pub mean_variance_estimator_sraw_offset: f32,
66 pub mean_variance_estimator_std: f32,
67 pub mean_variance_estimator_gamma_mean: f32,
68 pub mean_variance_estimator_gamma_variance: f32,
69 pub mean_variance_estimator_gamma_initial_mean: f32,
70 pub mean_variance_estimator_gamma_initial_variance: f32,
71 pub mean_variance_estimator_gamma_mean2: f32,
72 pub mean_variance_estimator_gamma_variance2: f32,
73 pub mean_variance_estimator_uptime_gamma: f32,
74 pub mean_variance_estimator_uptime_gating: f32,
75 pub mean_variance_estimator_gating_duration_minutes: f32,
76 pub mean_variance_estimator_sigmoid_k: f32,
77 pub mean_variance_estimator_sigmoid_x0: f32,
78 pub mox_model_sraw_std: f32,
79 pub mox_model_sraw_mean: f32,
80 pub sigmoid_scaled_k: f32,
81 pub sigmoid_scaled_x0: f32,
82 pub sigmoid_scaled_offset_default: f32,
83 pub adaptive_lowpass_a1: f32,
84 pub adaptive_lowpass_a2: f32,
85 pub adaptive_lowpass_initialized: bool,
86 pub adaptive_lowpass_x1: f32,
87 pub adaptive_lowpass_x2: f32,
88 pub adaptive_lowpass_x3: f32,
89}
90
91impl GasIndexAlgorithmParams {
92 const fn new_uninit(algorithm_type: AlgorithmType) -> Self {
93 GasIndexAlgorithmParams {
94 algorithm_type,
95 sampling_interval: 0.0,
96 index_offset: 0.0,
97 sraw_minimum: 0,
98 gating_max_duration_minutes: 0.0,
99 init_duration_mean: 0.0,
100 init_duration_variance: 0.0,
101 gating_threshold: 0.0,
102 index_gain: 0.0,
103 tau_mean_hours: 0.0,
104 tau_variance_hours: 0.0,
105 sraw_std_initial: 0.0,
106 uptime: 0.0,
107 sraw: 0.0,
108 gas_index: 0.0,
109 mean_variance_estimator_initialized: false,
110 mean_variance_estimator_mean: 0.0,
111 mean_variance_estimator_sraw_offset: 0.0,
112 mean_variance_estimator_std: 0.0,
113 mean_variance_estimator_gamma_mean: 0.0,
114 mean_variance_estimator_gamma_variance: 0.0,
115 mean_variance_estimator_gamma_initial_mean: 0.0,
116 mean_variance_estimator_gamma_initial_variance: 0.0,
117 mean_variance_estimator_gamma_mean2: 0.0,
118 mean_variance_estimator_gamma_variance2: 0.0,
119 mean_variance_estimator_uptime_gamma: 0.0,
120 mean_variance_estimator_uptime_gating: 0.0,
121 mean_variance_estimator_gating_duration_minutes: 0.0,
122 mean_variance_estimator_sigmoid_k: 0.0,
123 mean_variance_estimator_sigmoid_x0: 0.0,
124 mox_model_sraw_std: 0.0,
125 mox_model_sraw_mean: 0.0,
126 sigmoid_scaled_k: 0.0,
127 sigmoid_scaled_x0: 0.0,
128 sigmoid_scaled_offset_default: 0.0,
129 adaptive_lowpass_a1: 0.0,
130 adaptive_lowpass_a2: 0.0,
131 adaptive_lowpass_initialized: false,
132 adaptive_lowpass_x1: 0.0,
133 adaptive_lowpass_x2: 0.0,
134 adaptive_lowpass_x3: 0.0,
135 }
136 }
137
138 fn init_with_sampling_interval(
139 &mut self,
140 algorithm_type: AlgorithmType,
141 sampling_interval: f32,
142 ) {
143 self.algorithm_type = algorithm_type;
144 self.sampling_interval = sampling_interval;
145 match self.algorithm_type {
146 AlgorithmType::Nox => {
147 self.index_offset = 1.0;
148 self.sraw_minimum = 10000;
149 self.gating_max_duration_minutes = 60.0 * 12.0;
150 self.init_duration_mean = 3600.0 * 4.75f32;
151 self.init_duration_variance = 3600.0 * 5.70;
152 self.gating_threshold = 30.0;
153 }
154 AlgorithmType::Voc => {
155 self.index_offset = 100.0;
156 self.sraw_minimum = 20000;
157 self.gating_max_duration_minutes = 60.0 * 3.0;
158 self.init_duration_mean = 3600.0 * 0.75f32;
159 self.init_duration_variance = 3600.0 * 1.45f32;
160 self.gating_threshold = 340.0;
161 }
162 }
163 self.index_gain = 230.0;
164 self.tau_mean_hours = 12.0;
165 self.tau_variance_hours = 12.0;
166 self.sraw_std_initial = 50.0;
167 self.reset();
168 }
169
170 fn reset(&mut self) {
171 self.uptime = 0.0;
172 self.sraw = 0.0;
173 self.gas_index = 0.0;
174 self.init_instances();
175 }
176
177 fn init_instances(&mut self) {
178 self.mean_variance_estimator_set_parameters();
179 let sraw_std = self.mean_variance_estimator_get_std();
180 let sraw_mean = self.mean_variance_estimator_get_mean();
181 self.mox_model_set_parameters(sraw_std, sraw_mean);
182 match self.algorithm_type {
183 AlgorithmType::Nox => {
184 self.sigmoid_scaled_set_parameters(614.0, -0.0101f32, 1.0);
185 }
186 AlgorithmType::Voc => {
187 self.sigmoid_scaled_set_parameters(213.0, -0.0065f32, 100.0);
188 }
189 }
190 self.adaptive_lowpass_set_parameters();
191 }
192
193 fn process(&mut self, mut sraw: i32) -> i32 {
194 if self.uptime <= 45.0 {
195 self.uptime += self.sampling_interval;
196 } else {
197 if sraw > 0 && sraw < 65000 {
198 if sraw < self.sraw_minimum + 1 {
199 sraw = self.sraw_minimum + 1;
200 } else if sraw > self.sraw_minimum + 32767 {
201 sraw = self.sraw_minimum + 32767;
202 }
203 self.sraw = (sraw - self.sraw_minimum) as f32;
204 }
205 if self.algorithm_type == AlgorithmType::Voc
206 || self.mean_variance_estimator_is_initialized()
207 {
208 self.gas_index = self.mox_model_process(self.sraw);
209 self.gas_index = self.sigmoid_scaled_process(self.gas_index);
210 } else {
211 self.gas_index = self.index_offset;
212 }
213 self.gas_index = self.adaptive_lowpass_process(self.gas_index);
214 if self.gas_index < 0.5f32 {
215 self.gas_index = 0.5f32;
216 }
217 if self.sraw > 0.0 {
218 self.mean_variance_estimator_process(self.sraw);
219
220 let sraw_std = self.mean_variance_estimator_get_std();
221 let sraw_mean = self.mean_variance_estimator_get_mean();
222 self.mox_model_set_parameters(sraw_std, sraw_mean);
223 }
224 }
225 (self.gas_index + 0.5f32) as i32
226 }
227
228 fn mean_variance_estimator_set_parameters(&mut self) {
229 self.mean_variance_estimator_initialized = false;
230 self.mean_variance_estimator_mean = 0.0;
231 self.mean_variance_estimator_sraw_offset = 0.0;
232 self.mean_variance_estimator_std = self.sraw_std_initial;
233 self.mean_variance_estimator_gamma_mean = 8.0 * 64.0 * (self.sampling_interval / 3600.0)
234 / (self.tau_mean_hours + self.sampling_interval / 3600.0);
235 self.mean_variance_estimator_gamma_variance = 64.0 * (self.sampling_interval / 3600.0)
236 / (self.tau_variance_hours + self.sampling_interval / 3600.0);
237 match self.algorithm_type {
238 AlgorithmType::Nox => {
239 self.mean_variance_estimator_gamma_initial_mean =
240 8.0 * 64.0 * self.sampling_interval / (1200.0 + self.sampling_interval);
241 }
242 AlgorithmType::Voc => {
243 self.mean_variance_estimator_gamma_initial_mean =
244 8.0 * 64.0 * self.sampling_interval / (20.0 + self.sampling_interval);
245 }
246 }
247 self.mean_variance_estimator_gamma_initial_variance =
248 64.0 * self.sampling_interval / (2500.0 + self.sampling_interval);
249 self.mean_variance_estimator_gamma_mean2 = 0.0;
250 self.mean_variance_estimator_gamma_variance2 = 0.0;
251 self.mean_variance_estimator_uptime_gamma = 0.0;
252 self.mean_variance_estimator_uptime_gating = 0.0;
253 self.mean_variance_estimator_gating_duration_minutes = 0.0;
254 }
255
256 fn mean_variance_estimator_get_std(&mut self) -> f32 {
257 self.mean_variance_estimator_std
258 }
259
260 fn mean_variance_estimator_get_mean(&mut self) -> f32 {
261 self.mean_variance_estimator_mean + self.mean_variance_estimator_sraw_offset
262 }
263
264 fn mean_variance_estimator_is_initialized(&mut self) -> bool {
265 self.mean_variance_estimator_initialized
266 }
267
268 fn mean_variance_estimator_calculate_gamma(&mut self) {
269 let uptime_limit = 32767.0 - self.sampling_interval;
270 if self.mean_variance_estimator_uptime_gamma < uptime_limit {
271 self.mean_variance_estimator_uptime_gamma += self.sampling_interval;
272 }
273 if self.mean_variance_estimator_uptime_gating < uptime_limit {
274 self.mean_variance_estimator_uptime_gating += self.sampling_interval;
275 }
276 self.mean_variance_estimator_sigmoid_set_parameters(self.init_duration_mean, 0.01f32);
277 let sigmoid_gamma_mean =
278 self.mean_variance_estimator_sigmoid_process(self.mean_variance_estimator_uptime_gamma);
279 let gamma_mean = self.mean_variance_estimator_gamma_mean
280 + (self.mean_variance_estimator_gamma_initial_mean
281 - self.mean_variance_estimator_gamma_mean)
282 * sigmoid_gamma_mean;
283 let gating_threshold_mean = self.gating_threshold
284 + (510.0 - self.gating_threshold)
285 * self.mean_variance_estimator_sigmoid_process(
286 self.mean_variance_estimator_uptime_gating,
287 );
288 self.mean_variance_estimator_sigmoid_set_parameters(gating_threshold_mean, 0.09f32);
289 let sigmoid_gating_mean = self.mean_variance_estimator_sigmoid_process(self.gas_index);
290 self.mean_variance_estimator_gamma_mean2 = sigmoid_gating_mean * gamma_mean;
291 self.mean_variance_estimator_sigmoid_set_parameters(self.init_duration_variance, 0.01f32);
292 let sigmoid_gamma_variance =
293 self.mean_variance_estimator_sigmoid_process(self.mean_variance_estimator_uptime_gamma);
294 let gamma_variance = self.mean_variance_estimator_gamma_variance
295 + (self.mean_variance_estimator_gamma_initial_variance
296 - self.mean_variance_estimator_gamma_variance)
297 * (sigmoid_gamma_variance - sigmoid_gamma_mean);
298 let gating_threshold_variance = self.gating_threshold
299 + (510.0 - self.gating_threshold)
300 * self.mean_variance_estimator_sigmoid_process(
301 self.mean_variance_estimator_uptime_gating,
302 );
303 self.mean_variance_estimator_sigmoid_set_parameters(gating_threshold_variance, 0.09f32);
304 let sigmoid_gating_variance = self.mean_variance_estimator_sigmoid_process(self.gas_index);
305 self.mean_variance_estimator_gamma_variance2 = sigmoid_gating_variance * gamma_variance;
306
307 self.mean_variance_estimator_gating_duration_minutes +=
308 self.sampling_interval / 60.0 * ((1.0 - sigmoid_gating_mean) * (1.0 + 0.3f32) - 0.3f32);
309 if self.mean_variance_estimator_gating_duration_minutes < 0.0 {
310 self.mean_variance_estimator_gating_duration_minutes = 0.0;
311 }
312 if self.mean_variance_estimator_gating_duration_minutes > self.gating_max_duration_minutes {
313 self.mean_variance_estimator_uptime_gating = 0.0;
314 }
315 }
316
317 fn mean_variance_estimator_process(&mut self, mut sraw: f32) {
318 if !self.mean_variance_estimator_initialized {
319 self.mean_variance_estimator_initialized = true;
320 self.mean_variance_estimator_sraw_offset = sraw;
321 self.mean_variance_estimator_mean = 0.0;
322 } else {
323 if self.mean_variance_estimator_mean >= 100.0
324 || self.mean_variance_estimator_mean <= -100.0
325 {
326 self.mean_variance_estimator_sraw_offset += self.mean_variance_estimator_mean;
327 self.mean_variance_estimator_mean = 0.0;
328 }
329 sraw -= self.mean_variance_estimator_sraw_offset;
330
331 self.mean_variance_estimator_calculate_gamma();
332 let delta_sgp = (sraw - self.mean_variance_estimator_mean) / 64.0;
333 let c = if delta_sgp < 0.0 {
334 self.mean_variance_estimator_std - delta_sgp
335 } else {
336 self.mean_variance_estimator_std + delta_sgp
337 };
338 let mut additional_scaling = 1.0;
339 if c > 1440.0 {
340 additional_scaling = c / 1440.0 * (c / 1440.0);
341 }
342 self.mean_variance_estimator_std =
343 sqrtf(additional_scaling * (64.0 - self.mean_variance_estimator_gamma_variance2))
344 * sqrtf(
345 self.mean_variance_estimator_std
346 * (self.mean_variance_estimator_std / (64.0 * additional_scaling))
347 + self.mean_variance_estimator_gamma_variance2 * delta_sgp
348 / additional_scaling
349 * delta_sgp,
350 );
351 self.mean_variance_estimator_mean +=
352 self.mean_variance_estimator_gamma_mean2 * delta_sgp / 8.0;
353 };
354 }
355
356 fn mean_variance_estimator_sigmoid_set_parameters(&mut self, x0: f32, k: f32) {
357 self.mean_variance_estimator_sigmoid_k = k;
358 self.mean_variance_estimator_sigmoid_x0 = x0;
359 }
360
361 fn mean_variance_estimator_sigmoid_process(&mut self, sample: f32) -> f32 {
362 let x: f32 = self.mean_variance_estimator_sigmoid_k
363 * (sample - self.mean_variance_estimator_sigmoid_x0);
364 if x < -50.0 {
365 1.0
366 } else if x > 50.0 {
367 0.0
368 } else {
369 1.0 / (1.0 + expf(x))
370 }
371 }
372
373 fn mox_model_set_parameters(&mut self, sraw_std: f32, sraw_mean: f32) {
374 self.mox_model_sraw_std = sraw_std;
375 self.mox_model_sraw_mean = sraw_mean;
376 }
377
378 fn mox_model_process(&mut self, sraw: f32) -> f32 {
379 match self.algorithm_type {
380 AlgorithmType::Nox => (sraw - self.mox_model_sraw_mean) / 2000.0 * self.index_gain,
381 AlgorithmType::Voc => {
382 (sraw - self.mox_model_sraw_mean) / (-1.0 * (self.mox_model_sraw_std + 220.0))
383 * self.index_gain
384 }
385 }
386 }
387
388 fn sigmoid_scaled_set_parameters(&mut self, x0: f32, k: f32, offset_default: f32) {
389 self.sigmoid_scaled_k = k;
390 self.sigmoid_scaled_x0 = x0;
391 self.sigmoid_scaled_offset_default = offset_default;
392 }
393
394 fn sigmoid_scaled_process(&mut self, sample: f32) -> f32 {
395 let x = self.sigmoid_scaled_k * (sample - self.sigmoid_scaled_x0);
396 if x < -50.0 {
397 500.0
398 } else if x > 50.0 {
399 0.0
400 } else if sample >= 0.0 {
401 let shift = if self.sigmoid_scaled_offset_default == 1.0 {
402 500.0 / 499.0 * (1.0 - self.index_offset)
403 } else {
404 (500.0 - 5.0 * self.index_offset) / 4.0
405 };
406 (500.0 + shift) / (1.0 + expf(x)) - shift
407 } else {
408 self.index_offset / self.sigmoid_scaled_offset_default * (500.0 / (1.0 + expf(x)))
409 }
410 }
411
412 fn adaptive_lowpass_set_parameters(&mut self) {
413 self.adaptive_lowpass_a1 = self.sampling_interval / (20.0 + self.sampling_interval);
414 self.adaptive_lowpass_a2 = self.sampling_interval / (500.0 + self.sampling_interval);
415 self.adaptive_lowpass_initialized = false;
416 }
417
418 fn adaptive_lowpass_process(&mut self, sample: f32) -> f32 {
419 if !self.adaptive_lowpass_initialized {
420 self.adaptive_lowpass_x1 = sample;
421 self.adaptive_lowpass_x2 = sample;
422 self.adaptive_lowpass_x3 = sample;
423 self.adaptive_lowpass_initialized = true;
424 }
425 self.adaptive_lowpass_x1 = (1.0 - self.adaptive_lowpass_a1) * self.adaptive_lowpass_x1
426 + self.adaptive_lowpass_a1 * sample;
427 self.adaptive_lowpass_x2 = (1.0 - self.adaptive_lowpass_a2) * self.adaptive_lowpass_x2
428 + self.adaptive_lowpass_a2 * sample;
429 let mut abs_delta = self.adaptive_lowpass_x1 - self.adaptive_lowpass_x2;
430 if abs_delta < 0.0 {
431 abs_delta *= -1.0;
432 }
433 let f1 = expf(-0.2f32 * abs_delta);
434 let tau_a = (500.0 - 20.0) * f1 + 20.0;
435 let a3 = self.sampling_interval / (self.sampling_interval + tau_a);
436 self.adaptive_lowpass_x3 = (1.0 - a3) * self.adaptive_lowpass_x3 + a3 * sample;
437 self.adaptive_lowpass_x3
438 }
439}
440
441fn expf(value: f32) -> f32 {
442 F32Ext::exp(value)
443}
444
445fn sqrtf(value: f32) -> f32 {
446 F32Ext::sqrt(value)
447}
448
449#[cfg(test)]
450mod tests {
451 use super::*;
452
453 #[test]
454 fn voc_reaches_mean() {
455 let mut algo = GasIndexAlgorithm::new(AlgorithmType::Voc, 1.0);
456 for _ in 0..200 {
457 let _ = algo.process(1337);
458 }
459 assert_eq!(algo.process(1337), 100);
460 }
461
462 #[test]
463 fn nox_reaches_mean() {
464 let mut algo = GasIndexAlgorithm::new(AlgorithmType::Nox, 1.0);
465 for _ in 0..200 {
466 let _ = algo.process(1337);
467 }
468 assert_eq!(algo.process(1337), 1);
469 }
470}