Add bh1750, aht20, sgp40 drivers unified on native i2c_master API
Forked from managed_components (espressif/bh1750, espressif/aht20, esp-idf-lib/sgp40) and rewrote aht20/sgp40 internals to use driver/i2c_master.h directly instead of the i2c_bus and i2cdev wrapper components, so all three sensors share one create/delete handle pattern over a single i2c_master_bus_handle_t.
This commit is contained in:
Julien Lazarewicz
898
sgp40/sensirion_voc_algorithm.c
Normal file
898
sgp40/sensirion_voc_algorithm.c
Normal file
@@ -0,0 +1,898 @@
|
||||
/*
|
||||
* Copyright (c) 2020, 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
|
||||
|
||||
inline fix16_t fix16_from_int(int32_t a)
|
||||
{
|
||||
return a * FIX16_ONE;
|
||||
}
|
||||
|
||||
inline int32_t fix16_cast_to_int(fix16_t a)
|
||||
{
|
||||
return (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
|
||||
int32_t A = (inArg0 >> 16), C = (inArg1 >> 16);
|
||||
uint32_t B = (inArg0 & 0xFFFF), D = (inArg1 & 0xFFFF);
|
||||
|
||||
int32_t AC = A * C;
|
||||
int32_t AD_CB = A * D + C * B;
|
||||
uint32_t BD = B * D;
|
||||
|
||||
int32_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 the same (the sign).
|
||||
if (product_hi >> 31 != product_hi >> 15)
|
||||
return FIX16_OVERFLOW;
|
||||
#endif
|
||||
|
||||
#ifdef FIXMATH_NO_ROUNDING
|
||||
return (product_hi << 16) | (product_lo >> 16);
|
||||
#else
|
||||
// Subtracting 0x8000 (= 0.5) and then using signed right shift
|
||||
// achieves proper rounding to result-1, except in the corner
|
||||
// case of negative numbers and lowest word = 0x8000.
|
||||
// To handle that, we also have to subtract 1 for negative numbers.
|
||||
uint32_t product_lo_tmp = product_lo;
|
||||
product_lo -= 0x8000;
|
||||
product_lo -= (uint32_t)product_hi >> 31;
|
||||
if (product_lo > product_lo_tmp)
|
||||
product_hi--;
|
||||
|
||||
// Discard the lowest 16 bits. Note that this is not exactly the same
|
||||
// as dividing by 0x10000. For example if product = -1, result will
|
||||
// also be -1 and not 0. This is compensated by adding +1 to the result
|
||||
// and compensating this in turn in the rounding above.
|
||||
fix16_t result = (product_hi << 16) | (product_lo >> 16);
|
||||
result += 1;
|
||||
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_MINIMUM;
|
||||
|
||||
uint32_t remainder = (a >= 0) ? a : (-a);
|
||||
uint32_t divider = (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_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 = quotient;
|
||||
|
||||
/* Figure out the sign of result */
|
||||
if ((a ^ b) & 0x80000000)
|
||||
{
|
||||
#ifndef FIXMATH_NO_OVERFLOW
|
||||
if (result == FIX16_MINIMUM)
|
||||
return 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 = 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 == false))
|
||||
{
|
||||
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 == false))
|
||||
{
|
||||
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;
|
||||
}
|
||||
Reference in New Issue
Block a user