/* $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;
}
