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 51 static inline fix16_t fix16_from_int(int32_t a) { 52 return a * FIX16_ONE; 53 } 54 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 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 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 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 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 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 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 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 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 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 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 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 440 static void VocAlgorithm__mean_variance_estimator___init_instances( 441 VocAlgorithmParams* params) { 442 443 VocAlgorithm__mean_variance_estimator___sigmoid__init(params); 444 } 445 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 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 488 VocAlgorithm__mean_variance_estimator__get_std(VocAlgorithmParams* params) { 489 490 return params->m_Mean_Variance_Estimator___Std; 491 } 492 493 static fix16_t 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 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 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 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 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 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 687 static void VocAlgorithm__mox_model__init(VocAlgorithmParams* params) { 688 689 VocAlgorithm__mox_model__set_parameters(params, F16(1.), F16(0.)); 690 } 691 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 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 709 static void VocAlgorithm__sigmoid_scaled__init(VocAlgorithmParams* params) { 710 711 VocAlgorithm__sigmoid_scaled__set_parameters(params, F16(0.)); 712 } 713 714 static void 715 VocAlgorithm__sigmoid_scaled__set_parameters(VocAlgorithmParams* params, 716 fix16_t offset) { 717 718 params->m_Sigmoid_Scaled__Offset = offset; 719 } 720 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 752 static void VocAlgorithm__adaptive_lowpass__init(VocAlgorithmParams* params) { 753 754 VocAlgorithm__adaptive_lowpass__set_parameters(params); 755 } 756 757 static void 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 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
Indexes created Sun Sep 21 20:09:37 GMT 2025