1 /* $NetBSD: sensirion_voc_algorithm.c,v 1.2 2021/10/18 14:14:07 christos Exp $
2 */
3
4 /*
5 * Copyright (c) 2021, Sensirion AG
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions are met:
10 *
11 * * Redistributions of source code must retain the above copyright notice, this
12 * list of conditions and the following disclaimer.
13 *
14 * * Redistributions in binary form must reproduce the above copyright notice,
15 * this list of conditions and the following disclaimer in the documentation
16 * and/or other materials provided with the distribution.
17 *
18 * * Neither the name of Sensirion AG nor the names of its
19 * contributors may be used to endorse or promote products derived from
20 * this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
23 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
24 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
25 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
26 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
27 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
28 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
29 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
30 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
31 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 */
34
35 #include "sensirion_voc_algorithm.h"
36
37 /* The fixed point arithmetic parts of this code were originally created by
38 * https://github.com/PetteriAimonen/libfixmath
39 */
40
41 /*!< the maximum value of fix16_t */
42 #define FIX16_MAXIMUM 0x7FFFFFFF
43 /*!< the minimum value of fix16_t */
44 #define FIX16_MINIMUM 0x80000000
45 /*!< the value used to indicate overflows when FIXMATH_NO_OVERFLOW is not
46 * specified */
47 #define FIX16_OVERFLOW 0x80000000
48 /*!< fix16_t value of 1 */
49 #define FIX16_ONE 0x00010000
50
fix16_from_int(int32_t a)51 static inline fix16_t fix16_from_int(int32_t a) {
52 return a * FIX16_ONE;
53 }
54
fix16_cast_to_int(fix16_t a)55 static inline int32_t fix16_cast_to_int(fix16_t a) {
56 return (a >= 0) ? (a >> 16) : -((-a) >> 16);
57 }
58
59 /*! Multiplies the two given fix16_t's and returns the result. */
60 static fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1);
61
62 /*! Divides the first given fix16_t by the second and returns the result. */
63 static fix16_t fix16_div(fix16_t inArg0, fix16_t inArg1);
64
65 /*! Returns the square root of the given fix16_t. */
66 static fix16_t fix16_sqrt(fix16_t inValue);
67
68 /*! Returns the exponent (e^) of the given fix16_t. */
69 static fix16_t fix16_exp(fix16_t inValue);
70
fix16_mul(fix16_t inArg0,fix16_t inArg1)71 static fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1) {
72 // Each argument is divided to 16-bit parts.
73 // AB
74 // * CD
75 // -----------
76 // BD 16 * 16 -> 32 bit products
77 // CB
78 // AD
79 // AC
80 // |----| 64 bit product
81 uint32_t absArg0 = (uint32_t)((inArg0 >= 0) ? inArg0 : (-inArg0));
82 uint32_t absArg1 = (uint32_t)((inArg1 >= 0) ? inArg1 : (-inArg1));
83 uint32_t A = (absArg0 >> 16), C = (absArg1 >> 16);
84 uint32_t B = (absArg0 & 0xFFFF), D = (absArg1 & 0xFFFF);
85
86 uint32_t AC = A * C;
87 uint32_t AD_CB = A * D + C * B;
88 uint32_t BD = B * D;
89
90 uint32_t product_hi = AC + (AD_CB >> 16);
91
92 // Handle carry from lower 32 bits to upper part of result.
93 uint32_t ad_cb_temp = AD_CB << 16;
94 uint32_t product_lo = BD + ad_cb_temp;
95 if (product_lo < BD)
96 product_hi++;
97
98 #ifndef FIXMATH_NO_OVERFLOW
99 // The upper 17 bits should all be zero.
100 if (product_hi >> 15)
101 return (fix16_t)FIX16_OVERFLOW;
102 #endif
103
104 #ifdef FIXMATH_NO_ROUNDING
105 fix16_t result = (fix16_t)((product_hi << 16) | (product_lo >> 16));
106 if ((inArg0 < 0) != (inArg1 < 0))
107 result = -result;
108 return result;
109 #else
110 // Adding 0x8000 (= 0.5) and then using right shift
111 // achieves proper rounding to result.
112 // Handle carry from lower to upper part.
113 uint32_t product_lo_tmp = product_lo;
114 product_lo += 0x8000;
115 if (product_lo < product_lo_tmp)
116 product_hi++;
117
118 // Discard the lowest 16 bits and convert back to signed result.
119 fix16_t result = (fix16_t)((product_hi << 16) | (product_lo >> 16));
120 if ((inArg0 < 0) != (inArg1 < 0))
121 result = -result;
122 return result;
123 #endif
124 }
125
fix16_div(fix16_t a,fix16_t b)126 static fix16_t fix16_div(fix16_t a, fix16_t b) {
127 // This uses the basic binary restoring division algorithm.
128 // It appears to be faster to do the whole division manually than
129 // trying to compose a 64-bit divide out of 32-bit divisions on
130 // platforms without hardware divide.
131
132 if (b == 0)
133 return (fix16_t)FIX16_MINIMUM;
134
135 uint32_t remainder = (uint32_t)((a >= 0) ? a : (-a));
136 uint32_t divider = (uint32_t)((b >= 0) ? b : (-b));
137
138 uint32_t quotient = 0;
139 uint32_t bit = 0x10000;
140
141 /* The algorithm requires D >= R */
142 while (divider < remainder) {
143 divider <<= 1;
144 bit <<= 1;
145 }
146
147 #ifndef FIXMATH_NO_OVERFLOW
148 if (!bit)
149 return (fix16_t)FIX16_OVERFLOW;
150 #endif
151
152 if (divider & 0x80000000) {
153 // Perform one step manually to avoid overflows later.
154 // We know that divider's bottom bit is 0 here.
155 if (remainder >= divider) {
156 quotient |= bit;
157 remainder -= divider;
158 }
159 divider >>= 1;
160 bit >>= 1;
161 }
162
163 /* Main division loop */
164 while (bit && remainder) {
165 if (remainder >= divider) {
166 quotient |= bit;
167 remainder -= divider;
168 }
169
170 remainder <<= 1;
171 bit >>= 1;
172 }
173
174 #ifndef FIXMATH_NO_ROUNDING
175 if (remainder >= divider) {
176 quotient++;
177 }
178 #endif
179
180 fix16_t result = (fix16_t)quotient;
181
182 /* Figure out the sign of result */
183 if ((a < 0) != (b < 0)) {
184 #ifndef FIXMATH_NO_OVERFLOW
185 if (result == FIX16_MINIMUM)
186 return (fix16_t)FIX16_OVERFLOW;
187 #endif
188
189 result = -result;
190 }
191
192 return result;
193 }
194
fix16_sqrt(fix16_t x)195 static fix16_t fix16_sqrt(fix16_t x) {
196 // It is assumed that x is not negative
197
198 uint32_t num = (uint32_t)x;
199 uint32_t result = 0;
200 uint32_t bit;
201 uint8_t n;
202
203 bit = (uint32_t)1 << 30;
204 while (bit > num)
205 bit >>= 2;
206
207 // The main part is executed twice, in order to avoid
208 // using 64 bit values in computations.
209 for (n = 0; n < 2; n++) {
210 // First we get the top 24 bits of the answer.
211 while (bit) {
212 if (num >= result + bit) {
213 num -= result + bit;
214 result = (result >> 1) + bit;
215 } else {
216 result = (result >> 1);
217 }
218 bit >>= 2;
219 }
220
221 if (n == 0) {
222 // Then process it again to get the lowest 8 bits.
223 if (num > 65535) {
224 // The remainder 'num' is too large to be shifted left
225 // by 16, so we have to add 1 to result manually and
226 // adjust 'num' accordingly.
227 // num = a - (result + 0.5)^2
228 // = num + result^2 - (result + 0.5)^2
229 // = num - result - 0.5
230 num -= result;
231 num = (num << 16) - 0x8000;
232 result = (result << 16) + 0x8000;
233 } else {
234 num <<= 16;
235 result <<= 16;
236 }
237
238 bit = 1 << 14;
239 }
240 }
241
242 #ifndef FIXMATH_NO_ROUNDING
243 // Finally, if next bit would have been 1, round the result upwards.
244 if (num > result) {
245 result++;
246 }
247 #endif
248
249 return (fix16_t)result;
250 }
251
fix16_exp(fix16_t x)252 static fix16_t fix16_exp(fix16_t x) {
253 // Function to approximate exp(); optimized more for code size than speed
254
255 // exp(x) for x = +/- {1, 1/8, 1/64, 1/512}
256 #define NUM_EXP_VALUES 4
257 static const fix16_t exp_pos_values[NUM_EXP_VALUES] = {
258 F16(2.7182818), F16(1.1331485), F16(1.0157477), F16(1.0019550)};
259 static const fix16_t exp_neg_values[NUM_EXP_VALUES] = {
260 F16(0.3678794), F16(0.8824969), F16(0.9844964), F16(0.9980488)};
261 const fix16_t* exp_values;
262
263 fix16_t res, arg;
264 uint16_t i;
265
266 if (x >= F16(10.3972))
267 return FIX16_MAXIMUM;
268 if (x <= F16(-11.7835))
269 return 0;
270
271 if (x < 0) {
272 x = -x;
273 exp_values = exp_neg_values;
274 } else {
275 exp_values = exp_pos_values;
276 }
277
278 res = FIX16_ONE;
279 arg = FIX16_ONE;
280 for (i = 0; i < NUM_EXP_VALUES; i++) {
281 while (x >= arg) {
282 res = fix16_mul(res, exp_values[i]);
283 x -= arg;
284 }
285 arg >>= 3;
286 }
287 return res;
288 }
289
290 static void VocAlgorithm__init_instances(VocAlgorithmParams* params);
291 static void
292 VocAlgorithm__mean_variance_estimator__init(VocAlgorithmParams* params);
293 static void VocAlgorithm__mean_variance_estimator___init_instances(
294 VocAlgorithmParams* params);
295 static void VocAlgorithm__mean_variance_estimator__set_parameters(
296 VocAlgorithmParams* params, fix16_t std_initial,
297 fix16_t tau_mean_variance_hours, fix16_t gating_max_duration_minutes);
298 static void
299 VocAlgorithm__mean_variance_estimator__set_states(VocAlgorithmParams* params,
300 fix16_t mean, fix16_t std,
301 fix16_t uptime_gamma);
302 static fix16_t
303 VocAlgorithm__mean_variance_estimator__get_std(VocAlgorithmParams* params);
304 static fix16_t
305 VocAlgorithm__mean_variance_estimator__get_mean(VocAlgorithmParams* params);
306 static void VocAlgorithm__mean_variance_estimator___calculate_gamma(
307 VocAlgorithmParams* params, fix16_t voc_index_from_prior);
308 static void VocAlgorithm__mean_variance_estimator__process(
309 VocAlgorithmParams* params, fix16_t sraw, fix16_t voc_index_from_prior);
310 static void VocAlgorithm__mean_variance_estimator___sigmoid__init(
311 VocAlgorithmParams* params);
312 static void VocAlgorithm__mean_variance_estimator___sigmoid__set_parameters(
313 VocAlgorithmParams* params, fix16_t L, fix16_t X0, fix16_t K);
314 static fix16_t VocAlgorithm__mean_variance_estimator___sigmoid__process(
315 VocAlgorithmParams* params, fix16_t sample);
316 static void VocAlgorithm__mox_model__init(VocAlgorithmParams* params);
317 static void VocAlgorithm__mox_model__set_parameters(VocAlgorithmParams* params,
318 fix16_t SRAW_STD,
319 fix16_t SRAW_MEAN);
320 static fix16_t VocAlgorithm__mox_model__process(VocAlgorithmParams* params,
321 fix16_t sraw);
322 static void VocAlgorithm__sigmoid_scaled__init(VocAlgorithmParams* params);
323 static void
324 VocAlgorithm__sigmoid_scaled__set_parameters(VocAlgorithmParams* params,
325 fix16_t offset);
326 static fix16_t VocAlgorithm__sigmoid_scaled__process(VocAlgorithmParams* params,
327 fix16_t sample);
328 static void VocAlgorithm__adaptive_lowpass__init(VocAlgorithmParams* params);
329 static void
330 VocAlgorithm__adaptive_lowpass__set_parameters(VocAlgorithmParams* params);
331 static fix16_t
332 VocAlgorithm__adaptive_lowpass__process(VocAlgorithmParams* params,
333 fix16_t sample);
334
VocAlgorithm_init(VocAlgorithmParams * params)335 void VocAlgorithm_init(VocAlgorithmParams* params) {
336
337 params->mVoc_Index_Offset = F16(VocAlgorithm_VOC_INDEX_OFFSET_DEFAULT);
338 params->mTau_Mean_Variance_Hours =
339 F16(VocAlgorithm_TAU_MEAN_VARIANCE_HOURS);
340 params->mGating_Max_Duration_Minutes =
341 F16(VocAlgorithm_GATING_MAX_DURATION_MINUTES);
342 params->mSraw_Std_Initial = F16(VocAlgorithm_SRAW_STD_INITIAL);
343 params->mUptime = F16(0.);
344 params->mSraw = F16(0.);
345 params->mVoc_Index = 0;
346 VocAlgorithm__init_instances(params);
347 }
348
VocAlgorithm__init_instances(VocAlgorithmParams * params)349 static void VocAlgorithm__init_instances(VocAlgorithmParams* params) {
350
351 VocAlgorithm__mean_variance_estimator__init(params);
352 VocAlgorithm__mean_variance_estimator__set_parameters(
353 params, params->mSraw_Std_Initial, params->mTau_Mean_Variance_Hours,
354 params->mGating_Max_Duration_Minutes);
355 VocAlgorithm__mox_model__init(params);
356 VocAlgorithm__mox_model__set_parameters(
357 params, VocAlgorithm__mean_variance_estimator__get_std(params),
358 VocAlgorithm__mean_variance_estimator__get_mean(params));
359 VocAlgorithm__sigmoid_scaled__init(params);
360 VocAlgorithm__sigmoid_scaled__set_parameters(params,
361 params->mVoc_Index_Offset);
362 VocAlgorithm__adaptive_lowpass__init(params);
363 VocAlgorithm__adaptive_lowpass__set_parameters(params);
364 }
365
VocAlgorithm_get_states(VocAlgorithmParams * params,int32_t * state0,int32_t * state1)366 void VocAlgorithm_get_states(VocAlgorithmParams* params, int32_t* state0,
367 int32_t* state1) {
368
369 *state0 = VocAlgorithm__mean_variance_estimator__get_mean(params);
370 *state1 = VocAlgorithm__mean_variance_estimator__get_std(params);
371 return;
372 }
373
VocAlgorithm_set_states(VocAlgorithmParams * params,int32_t state0,int32_t state1)374 void VocAlgorithm_set_states(VocAlgorithmParams* params, int32_t state0,
375 int32_t state1) {
376
377 VocAlgorithm__mean_variance_estimator__set_states(
378 params, state0, state1, F16(VocAlgorithm_PERSISTENCE_UPTIME_GAMMA));
379 params->mSraw = state0;
380 }
381
VocAlgorithm_set_tuning_parameters(VocAlgorithmParams * params,int32_t voc_index_offset,int32_t learning_time_hours,int32_t gating_max_duration_minutes,int32_t std_initial)382 void VocAlgorithm_set_tuning_parameters(VocAlgorithmParams* params,
383 int32_t voc_index_offset,
384 int32_t learning_time_hours,
385 int32_t gating_max_duration_minutes,
386 int32_t std_initial) {
387
388 params->mVoc_Index_Offset = (fix16_from_int(voc_index_offset));
389 params->mTau_Mean_Variance_Hours = (fix16_from_int(learning_time_hours));
390 params->mGating_Max_Duration_Minutes =
391 (fix16_from_int(gating_max_duration_minutes));
392 params->mSraw_Std_Initial = (fix16_from_int(std_initial));
393 VocAlgorithm__init_instances(params);
394 }
395
VocAlgorithm_process(VocAlgorithmParams * params,int32_t sraw,int32_t * voc_index)396 void VocAlgorithm_process(VocAlgorithmParams* params, int32_t sraw,
397 int32_t* voc_index) {
398
399 if ((params->mUptime <= F16(VocAlgorithm_INITIAL_BLACKOUT))) {
400 params->mUptime =
401 (params->mUptime + F16(VocAlgorithm_SAMPLING_INTERVAL));
402 } else {
403 if (((sraw > 0) && (sraw < 65000))) {
404 if ((sraw < 20001)) {
405 sraw = 20001;
406 } else if ((sraw > 52767)) {
407 sraw = 52767;
408 }
409 params->mSraw = (fix16_from_int((sraw - 20000)));
410 }
411 params->mVoc_Index =
412 VocAlgorithm__mox_model__process(params, params->mSraw);
413 params->mVoc_Index =
414 VocAlgorithm__sigmoid_scaled__process(params, params->mVoc_Index);
415 params->mVoc_Index =
416 VocAlgorithm__adaptive_lowpass__process(params, params->mVoc_Index);
417 if ((params->mVoc_Index < F16(0.5))) {
418 params->mVoc_Index = F16(0.5);
419 }
420 if ((params->mSraw > F16(0.))) {
421 VocAlgorithm__mean_variance_estimator__process(
422 params, params->mSraw, params->mVoc_Index);
423 VocAlgorithm__mox_model__set_parameters(
424 params, VocAlgorithm__mean_variance_estimator__get_std(params),
425 VocAlgorithm__mean_variance_estimator__get_mean(params));
426 }
427 }
428 *voc_index = (fix16_cast_to_int((params->mVoc_Index + F16(0.5))));
429 return;
430 }
431
432 static void
VocAlgorithm__mean_variance_estimator__init(VocAlgorithmParams * params)433 VocAlgorithm__mean_variance_estimator__init(VocAlgorithmParams* params) {
434
435 VocAlgorithm__mean_variance_estimator__set_parameters(params, F16(0.),
436 F16(0.), F16(0.));
437 VocAlgorithm__mean_variance_estimator___init_instances(params);
438 }
439
VocAlgorithm__mean_variance_estimator___init_instances(VocAlgorithmParams * params)440 static void VocAlgorithm__mean_variance_estimator___init_instances(
441 VocAlgorithmParams* params) {
442
443 VocAlgorithm__mean_variance_estimator___sigmoid__init(params);
444 }
445
VocAlgorithm__mean_variance_estimator__set_parameters(VocAlgorithmParams * params,fix16_t std_initial,fix16_t tau_mean_variance_hours,fix16_t gating_max_duration_minutes)446 static void VocAlgorithm__mean_variance_estimator__set_parameters(
447 VocAlgorithmParams* params, fix16_t std_initial,
448 fix16_t tau_mean_variance_hours, fix16_t gating_max_duration_minutes) {
449
450 params->m_Mean_Variance_Estimator__Gating_Max_Duration_Minutes =
451 gating_max_duration_minutes;
452 params->m_Mean_Variance_Estimator___Initialized = false;
453 params->m_Mean_Variance_Estimator___Mean = F16(0.);
454 params->m_Mean_Variance_Estimator___Sraw_Offset = F16(0.);
455 params->m_Mean_Variance_Estimator___Std = std_initial;
456 params->m_Mean_Variance_Estimator___Gamma =
457 (fix16_div(F16((VocAlgorithm_MEAN_VARIANCE_ESTIMATOR__GAMMA_SCALING *
458 (VocAlgorithm_SAMPLING_INTERVAL / 3600.))),
459 (tau_mean_variance_hours +
460 F16((VocAlgorithm_SAMPLING_INTERVAL / 3600.)))));
461 params->m_Mean_Variance_Estimator___Gamma_Initial_Mean =
462 F16(((VocAlgorithm_MEAN_VARIANCE_ESTIMATOR__GAMMA_SCALING *
463 VocAlgorithm_SAMPLING_INTERVAL) /
464 (VocAlgorithm_TAU_INITIAL_MEAN + VocAlgorithm_SAMPLING_INTERVAL)));
465 params->m_Mean_Variance_Estimator___Gamma_Initial_Variance = F16(
466 ((VocAlgorithm_MEAN_VARIANCE_ESTIMATOR__GAMMA_SCALING *
467 VocAlgorithm_SAMPLING_INTERVAL) /
468 (VocAlgorithm_TAU_INITIAL_VARIANCE + VocAlgorithm_SAMPLING_INTERVAL)));
469 params->m_Mean_Variance_Estimator__Gamma_Mean = F16(0.);
470 params->m_Mean_Variance_Estimator__Gamma_Variance = F16(0.);
471 params->m_Mean_Variance_Estimator___Uptime_Gamma = F16(0.);
472 params->m_Mean_Variance_Estimator___Uptime_Gating = F16(0.);
473 params->m_Mean_Variance_Estimator___Gating_Duration_Minutes = F16(0.);
474 }
475
476 static void
VocAlgorithm__mean_variance_estimator__set_states(VocAlgorithmParams * params,fix16_t mean,fix16_t std,fix16_t uptime_gamma)477 VocAlgorithm__mean_variance_estimator__set_states(VocAlgorithmParams* params,
478 fix16_t mean, fix16_t std,
479 fix16_t uptime_gamma) {
480
481 params->m_Mean_Variance_Estimator___Mean = mean;
482 params->m_Mean_Variance_Estimator___Std = std;
483 params->m_Mean_Variance_Estimator___Uptime_Gamma = uptime_gamma;
484 params->m_Mean_Variance_Estimator___Initialized = true;
485 }
486
487 static fix16_t
VocAlgorithm__mean_variance_estimator__get_std(VocAlgorithmParams * params)488 VocAlgorithm__mean_variance_estimator__get_std(VocAlgorithmParams* params) {
489
490 return params->m_Mean_Variance_Estimator___Std;
491 }
492
493 static fix16_t
VocAlgorithm__mean_variance_estimator__get_mean(VocAlgorithmParams * params)494 VocAlgorithm__mean_variance_estimator__get_mean(VocAlgorithmParams* params) {
495
496 return (params->m_Mean_Variance_Estimator___Mean +
497 params->m_Mean_Variance_Estimator___Sraw_Offset);
498 }
499
VocAlgorithm__mean_variance_estimator___calculate_gamma(VocAlgorithmParams * params,fix16_t voc_index_from_prior)500 static void VocAlgorithm__mean_variance_estimator___calculate_gamma(
501 VocAlgorithmParams* params, fix16_t voc_index_from_prior) {
502
503 fix16_t uptime_limit;
504 fix16_t sigmoid_gamma_mean;
505 fix16_t gamma_mean;
506 fix16_t gating_threshold_mean;
507 fix16_t sigmoid_gating_mean;
508 fix16_t sigmoid_gamma_variance;
509 fix16_t gamma_variance;
510 fix16_t gating_threshold_variance;
511 fix16_t sigmoid_gating_variance;
512
513 uptime_limit = F16((VocAlgorithm_MEAN_VARIANCE_ESTIMATOR__FIX16_MAX -
514 VocAlgorithm_SAMPLING_INTERVAL));
515 if ((params->m_Mean_Variance_Estimator___Uptime_Gamma < uptime_limit)) {
516 params->m_Mean_Variance_Estimator___Uptime_Gamma =
517 (params->m_Mean_Variance_Estimator___Uptime_Gamma +
518 F16(VocAlgorithm_SAMPLING_INTERVAL));
519 }
520 if ((params->m_Mean_Variance_Estimator___Uptime_Gating < uptime_limit)) {
521 params->m_Mean_Variance_Estimator___Uptime_Gating =
522 (params->m_Mean_Variance_Estimator___Uptime_Gating +
523 F16(VocAlgorithm_SAMPLING_INTERVAL));
524 }
525 VocAlgorithm__mean_variance_estimator___sigmoid__set_parameters(
526 params, F16(1.), F16(VocAlgorithm_INIT_DURATION_MEAN),
527 F16(VocAlgorithm_INIT_TRANSITION_MEAN));
528 sigmoid_gamma_mean =
529 VocAlgorithm__mean_variance_estimator___sigmoid__process(
530 params, params->m_Mean_Variance_Estimator___Uptime_Gamma);
531 gamma_mean =
532 (params->m_Mean_Variance_Estimator___Gamma +
533 (fix16_mul((params->m_Mean_Variance_Estimator___Gamma_Initial_Mean -
534 params->m_Mean_Variance_Estimator___Gamma),
535 sigmoid_gamma_mean)));
536 gating_threshold_mean =
537 (F16(VocAlgorithm_GATING_THRESHOLD) +
538 (fix16_mul(
539 F16((VocAlgorithm_GATING_THRESHOLD_INITIAL -
540 VocAlgorithm_GATING_THRESHOLD)),
541 VocAlgorithm__mean_variance_estimator___sigmoid__process(
542 params, params->m_Mean_Variance_Estimator___Uptime_Gating))));
543 VocAlgorithm__mean_variance_estimator___sigmoid__set_parameters(
544 params, F16(1.), gating_threshold_mean,
545 F16(VocAlgorithm_GATING_THRESHOLD_TRANSITION));
546 sigmoid_gating_mean =
547 VocAlgorithm__mean_variance_estimator___sigmoid__process(
548 params, voc_index_from_prior);
549 params->m_Mean_Variance_Estimator__Gamma_Mean =
550 (fix16_mul(sigmoid_gating_mean, gamma_mean));
551 VocAlgorithm__mean_variance_estimator___sigmoid__set_parameters(
552 params, F16(1.), F16(VocAlgorithm_INIT_DURATION_VARIANCE),
553 F16(VocAlgorithm_INIT_TRANSITION_VARIANCE));
554 sigmoid_gamma_variance =
555 VocAlgorithm__mean_variance_estimator___sigmoid__process(
556 params, params->m_Mean_Variance_Estimator___Uptime_Gamma);
557 gamma_variance =
558 (params->m_Mean_Variance_Estimator___Gamma +
559 (fix16_mul(
560 (params->m_Mean_Variance_Estimator___Gamma_Initial_Variance -
561 params->m_Mean_Variance_Estimator___Gamma),
562 (sigmoid_gamma_variance - sigmoid_gamma_mean))));
563 gating_threshold_variance =
564 (F16(VocAlgorithm_GATING_THRESHOLD) +
565 (fix16_mul(
566 F16((VocAlgorithm_GATING_THRESHOLD_INITIAL -
567 VocAlgorithm_GATING_THRESHOLD)),
568 VocAlgorithm__mean_variance_estimator___sigmoid__process(
569 params, params->m_Mean_Variance_Estimator___Uptime_Gating))));
570 VocAlgorithm__mean_variance_estimator___sigmoid__set_parameters(
571 params, F16(1.), gating_threshold_variance,
572 F16(VocAlgorithm_GATING_THRESHOLD_TRANSITION));
573 sigmoid_gating_variance =
574 VocAlgorithm__mean_variance_estimator___sigmoid__process(
575 params, voc_index_from_prior);
576 params->m_Mean_Variance_Estimator__Gamma_Variance =
577 (fix16_mul(sigmoid_gating_variance, gamma_variance));
578 params->m_Mean_Variance_Estimator___Gating_Duration_Minutes =
579 (params->m_Mean_Variance_Estimator___Gating_Duration_Minutes +
580 (fix16_mul(F16((VocAlgorithm_SAMPLING_INTERVAL / 60.)),
581 ((fix16_mul((F16(1.) - sigmoid_gating_mean),
582 F16((1. + VocAlgorithm_GATING_MAX_RATIO)))) -
583 F16(VocAlgorithm_GATING_MAX_RATIO)))));
584 if ((params->m_Mean_Variance_Estimator___Gating_Duration_Minutes <
585 F16(0.))) {
586 params->m_Mean_Variance_Estimator___Gating_Duration_Minutes = F16(0.);
587 }
588 if ((params->m_Mean_Variance_Estimator___Gating_Duration_Minutes >
589 params->m_Mean_Variance_Estimator__Gating_Max_Duration_Minutes)) {
590 params->m_Mean_Variance_Estimator___Uptime_Gating = F16(0.);
591 }
592 }
593
VocAlgorithm__mean_variance_estimator__process(VocAlgorithmParams * params,fix16_t sraw,fix16_t voc_index_from_prior)594 static void VocAlgorithm__mean_variance_estimator__process(
595 VocAlgorithmParams* params, fix16_t sraw, fix16_t voc_index_from_prior) {
596
597 fix16_t delta_sgp;
598 fix16_t c;
599 fix16_t additional_scaling;
600
601 if (!params->m_Mean_Variance_Estimator___Initialized) {
602 params->m_Mean_Variance_Estimator___Initialized = true;
603 params->m_Mean_Variance_Estimator___Sraw_Offset = sraw;
604 params->m_Mean_Variance_Estimator___Mean = F16(0.);
605 } else {
606 if (((params->m_Mean_Variance_Estimator___Mean >= F16(100.)) ||
607 (params->m_Mean_Variance_Estimator___Mean <= F16(-100.)))) {
608 params->m_Mean_Variance_Estimator___Sraw_Offset =
609 (params->m_Mean_Variance_Estimator___Sraw_Offset +
610 params->m_Mean_Variance_Estimator___Mean);
611 params->m_Mean_Variance_Estimator___Mean = F16(0.);
612 }
613 sraw = (sraw - params->m_Mean_Variance_Estimator___Sraw_Offset);
614 VocAlgorithm__mean_variance_estimator___calculate_gamma(
615 params, voc_index_from_prior);
616 delta_sgp = (fix16_div(
617 (sraw - params->m_Mean_Variance_Estimator___Mean),
618 F16(VocAlgorithm_MEAN_VARIANCE_ESTIMATOR__GAMMA_SCALING)));
619 if ((delta_sgp < F16(0.))) {
620 c = (params->m_Mean_Variance_Estimator___Std - delta_sgp);
621 } else {
622 c = (params->m_Mean_Variance_Estimator___Std + delta_sgp);
623 }
624 additional_scaling = F16(1.);
625 if ((c > F16(1440.))) {
626 additional_scaling = F16(4.);
627 }
628 params->m_Mean_Variance_Estimator___Std = (fix16_mul(
629 fix16_sqrt((fix16_mul(
630 additional_scaling,
631 (F16(VocAlgorithm_MEAN_VARIANCE_ESTIMATOR__GAMMA_SCALING) -
632 params->m_Mean_Variance_Estimator__Gamma_Variance)))),
633 fix16_sqrt((
634 (fix16_mul(
635 params->m_Mean_Variance_Estimator___Std,
636 (fix16_div(
637 params->m_Mean_Variance_Estimator___Std,
638 (fix16_mul(
639 F16(VocAlgorithm_MEAN_VARIANCE_ESTIMATOR__GAMMA_SCALING),
640 additional_scaling)))))) +
641 (fix16_mul(
642 (fix16_div(
643 (fix16_mul(
644 params->m_Mean_Variance_Estimator__Gamma_Variance,
645 delta_sgp)),
646 additional_scaling)),
647 delta_sgp))))));
648 params->m_Mean_Variance_Estimator___Mean =
649 (params->m_Mean_Variance_Estimator___Mean +
650 (fix16_mul(params->m_Mean_Variance_Estimator__Gamma_Mean,
651 delta_sgp)));
652 }
653 }
654
VocAlgorithm__mean_variance_estimator___sigmoid__init(VocAlgorithmParams * params)655 static void VocAlgorithm__mean_variance_estimator___sigmoid__init(
656 VocAlgorithmParams* params) {
657
658 VocAlgorithm__mean_variance_estimator___sigmoid__set_parameters(
659 params, F16(0.), F16(0.), F16(0.));
660 }
661
VocAlgorithm__mean_variance_estimator___sigmoid__set_parameters(VocAlgorithmParams * params,fix16_t L,fix16_t X0,fix16_t K)662 static void VocAlgorithm__mean_variance_estimator___sigmoid__set_parameters(
663 VocAlgorithmParams* params, fix16_t L, fix16_t X0, fix16_t K) {
664
665 params->m_Mean_Variance_Estimator___Sigmoid__L = L;
666 params->m_Mean_Variance_Estimator___Sigmoid__K = K;
667 params->m_Mean_Variance_Estimator___Sigmoid__X0 = X0;
668 }
669
VocAlgorithm__mean_variance_estimator___sigmoid__process(VocAlgorithmParams * params,fix16_t sample)670 static fix16_t VocAlgorithm__mean_variance_estimator___sigmoid__process(
671 VocAlgorithmParams* params, fix16_t sample) {
672
673 fix16_t x;
674
675 x = (fix16_mul(params->m_Mean_Variance_Estimator___Sigmoid__K,
676 (sample - params->m_Mean_Variance_Estimator___Sigmoid__X0)));
677 if ((x < F16(-50.))) {
678 return params->m_Mean_Variance_Estimator___Sigmoid__L;
679 } else if ((x > F16(50.))) {
680 return F16(0.);
681 } else {
682 return (fix16_div(params->m_Mean_Variance_Estimator___Sigmoid__L,
683 (F16(1.) + fix16_exp(x))));
684 }
685 }
686
VocAlgorithm__mox_model__init(VocAlgorithmParams * params)687 static void VocAlgorithm__mox_model__init(VocAlgorithmParams* params) {
688
689 VocAlgorithm__mox_model__set_parameters(params, F16(1.), F16(0.));
690 }
691
VocAlgorithm__mox_model__set_parameters(VocAlgorithmParams * params,fix16_t SRAW_STD,fix16_t SRAW_MEAN)692 static void VocAlgorithm__mox_model__set_parameters(VocAlgorithmParams* params,
693 fix16_t SRAW_STD,
694 fix16_t SRAW_MEAN) {
695
696 params->m_Mox_Model__Sraw_Std = SRAW_STD;
697 params->m_Mox_Model__Sraw_Mean = SRAW_MEAN;
698 }
699
VocAlgorithm__mox_model__process(VocAlgorithmParams * params,fix16_t sraw)700 static fix16_t VocAlgorithm__mox_model__process(VocAlgorithmParams* params,
701 fix16_t sraw) {
702
703 return (fix16_mul((fix16_div((sraw - params->m_Mox_Model__Sraw_Mean),
704 (-(params->m_Mox_Model__Sraw_Std +
705 F16(VocAlgorithm_SRAW_STD_BONUS))))),
706 F16(VocAlgorithm_VOC_INDEX_GAIN)));
707 }
708
VocAlgorithm__sigmoid_scaled__init(VocAlgorithmParams * params)709 static void VocAlgorithm__sigmoid_scaled__init(VocAlgorithmParams* params) {
710
711 VocAlgorithm__sigmoid_scaled__set_parameters(params, F16(0.));
712 }
713
714 static void
VocAlgorithm__sigmoid_scaled__set_parameters(VocAlgorithmParams * params,fix16_t offset)715 VocAlgorithm__sigmoid_scaled__set_parameters(VocAlgorithmParams* params,
716 fix16_t offset) {
717
718 params->m_Sigmoid_Scaled__Offset = offset;
719 }
720
VocAlgorithm__sigmoid_scaled__process(VocAlgorithmParams * params,fix16_t sample)721 static fix16_t VocAlgorithm__sigmoid_scaled__process(VocAlgorithmParams* params,
722 fix16_t sample) {
723
724 fix16_t x;
725 fix16_t shift;
726
727 x = (fix16_mul(F16(VocAlgorithm_SIGMOID_K),
728 (sample - F16(VocAlgorithm_SIGMOID_X0))));
729 if ((x < F16(-50.))) {
730 return F16(VocAlgorithm_SIGMOID_L);
731 } else if ((x > F16(50.))) {
732 return F16(0.);
733 } else {
734 if ((sample >= F16(0.))) {
735 shift = (fix16_div(
736 (F16(VocAlgorithm_SIGMOID_L) -
737 (fix16_mul(F16(5.), params->m_Sigmoid_Scaled__Offset))),
738 F16(4.)));
739 return ((fix16_div((F16(VocAlgorithm_SIGMOID_L) + shift),
740 (F16(1.) + fix16_exp(x)))) -
741 shift);
742 } else {
743 return (fix16_mul(
744 (fix16_div(params->m_Sigmoid_Scaled__Offset,
745 F16(VocAlgorithm_VOC_INDEX_OFFSET_DEFAULT))),
746 (fix16_div(F16(VocAlgorithm_SIGMOID_L),
747 (F16(1.) + fix16_exp(x))))));
748 }
749 }
750 }
751
VocAlgorithm__adaptive_lowpass__init(VocAlgorithmParams * params)752 static void VocAlgorithm__adaptive_lowpass__init(VocAlgorithmParams* params) {
753
754 VocAlgorithm__adaptive_lowpass__set_parameters(params);
755 }
756
757 static void
VocAlgorithm__adaptive_lowpass__set_parameters(VocAlgorithmParams * params)758 VocAlgorithm__adaptive_lowpass__set_parameters(VocAlgorithmParams* params) {
759
760 params->m_Adaptive_Lowpass__A1 =
761 F16((VocAlgorithm_SAMPLING_INTERVAL /
762 (VocAlgorithm_LP_TAU_FAST + VocAlgorithm_SAMPLING_INTERVAL)));
763 params->m_Adaptive_Lowpass__A2 =
764 F16((VocAlgorithm_SAMPLING_INTERVAL /
765 (VocAlgorithm_LP_TAU_SLOW + VocAlgorithm_SAMPLING_INTERVAL)));
766 params->m_Adaptive_Lowpass___Initialized = false;
767 }
768
769 static fix16_t
VocAlgorithm__adaptive_lowpass__process(VocAlgorithmParams * params,fix16_t sample)770 VocAlgorithm__adaptive_lowpass__process(VocAlgorithmParams* params,
771 fix16_t sample) {
772
773 fix16_t abs_delta;
774 fix16_t F1;
775 fix16_t tau_a;
776 fix16_t a3;
777
778 if (!params->m_Adaptive_Lowpass___Initialized) {
779 params->m_Adaptive_Lowpass___X1 = sample;
780 params->m_Adaptive_Lowpass___X2 = sample;
781 params->m_Adaptive_Lowpass___X3 = sample;
782 params->m_Adaptive_Lowpass___Initialized = true;
783 }
784 params->m_Adaptive_Lowpass___X1 =
785 ((fix16_mul((F16(1.) - params->m_Adaptive_Lowpass__A1),
786 params->m_Adaptive_Lowpass___X1)) +
787 (fix16_mul(params->m_Adaptive_Lowpass__A1, sample)));
788 params->m_Adaptive_Lowpass___X2 =
789 ((fix16_mul((F16(1.) - params->m_Adaptive_Lowpass__A2),
790 params->m_Adaptive_Lowpass___X2)) +
791 (fix16_mul(params->m_Adaptive_Lowpass__A2, sample)));
792 abs_delta =
793 (params->m_Adaptive_Lowpass___X1 - params->m_Adaptive_Lowpass___X2);
794 if ((abs_delta < F16(0.))) {
795 abs_delta = (-abs_delta);
796 }
797 F1 = fix16_exp((fix16_mul(F16(VocAlgorithm_LP_ALPHA), abs_delta)));
798 tau_a =
799 ((fix16_mul(F16((VocAlgorithm_LP_TAU_SLOW - VocAlgorithm_LP_TAU_FAST)),
800 F1)) +
801 F16(VocAlgorithm_LP_TAU_FAST));
802 a3 = (fix16_div(F16(VocAlgorithm_SAMPLING_INTERVAL),
803 (F16(VocAlgorithm_SAMPLING_INTERVAL) + tau_a)));
804 params->m_Adaptive_Lowpass___X3 =
805 ((fix16_mul((F16(1.) - a3), params->m_Adaptive_Lowpass___X3)) +
806 (fix16_mul(a3, sample)));
807 return params->m_Adaptive_Lowpass___X3;
808 }
809