veml6030/
correction.rs

1use crate::{Gain, IntegrationTime};
2
3/// Calculate raw value for threshold applying compensation if necessary.
4///
5/// For values higher than 1000 lx and 1/4 or 1/8 gain, the inverse of the
6/// compensation formula is applied. This involves quite some math so it
7/// may be interesting to calculate the threshold values ahead of time.
8pub fn calculate_raw_threshold_value(it: IntegrationTime, gain: Gain, lux: f32) -> u16 {
9    let factor = get_lux_raw_conversion_factor(it, gain);
10    if (gain == Gain::OneQuarter || gain == Gain::OneEighth) && lux > 1000.0 {
11        let lux = inverse_high_lux_correction(f64::from(lux));
12        (lux / f64::from(factor)) as u16
13    } else {
14        (f64::from(lux) / f64::from(factor)) as u16
15    }
16}
17
18pub(crate) fn get_lux_raw_conversion_factor(it: IntegrationTime, gain: Gain) -> f32 {
19    let gain_factor = match gain {
20        Gain::Two => 1.0,
21        Gain::One => 2.0,
22        Gain::OneQuarter => 8.0,
23        Gain::OneEighth => 16.0,
24    };
25    let it_factor = match it {
26        IntegrationTime::Ms800 => 0.0036,
27        IntegrationTime::Ms400 => 0.0072,
28        IntegrationTime::Ms200 => 0.0144,
29        IntegrationTime::Ms100 => 0.0288,
30        IntegrationTime::Ms50 => 0.0576,
31        IntegrationTime::Ms25 => 0.1152,
32    };
33    gain_factor * it_factor
34}
35
36const C0: f64 = 1.0023;
37const C1: f64 = 8.1488e-05;
38const C2: f64 = -9.3924e-09;
39const C3: f64 = 6.0135e-13;
40
41pub(crate) fn correct_high_lux(lux: f64) -> f64 {
42    lux * lux * lux * lux * C3 + lux * lux * lux * C2 + lux * lux * C1 + lux * C0
43}
44
45fn inverse_high_lux_correction(lux: f64) -> f64 {
46    // Inverse of the polinomial used to correct for lux > 1000.
47    // `y = 6.0135e-13*(x^4) - 9.3924e-9*(x^3) + 8.1488e-5*(x^2) + 1.0023*x`.
48    // This runs into underflow/overlow issues if trying to solve it directly.
49    // However, it can be solved for unknown coefficients and then
50    // we put in the values.
51    let base_expr = 2.0 * libm::pow(C1, 3.0) - 9.0 * C2 * C1 * C0 + 27.0 * C3 * libm::pow(C0, 2.0)
52        - 27.0 * libm::pow(C2, 2.0) * lux
53        + 72.0 * C3 * C1 * lux;
54    let inner_expr = libm::pow(C1, 2.0) - 3.0 * C2 * C0 - 12.0 * C3 * lux;
55    let sqrt_expr = libm::sqrt(-4.0 * libm::pow(inner_expr, 3.0) + libm::pow(base_expr, 2.0));
56    let cube_root_expr = libm::pow(base_expr + sqrt_expr, 1.0 / 3.0);
57    let first_term = -C2 / (4.0 * C3);
58    let second_term = -libm::sqrt(
59        libm::pow(C2, 2.0) / (4.0 * libm::pow(C3, 2.0)) - (2.0 * C1) / (3.0 * C3)
60            + (libm::pow(2.0, 1.0 / 3.0) * inner_expr) / (3.0 * C3 * cube_root_expr)
61            + cube_root_expr / (3.0 * libm::pow(2.0, 1.0 / 3.0) * C3),
62    ) / 2.0;
63    let third_term = libm::sqrt(
64        libm::pow(C2, 2.0) / (2.0 * libm::pow(C3, 2.0))
65            - (4.0 * C1) / (3.0 * C3)
66            - (libm::pow(2.0, 1.0 / 3.0) * inner_expr) / (3.0 * C3 * cube_root_expr)
67            - cube_root_expr / (3.0 * libm::pow(2.0, 1.0 / 3.0) * C3)
68            - (-(libm::pow(C2, 3.0) / libm::pow(C3, 3.0)) + (4.0 * C2 * C1) / libm::pow(C3, 2.0)
69                - (8.0 * C0) / C3)
70                / (4.0
71                    * libm::sqrt(
72                        libm::pow(C2, 2.0) / (4.0 * libm::pow(C3, 2.0)) - (2.0 * C1) / (3.0 * C3)
73                            + (libm::pow(2.0, 1.0 / 3.0) * inner_expr)
74                                / (3.0 * C3 * cube_root_expr)
75                            + cube_root_expr / (3.0 * libm::pow(2.0, 1.0 / 3.0) * C3),
76                    )),
77    ) / 2.0;
78
79    first_term + second_term + third_term
80}
81
82#[cfg(test)]
83mod correction_tests {
84    use super::*;
85
86    macro_rules! check_correction {
87        ($name:ident, $lux:expr) => {
88            #[test]
89            fn $name() {
90                let lux = $lux;
91                let corrected = correct_high_lux(lux);
92                let inverse_correction = inverse_high_lux_correction(corrected);
93                assert!(lux - 0.5 < inverse_correction);
94                assert!(lux + 0.5 > inverse_correction);
95            }
96        };
97    }
98
99    check_correction!(_1000, 1000.0);
100    check_correction!(_1500, 1500.0);
101    check_correction!(_2500, 2500.0);
102    check_correction!(_5000, 5000.0);
103    check_correction!(_7890, 7890.0);
104    check_correction!(_10000, 10000.0);
105    check_correction!(_15000, 15000.0);
106    check_correction!(_20000, 20000.0);
107    check_correction!(_56789, 56789.0);
108    check_correction!(_78901, 78901.0);
109    check_correction!(_120000, 120_000.0);
110}
veml6030/correction.rs

veml6030/
correction.rs
