/* $NetBSD: sensirion_voc_algorithm.c,v 1.2 2021/10/18 14:14:07 christos Exp $ */ /* * Copyright (c) 2021, Sensirion AG * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. * * * Neither the name of Sensirion AG nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include "sensirion_voc_algorithm.h" /* The fixed point arithmetic parts of this code were originally created by * https://github.com/PetteriAimonen/libfixmath */ /*!< the maximum value of fix16_t */ #define FIX16_MAXIMUM 0x7FFFFFFF /*!< the minimum value of fix16_t */ #define FIX16_MINIMUM 0x80000000 /*!< the value used to indicate overflows when FIXMATH_NO_OVERFLOW is not * specified */ #define FIX16_OVERFLOW 0x80000000 /*!< fix16_t value of 1 */ #define FIX16_ONE 0x00010000 static inline fix16_t fix16_from_int(int32_t a) { return a * FIX16_ONE; } static inline int32_t fix16_cast_to_int(fix16_t a) { return (a >= 0) ? (a >> 16) : -((-a) >> 16); } /*! Multiplies the two given fix16_t's and returns the result. */ static fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1); /*! Divides the first given fix16_t by the second and returns the result. */ static fix16_t fix16_div(fix16_t inArg0, fix16_t inArg1); /*! Returns the square root of the given fix16_t. */ static fix16_t fix16_sqrt(fix16_t inValue); /*! Returns the exponent (e^) of the given fix16_t. */ static fix16_t fix16_exp(fix16_t inValue); static fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1) { // Each argument is divided to 16-bit parts. // AB // * CD // ----------- // BD 16 * 16 -> 32 bit products // CB // AD // AC // |----| 64 bit product uint32_t absArg0 = (uint32_t)((inArg0 >= 0) ? inArg0 : (-inArg0)); uint32_t absArg1 = (uint32_t)((inArg1 >= 0) ? inArg1 : (-inArg1)); uint32_t A = (absArg0 >> 16), C = (absArg1 >> 16); uint32_t B = (absArg0 & 0xFFFF), D = (absArg1 & 0xFFFF); uint32_t AC = A * C; uint32_t AD_CB = A * D + C * B; uint32_t BD = B * D; uint32_t product_hi = AC + (AD_CB >> 16); // Handle carry from lower 32 bits to upper part of result. uint32_t ad_cb_temp = AD_CB << 16; uint32_t product_lo = BD + ad_cb_temp; if (product_lo < BD) product_hi++; #ifndef FIXMATH_NO_OVERFLOW // The upper 17 bits should all be zero. if (product_hi >> 15) return (fix16_t)FIX16_OVERFLOW; #endif #ifdef FIXMATH_NO_ROUNDING fix16_t result = (fix16_t)((product_hi << 16) | (product_lo >> 16)); if ((inArg0 < 0) != (inArg1 < 0)) result = -result; return result; #else // Adding 0x8000 (= 0.5) and then using right shift // achieves proper rounding to result. // Handle carry from lower to upper part. uint32_t product_lo_tmp = product_lo; product_lo += 0x8000; if (product_lo < product_lo_tmp) product_hi++; // Discard the lowest 16 bits and convert back to signed result. fix16_t result = (fix16_t)((product_hi << 16) | (product_lo >> 16)); if ((inArg0 < 0) != (inArg1 < 0)) result = -result; return result; #endif } static fix16_t fix16_div(fix16_t a, fix16_t b) { // This uses the basic binary restoring division algorithm. // It appears to be faster to do the whole division manually than // trying to compose a 64-bit divide out of 32-bit divisions on // platforms without hardware divide. if (b == 0) return (fix16_t)FIX16_MINIMUM; uint32_t remainder = (uint32_t)((a >= 0) ? a : (-a)); uint32_t divider = (uint32_t)((b >= 0) ? b : (-b)); uint32_t quotient = 0; uint32_t bit = 0x10000; /* The algorithm requires D >= R */ while (divider < remainder) { divider <<= 1; bit <<= 1; } #ifndef FIXMATH_NO_OVERFLOW if (!bit) return (fix16_t)FIX16_OVERFLOW; #endif if (divider & 0x80000000) { // Perform one step manually to avoid overflows later. // We know that divider's bottom bit is 0 here. if (remainder >= divider) { quotient |= bit; remainder -= divider; } divider >>= 1; bit >>= 1; } /* Main division loop */ while (bit && remainder) { if (remainder >= divider) { quotient |= bit; remainder -= divider; } remainder <<= 1; bit >>= 1; } #ifndef FIXMATH_NO_ROUNDING if (remainder >= divider) { quotient++; } #endif fix16_t result = (fix16_t)quotient; /* Figure out the sign of result */ if ((a < 0) != (b < 0)) { #ifndef FIXMATH_NO_OVERFLOW if (result == FIX16_MINIMUM) return (fix16_t)FIX16_OVERFLOW; #endif result = -result; } return result; } static fix16_t fix16_sqrt(fix16_t x) { // It is assumed that x is not negative uint32_t num = (uint32_t)x; uint32_t result = 0; uint32_t bit; uint8_t n; bit = (uint32_t)1 << 30; while (bit > num) bit >>= 2; // The main part is executed twice, in order to avoid // using 64 bit values in computations. for (n = 0; n < 2; n++) { // First we get the top 24 bits of the answer. while (bit) { if (num >= result + bit) { num -= result + bit; result = (result >> 1) + bit; } else { result = (result >> 1); } bit >>= 2; } if (n == 0) { // Then process it again to get the lowest 8 bits. if (num > 65535) { // The remainder 'num' is too large to be shifted left // by 16, so we have to add 1 to result manually and // adjust 'num' accordingly. // num = a - (result + 0.5)^2 // = num + result^2 - (result + 0.5)^2 // = num - result - 0.5 num -= result; num = (num << 16) - 0x8000; result = (result << 16) + 0x8000; } else { num <<= 16; result <<= 16; } bit = 1 << 14; } } #ifndef FIXMATH_NO_ROUNDING // Finally, if next bit would have been 1, round the result upwards. if (num > result) { result++; } #endif return (fix16_t)result; } static fix16_t fix16_exp(fix16_t x) { // Function to approximate exp(); optimized more for code size than speed // exp(x) for x = +/- {1, 1/8, 1/64, 1/512} #define NUM_EXP_VALUES 4 static const fix16_t exp_pos_values[NUM_EXP_VALUES] = { F16(2.7182818), F16(1.1331485), F16(1.0157477), F16(1.0019550)}; static const fix16_t exp_neg_values[NUM_EXP_VALUES] = { F16(0.3678794), F16(0.8824969), F16(0.9844964), F16(0.9980488)}; const fix16_t* exp_values; fix16_t res, arg; uint16_t i; if (x >= F16(10.3972)) return FIX16_MAXIMUM; if (x <= F16(-11.7835)) return 0; if (x < 0) { x = -x; exp_values = exp_neg_values; } else { exp_values = exp_pos_values; } res = FIX16_ONE; arg = FIX16_ONE; for (i = 0; i < NUM_EXP_VALUES; i++) { while (x >= arg) { res = fix16_mul(res, exp_values[i]); x -= arg; } arg >>= 3; } return res; } static void VocAlgorithm__init_instances(VocAlgorithmParams* params); static void VocAlgorithm__mean_variance_estimator__init(VocAlgorithmParams* params); static void VocAlgorithm__mean_variance_estimator___init_instances( VocAlgorithmParams* params); static void VocAlgorithm__mean_variance_estimator__set_parameters( VocAlgorithmParams* params, fix16_t std_initial, fix16_t tau_mean_variance_hours, fix16_t gating_max_duration_minutes); static void VocAlgorithm__mean_variance_estimator__set_states(VocAlgorithmParams* params, fix16_t mean, fix16_t std, fix16_t uptime_gamma); static fix16_t VocAlgorithm__mean_variance_estimator__get_std(VocAlgorithmParams* params); static fix16_t VocAlgorithm__mean_variance_estimator__get_mean(VocAlgorithmParams* params); static void VocAlgorithm__mean_variance_estimator___calculate_gamma( VocAlgorithmParams* params, fix16_t voc_index_from_prior); static void VocAlgorithm__mean_variance_estimator__process( VocAlgorithmParams* params, fix16_t sraw, fix16_t voc_index_from_prior); static void VocAlgorithm__mean_variance_estimator___sigmoid__init( VocAlgorithmParams* params); static void VocAlgorithm__mean_variance_estimator___sigmoid__set_parameters( VocAlgorithmParams* params, fix16_t L, fix16_t X0, fix16_t K); static fix16_t VocAlgorithm__mean_variance_estimator___sigmoid__process( VocAlgorithmParams* params, fix16_t sample); static void VocAlgorithm__mox_model__init(VocAlgorithmParams* params); static void VocAlgorithm__mox_model__set_parameters(VocAlgorithmParams* params, fix16_t SRAW_STD, fix16_t SRAW_MEAN); static fix16_t VocAlgorithm__mox_model__process(VocAlgorithmParams* params, fix16_t sraw); static void VocAlgorithm__sigmoid_scaled__init(VocAlgorithmParams* params); static void VocAlgorithm__sigmoid_scaled__set_parameters(VocAlgorithmParams* params, fix16_t offset); static fix16_t VocAlgorithm__sigmoid_scaled__process(VocAlgorithmParams* params, fix16_t sample); static void VocAlgorithm__adaptive_lowpass__init(VocAlgorithmParams* params); static void VocAlgorithm__adaptive_lowpass__set_parameters(VocAlgorithmParams* params); static fix16_t VocAlgorithm__adaptive_lowpass__process(VocAlgorithmParams* params, fix16_t sample); void VocAlgorithm_init(VocAlgorithmParams* params) { params->mVoc_Index_Offset = F16(VocAlgorithm_VOC_INDEX_OFFSET_DEFAULT); params->mTau_Mean_Variance_Hours = F16(VocAlgorithm_TAU_MEAN_VARIANCE_HOURS); params->mGating_Max_Duration_Minutes = F16(VocAlgorithm_GATING_MAX_DURATION_MINUTES); params->mSraw_Std_Initial = F16(VocAlgorithm_SRAW_STD_INITIAL); params->mUptime = F16(0.); params->mSraw = F16(0.); params->mVoc_Index = 0; VocAlgorithm__init_instances(params); } static void VocAlgorithm__init_instances(VocAlgorithmParams* params) { VocAlgorithm__mean_variance_estimator__init(params); VocAlgorithm__mean_variance_estimator__set_parameters( params, params->mSraw_Std_Initial, params->mTau_Mean_Variance_Hours, params->mGating_Max_Duration_Minutes); VocAlgorithm__mox_model__init(params); VocAlgorithm__mox_model__set_parameters( params, VocAlgorithm__mean_variance_estimator__get_std(params), VocAlgorithm__mean_variance_estimator__get_mean(params)); VocAlgorithm__sigmoid_scaled__init(params); VocAlgorithm__sigmoid_scaled__set_parameters(params, params->mVoc_Index_Offset); VocAlgorithm__adaptive_lowpass__init(params); VocAlgorithm__adaptive_lowpass__set_parameters(params); } void VocAlgorithm_get_states(VocAlgorithmParams* params, int32_t* state0, int32_t* state1) { *state0 = VocAlgorithm__mean_variance_estimator__get_mean(params); *state1 = VocAlgorithm__mean_variance_estimator__get_std(params); return; } void VocAlgorithm_set_states(VocAlgorithmParams* params, int32_t state0, int32_t state1) { VocAlgorithm__mean_variance_estimator__set_states( params, state0, state1, F16(VocAlgorithm_PERSISTENCE_UPTIME_GAMMA)); params->mSraw = state0; } void 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) { params->mVoc_Index_Offset = (fix16_from_int(voc_index_offset)); params->mTau_Mean_Variance_Hours = (fix16_from_int(learning_time_hours)); params->mGating_Max_Duration_Minutes = (fix16_from_int(gating_max_duration_minutes)); params->mSraw_Std_Initial = (fix16_from_int(std_initial)); VocAlgorithm__init_instances(params); } void VocAlgorithm_process(VocAlgorithmParams* params, int32_t sraw, int32_t* voc_index) { if ((params->mUptime <= F16(VocAlgorithm_INITIAL_BLACKOUT))) { params->mUptime = (params->mUptime + F16(VocAlgorithm_SAMPLING_INTERVAL)); } else { if (((sraw > 0) && (sraw < 65000))) { if ((sraw < 20001)) { sraw = 20001; } else if ((sraw > 52767)) { sraw = 52767; } params->mSraw = (fix16_from_int((sraw - 20000))); } params->mVoc_Index = VocAlgorithm__mox_model__process(params, params->mSraw); params->mVoc_Index = VocAlgorithm__sigmoid_scaled__process(params, params->mVoc_Index); params->mVoc_Index = VocAlgorithm__adaptive_lowpass__process(params, params->mVoc_Index); if ((params->mVoc_Index < F16(0.5))) { params->mVoc_Index = F16(0.5); } if ((params->mSraw > F16(0.))) { VocAlgorithm__mean_variance_estimator__process( params, params->mSraw, params->mVoc_Index); VocAlgorithm__mox_model__set_parameters( params, VocAlgorithm__mean_variance_estimator__get_std(params), VocAlgorithm__mean_variance_estimator__get_mean(params)); } } *voc_index = (fix16_cast_to_int((params->mVoc_Index + F16(0.5)))); return; } static void VocAlgorithm__mean_variance_estimator__init(VocAlgorithmParams* params) { VocAlgorithm__mean_variance_estimator__set_parameters(params, F16(0.), F16(0.), F16(0.)); VocAlgorithm__mean_variance_estimator___init_instances(params); } static void VocAlgorithm__mean_variance_estimator___init_instances( VocAlgorithmParams* params) { VocAlgorithm__mean_variance_estimator___sigmoid__init(params); } static void VocAlgorithm__mean_variance_estimator__set_parameters( VocAlgorithmParams* params, fix16_t std_initial, fix16_t tau_mean_variance_hours, fix16_t gating_max_duration_minutes) { params->m_Mean_Variance_Estimator__Gating_Max_Duration_Minutes = gating_max_duration_minutes; params->m_Mean_Variance_Estimator___Initialized = false; params->m_Mean_Variance_Estimator___Mean = F16(0.); params->m_Mean_Variance_Estimator___Sraw_Offset = F16(0.); params->m_Mean_Variance_Estimator___Std = std_initial; params->m_Mean_Variance_Estimator___Gamma = (fix16_div(F16((VocAlgorithm_MEAN_VARIANCE_ESTIMATOR__GAMMA_SCALING * (VocAlgorithm_SAMPLING_INTERVAL / 3600.))), (tau_mean_variance_hours + F16((VocAlgorithm_SAMPLING_INTERVAL / 3600.))))); params->m_Mean_Variance_Estimator___Gamma_Initial_Mean = F16(((VocAlgorithm_MEAN_VARIANCE_ESTIMATOR__GAMMA_SCALING * VocAlgorithm_SAMPLING_INTERVAL) / (VocAlgorithm_TAU_INITIAL_MEAN + VocAlgorithm_SAMPLING_INTERVAL))); params->m_Mean_Variance_Estimator___Gamma_Initial_Variance = F16( ((VocAlgorithm_MEAN_VARIANCE_ESTIMATOR__GAMMA_SCALING * VocAlgorithm_SAMPLING_INTERVAL) / (VocAlgorithm_TAU_INITIAL_VARIANCE + VocAlgorithm_SAMPLING_INTERVAL))); params->m_Mean_Variance_Estimator__Gamma_Mean = F16(0.); params->m_Mean_Variance_Estimator__Gamma_Variance = F16(0.); params->m_Mean_Variance_Estimator___Uptime_Gamma = F16(0.); params->m_Mean_Variance_Estimator___Uptime_Gating = F16(0.); params->m_Mean_Variance_Estimator___Gating_Duration_Minutes = F16(0.); } static void VocAlgorithm__mean_variance_estimator__set_states(VocAlgorithmParams* params, fix16_t mean, fix16_t std, fix16_t uptime_gamma) { params->m_Mean_Variance_Estimator___Mean = mean; params->m_Mean_Variance_Estimator___Std = std; params->m_Mean_Variance_Estimator___Uptime_Gamma = uptime_gamma; params->m_Mean_Variance_Estimator___Initialized = true; } static fix16_t VocAlgorithm__mean_variance_estimator__get_std(VocAlgorithmParams* params) { return params->m_Mean_Variance_Estimator___Std; } static fix16_t VocAlgorithm__mean_variance_estimator__get_mean(VocAlgorithmParams* params) { return (params->m_Mean_Variance_Estimator___Mean + params->m_Mean_Variance_Estimator___Sraw_Offset); } static void VocAlgorithm__mean_variance_estimator___calculate_gamma( VocAlgorithmParams* params, fix16_t voc_index_from_prior) { fix16_t uptime_limit; fix16_t sigmoid_gamma_mean; fix16_t gamma_mean; fix16_t gating_threshold_mean; fix16_t sigmoid_gating_mean; fix16_t sigmoid_gamma_variance; fix16_t gamma_variance; fix16_t gating_threshold_variance; fix16_t sigmoid_gating_variance; uptime_limit = F16((VocAlgorithm_MEAN_VARIANCE_ESTIMATOR__FIX16_MAX - VocAlgorithm_SAMPLING_INTERVAL)); if ((params->m_Mean_Variance_Estimator___Uptime_Gamma < uptime_limit)) { params->m_Mean_Variance_Estimator___Uptime_Gamma = (params->m_Mean_Variance_Estimator___Uptime_Gamma + F16(VocAlgorithm_SAMPLING_INTERVAL)); } if ((params->m_Mean_Variance_Estimator___Uptime_Gating < uptime_limit)) { params->m_Mean_Variance_Estimator___Uptime_Gating = (params->m_Mean_Variance_Estimator___Uptime_Gating + F16(VocAlgorithm_SAMPLING_INTERVAL)); } VocAlgorithm__mean_variance_estimator___sigmoid__set_parameters( params, F16(1.), F16(VocAlgorithm_INIT_DURATION_MEAN), F16(VocAlgorithm_INIT_TRANSITION_MEAN)); sigmoid_gamma_mean = VocAlgorithm__mean_variance_estimator___sigmoid__process( params, params->m_Mean_Variance_Estimator___Uptime_Gamma); gamma_mean = (params->m_Mean_Variance_Estimator___Gamma + (fix16_mul((params->m_Mean_Variance_Estimator___Gamma_Initial_Mean - params->m_Mean_Variance_Estimator___Gamma), sigmoid_gamma_mean))); gating_threshold_mean = (F16(VocAlgorithm_GATING_THRESHOLD) + (fix16_mul( F16((VocAlgorithm_GATING_THRESHOLD_INITIAL - VocAlgorithm_GATING_THRESHOLD)), VocAlgorithm__mean_variance_estimator___sigmoid__process( params, params->m_Mean_Variance_Estimator___Uptime_Gating)))); VocAlgorithm__mean_variance_estimator___sigmoid__set_parameters( params, F16(1.), gating_threshold_mean, F16(VocAlgorithm_GATING_THRESHOLD_TRANSITION)); sigmoid_gating_mean = VocAlgorithm__mean_variance_estimator___sigmoid__process( params, voc_index_from_prior); params->m_Mean_Variance_Estimator__Gamma_Mean = (fix16_mul(sigmoid_gating_mean, gamma_mean)); VocAlgorithm__mean_variance_estimator___sigmoid__set_parameters( params, F16(1.), F16(VocAlgorithm_INIT_DURATION_VARIANCE), F16(VocAlgorithm_INIT_TRANSITION_VARIANCE)); sigmoid_gamma_variance = VocAlgorithm__mean_variance_estimator___sigmoid__process( params, params->m_Mean_Variance_Estimator___Uptime_Gamma); gamma_variance = (params->m_Mean_Variance_Estimator___Gamma + (fix16_mul( (params->m_Mean_Variance_Estimator___Gamma_Initial_Variance - params->m_Mean_Variance_Estimator___Gamma), (sigmoid_gamma_variance - sigmoid_gamma_mean)))); gating_threshold_variance = (F16(VocAlgorithm_GATING_THRESHOLD) + (fix16_mul( F16((VocAlgorithm_GATING_THRESHOLD_INITIAL - VocAlgorithm_GATING_THRESHOLD)), VocAlgorithm__mean_variance_estimator___sigmoid__process( params, params->m_Mean_Variance_Estimator___Uptime_Gating)))); VocAlgorithm__mean_variance_estimator___sigmoid__set_parameters( params, F16(1.), gating_threshold_variance, F16(VocAlgorithm_GATING_THRESHOLD_TRANSITION)); sigmoid_gating_variance = VocAlgorithm__mean_variance_estimator___sigmoid__process( params, voc_index_from_prior); params->m_Mean_Variance_Estimator__Gamma_Variance = (fix16_mul(sigmoid_gating_variance, gamma_variance)); params->m_Mean_Variance_Estimator___Gating_Duration_Minutes = (params->m_Mean_Variance_Estimator___Gating_Duration_Minutes + (fix16_mul(F16((VocAlgorithm_SAMPLING_INTERVAL / 60.)), ((fix16_mul((F16(1.) - sigmoid_gating_mean), F16((1. + VocAlgorithm_GATING_MAX_RATIO)))) - F16(VocAlgorithm_GATING_MAX_RATIO))))); if ((params->m_Mean_Variance_Estimator___Gating_Duration_Minutes < F16(0.))) { params->m_Mean_Variance_Estimator___Gating_Duration_Minutes = F16(0.); } if ((params->m_Mean_Variance_Estimator___Gating_Duration_Minutes > params->m_Mean_Variance_Estimator__Gating_Max_Duration_Minutes)) { params->m_Mean_Variance_Estimator___Uptime_Gating = F16(0.); } } static void VocAlgorithm__mean_variance_estimator__process( VocAlgorithmParams* params, fix16_t sraw, fix16_t voc_index_from_prior) { fix16_t delta_sgp; fix16_t c; fix16_t additional_scaling; if (!params->m_Mean_Variance_Estimator___Initialized) { params->m_Mean_Variance_Estimator___Initialized = true; params->m_Mean_Variance_Estimator___Sraw_Offset = sraw; params->m_Mean_Variance_Estimator___Mean = F16(0.); } else { if (((params->m_Mean_Variance_Estimator___Mean >= F16(100.)) || (params->m_Mean_Variance_Estimator___Mean <= F16(-100.)))) { params->m_Mean_Variance_Estimator___Sraw_Offset = (params->m_Mean_Variance_Estimator___Sraw_Offset + params->m_Mean_Variance_Estimator___Mean); params->m_Mean_Variance_Estimator___Mean = F16(0.); } sraw = (sraw - params->m_Mean_Variance_Estimator___Sraw_Offset); VocAlgorithm__mean_variance_estimator___calculate_gamma( params, voc_index_from_prior); delta_sgp = (fix16_div( (sraw - params->m_Mean_Variance_Estimator___Mean), F16(VocAlgorithm_MEAN_VARIANCE_ESTIMATOR__GAMMA_SCALING))); if ((delta_sgp < F16(0.))) { c = (params->m_Mean_Variance_Estimator___Std - delta_sgp); } else { c = (params->m_Mean_Variance_Estimator___Std + delta_sgp); } additional_scaling = F16(1.); if ((c > F16(1440.))) { additional_scaling = F16(4.); } params->m_Mean_Variance_Estimator___Std = (fix16_mul( fix16_sqrt((fix16_mul( additional_scaling, (F16(VocAlgorithm_MEAN_VARIANCE_ESTIMATOR__GAMMA_SCALING) - params->m_Mean_Variance_Estimator__Gamma_Variance)))), fix16_sqrt(( (fix16_mul( params->m_Mean_Variance_Estimator___Std, (fix16_div( params->m_Mean_Variance_Estimator___Std, (fix16_mul( F16(VocAlgorithm_MEAN_VARIANCE_ESTIMATOR__GAMMA_SCALING), additional_scaling)))))) + (fix16_mul( (fix16_div( (fix16_mul( params->m_Mean_Variance_Estimator__Gamma_Variance, delta_sgp)), additional_scaling)), delta_sgp)))))); params->m_Mean_Variance_Estimator___Mean = (params->m_Mean_Variance_Estimator___Mean + (fix16_mul(params->m_Mean_Variance_Estimator__Gamma_Mean, delta_sgp))); } } static void VocAlgorithm__mean_variance_estimator___sigmoid__init( VocAlgorithmParams* params) { VocAlgorithm__mean_variance_estimator___sigmoid__set_parameters( params, F16(0.), F16(0.), F16(0.)); } static void VocAlgorithm__mean_variance_estimator___sigmoid__set_parameters( VocAlgorithmParams* params, fix16_t L, fix16_t X0, fix16_t K) { params->m_Mean_Variance_Estimator___Sigmoid__L = L; params->m_Mean_Variance_Estimator___Sigmoid__K = K; params->m_Mean_Variance_Estimator___Sigmoid__X0 = X0; } static fix16_t VocAlgorithm__mean_variance_estimator___sigmoid__process( VocAlgorithmParams* params, fix16_t sample) { fix16_t x; x = (fix16_mul(params->m_Mean_Variance_Estimator___Sigmoid__K, (sample - params->m_Mean_Variance_Estimator___Sigmoid__X0))); if ((x < F16(-50.))) { return params->m_Mean_Variance_Estimator___Sigmoid__L; } else if ((x > F16(50.))) { return F16(0.); } else { return (fix16_div(params->m_Mean_Variance_Estimator___Sigmoid__L, (F16(1.) + fix16_exp(x)))); } } static void VocAlgorithm__mox_model__init(VocAlgorithmParams* params) { VocAlgorithm__mox_model__set_parameters(params, F16(1.), F16(0.)); } static void VocAlgorithm__mox_model__set_parameters(VocAlgorithmParams* params, fix16_t SRAW_STD, fix16_t SRAW_MEAN) { params->m_Mox_Model__Sraw_Std = SRAW_STD; params->m_Mox_Model__Sraw_Mean = SRAW_MEAN; } static fix16_t VocAlgorithm__mox_model__process(VocAlgorithmParams* params, fix16_t sraw) { return (fix16_mul((fix16_div((sraw - params->m_Mox_Model__Sraw_Mean), (-(params->m_Mox_Model__Sraw_Std + F16(VocAlgorithm_SRAW_STD_BONUS))))), F16(VocAlgorithm_VOC_INDEX_GAIN))); } static void VocAlgorithm__sigmoid_scaled__init(VocAlgorithmParams* params) { VocAlgorithm__sigmoid_scaled__set_parameters(params, F16(0.)); } static void VocAlgorithm__sigmoid_scaled__set_parameters(VocAlgorithmParams* params, fix16_t offset) { params->m_Sigmoid_Scaled__Offset = offset; } static fix16_t VocAlgorithm__sigmoid_scaled__process(VocAlgorithmParams* params, fix16_t sample) { fix16_t x; fix16_t shift; x = (fix16_mul(F16(VocAlgorithm_SIGMOID_K), (sample - F16(VocAlgorithm_SIGMOID_X0)))); if ((x < F16(-50.))) { return F16(VocAlgorithm_SIGMOID_L); } else if ((x > F16(50.))) { return F16(0.); } else { if ((sample >= F16(0.))) { shift = (fix16_div( (F16(VocAlgorithm_SIGMOID_L) - (fix16_mul(F16(5.), params->m_Sigmoid_Scaled__Offset))), F16(4.))); return ((fix16_div((F16(VocAlgorithm_SIGMOID_L) + shift), (F16(1.) + fix16_exp(x)))) - shift); } else { return (fix16_mul( (fix16_div(params->m_Sigmoid_Scaled__Offset, F16(VocAlgorithm_VOC_INDEX_OFFSET_DEFAULT))), (fix16_div(F16(VocAlgorithm_SIGMOID_L), (F16(1.) + fix16_exp(x)))))); } } } static void VocAlgorithm__adaptive_lowpass__init(VocAlgorithmParams* params) { VocAlgorithm__adaptive_lowpass__set_parameters(params); } static void VocAlgorithm__adaptive_lowpass__set_parameters(VocAlgorithmParams* params) { params->m_Adaptive_Lowpass__A1 = F16((VocAlgorithm_SAMPLING_INTERVAL / (VocAlgorithm_LP_TAU_FAST + VocAlgorithm_SAMPLING_INTERVAL))); params->m_Adaptive_Lowpass__A2 = F16((VocAlgorithm_SAMPLING_INTERVAL / (VocAlgorithm_LP_TAU_SLOW + VocAlgorithm_SAMPLING_INTERVAL))); params->m_Adaptive_Lowpass___Initialized = false; } static fix16_t VocAlgorithm__adaptive_lowpass__process(VocAlgorithmParams* params, fix16_t sample) { fix16_t abs_delta; fix16_t F1; fix16_t tau_a; fix16_t a3; if (!params->m_Adaptive_Lowpass___Initialized) { params->m_Adaptive_Lowpass___X1 = sample; params->m_Adaptive_Lowpass___X2 = sample; params->m_Adaptive_Lowpass___X3 = sample; params->m_Adaptive_Lowpass___Initialized = true; } params->m_Adaptive_Lowpass___X1 = ((fix16_mul((F16(1.) - params->m_Adaptive_Lowpass__A1), params->m_Adaptive_Lowpass___X1)) + (fix16_mul(params->m_Adaptive_Lowpass__A1, sample))); params->m_Adaptive_Lowpass___X2 = ((fix16_mul((F16(1.) - params->m_Adaptive_Lowpass__A2), params->m_Adaptive_Lowpass___X2)) + (fix16_mul(params->m_Adaptive_Lowpass__A2, sample))); abs_delta = (params->m_Adaptive_Lowpass___X1 - params->m_Adaptive_Lowpass___X2); if ((abs_delta < F16(0.))) { abs_delta = (-abs_delta); } F1 = fix16_exp((fix16_mul(F16(VocAlgorithm_LP_ALPHA), abs_delta))); tau_a = ((fix16_mul(F16((VocAlgorithm_LP_TAU_SLOW - VocAlgorithm_LP_TAU_FAST)), F1)) + F16(VocAlgorithm_LP_TAU_FAST)); a3 = (fix16_div(F16(VocAlgorithm_SAMPLING_INTERVAL), (F16(VocAlgorithm_SAMPLING_INTERVAL) + tau_a))); params->m_Adaptive_Lowpass___X3 = ((fix16_mul((F16(1.) - a3), params->m_Adaptive_Lowpass___X3)) + (fix16_mul(a3, sample))); return params->m_Adaptive_Lowpass___X3; }