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