@mentra/bluetooth-sdk 0.1.0

package/ios/Packages/CoreObjC/energy.c

@@ -0,0 +1,70 @@
+/******************************************************************************
+ *
+ *  Copyright 2022 Google LLC
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at:
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ *
+ ******************************************************************************/
+
+#include "energy.h"
+#include "tables.h"
+
+
+/**
+ * Energy estimation per band
+ */
+bool lc3_energy_compute(
+    enum lc3_dt dt, enum lc3_srate sr, const float *x, float *e)
+{
+    static const int n1_table[LC3_NUM_DT][LC3_NUM_SRATE] = {
+        [LC3_DT_7M5] = { 56, 34, 27, 24, 22 },
+        [LC3_DT_10M] = { 49, 28, 23, 20, 18 },
+    };
+
+    /* First bands are 1 coefficient width */
+
+    int n1 = n1_table[dt][sr];
+    float e_sum[2] = { 0, 0 };
+    int iband;
+
+    for (iband = 0; iband < n1; iband++) {
+        *e = x[iband] * x[iband];
+        e_sum[0] += *(e++);
+    }
+
+    /* Mean the square of coefficients within each band,
+     * note that 7.5ms 8KHz frame has more bands than samples */
+
+    int nb = LC3_MIN(LC3_NUM_BANDS, LC3_NS(dt, sr));
+    int iband_h = nb - 2*(2 - dt);
+    const int *lim = lc3_band_lim[dt][sr];
+
+    for (int i = lim[iband]; iband < nb; iband++) {
+        int ie = lim[iband+1];
+        int n = ie - i;
+
+        float sx2 = x[i] * x[i];
+        for (i++; i < ie; i++)
+            sx2 += x[i] * x[i];
+
+        *e = sx2 / n;
+        e_sum[iband >= iband_h] += *(e++);
+    }
+
+    for (; iband < LC3_NUM_BANDS; iband++)
+        *(e++) = 0;
+
+    /* Return the near nyquist flag */
+
+    return e_sum[1] > 30 * e_sum[0];
+}

package/ios/Packages/CoreObjC/energy.h

@@ -0,0 +1,43 @@
+/******************************************************************************
+ *
+ *  Copyright 2022 Google LLC
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at:
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ *
+ ******************************************************************************/
+
+/**
+ * LC3 - Energy estimation per band
+ *
+ * Reference : Low Complexity Communication Codec (LC3)
+ *             Bluetooth Specification v1.0
+ */
+
+#ifndef __LC3_ENERGY_H
+#define __LC3_ENERGY_H
+
+#include "common.h"
+
+
+/**
+ * Energy estimation per band
+ * dt, sr          Duration and samplerate of the frame
+ * x               Input MDCT coefficient
+ * e               Energy estimation per bands
+ * return          True when high energy detected near Nyquist frequency
+ */
+bool lc3_energy_compute(
+    enum lc3_dt dt, enum lc3_srate sr, const float *x, float *e);
+
+
+#endif /* __LC3_ENERGY_H */

package/ios/Packages/CoreObjC/fastmath.h

@@ -0,0 +1,158 @@
+/******************************************************************************
+ *
+ *  Copyright 2022 Google LLC
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at:
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ *
+ ******************************************************************************/
+
+/**
+ * LC3 - Mathematics function approximation
+ */
+
+#ifndef __LC3_FASTMATH_H
+#define __LC3_FASTMATH_H
+
+#include <stdint.h>
+#include <math.h>
+
+
+/**
+ * Fast 2^n approximation
+ * x               Operand, range -8 to 8
+ * return          2^x approximation (max relative error ~ 7e-6)
+ */
+static inline float fast_exp2f(float x)
+{
+    float y;
+
+    /* --- Polynomial approx in range -0.5 to 0.5 --- */
+
+    static const float c[] = { 1.27191277e-09, 1.47415221e-07,
+        1.35510312e-05, 9.38375815e-04, 4.33216946e-02 };
+
+    y = (    c[0]) * x;
+    y = (y + c[1]) * x;
+    y = (y + c[2]) * x;
+    y = (y + c[3]) * x;
+    y = (y + c[4]) * x;
+    y = (y + 1.f);
+
+    /* --- Raise to the power of 16  --- */
+
+    y = y*y;
+    y = y*y;
+    y = y*y;
+    y = y*y;
+
+    return y;
+}
+
+/**
+ * Fast log2(x) approximation
+ * x               Operand, greater than 0
+ * return          log2(x) approximation (max absolute error ~ 1e-4)
+ */
+static inline float fast_log2f(float x)
+{
+    float y;
+    int e;
+
+    /* --- Polynomial approx in range 0.5 to 1 --- */
+
+    static const float c[] = {
+        -1.29479677, 5.11769018, -8.42295281, 8.10557963, -3.50567360 };
+
+    x = frexpf(x, &e);
+
+    y = (    c[0]) * x;
+    y = (y + c[1]) * x;
+    y = (y + c[2]) * x;
+    y = (y + c[3]) * x;
+    y = (y + c[4]);
+
+    /* --- Add log2f(2^e) and return --- */
+
+    return e + y;
+}
+
+/**
+ * Fast log10(x) approximation
+ * x               Operand, greater than 0
+ * return          log10(x) approximation (max absolute error ~ 1e-4)
+ */
+static inline float fast_log10f(float x)
+{
+    return log10f(2) * fast_log2f(x);
+}
+
+/**
+ * Fast `10 * log10(x)` (or dB) approximation in fixed Q16
+ * x               Operand, in range 2^-63 to 2^63 (1e-19 to 1e19)
+ * return          10 * log10(x) in fixed Q16 (-190 to 192 dB)
+ *
+ * - The 0 value is accepted and return the minimum value ~ -191dB
+ * - This function assumed that float 32 bits is coded IEEE 754
+ */
+static inline int32_t fast_db_q16(float x)
+{
+    /* --- Table in Q15 --- */
+
+    static const uint16_t t[][2] = {
+
+        /* [n][0] = 10 * log10(2) * log2(1 + n/32), with n = [0..15]     */
+        /* [n][1] = [n+1][0] - [n][0] (while defining [16][0])           */
+
+        {     0, 4379 }, {  4379, 4248 }, {  8627, 4125 }, { 12753, 4009 },
+        { 16762, 3899 }, { 20661, 3795 }, { 24456, 3697 }, { 28153, 3603 },
+        { 31755, 3514 }, { 35269, 3429 }, { 38699, 3349 }, { 42047, 3272 },
+        { 45319, 3198 }, { 48517, 3128 }, { 51645, 3061 }, { 54705, 2996 },
+
+        /* [n][0] = 10 * log10(2) * log2(1 + n/32) - 10 * log10(2) / 2,  */
+        /*     with n = [16..31]                                         */
+        /* [n][1] = [n+1][0] - [n][0] (while defining [32][0])           */
+
+        {  8381, 2934 }, { 11315, 2875 }, { 14190, 2818 }, { 17008, 2763 },
+        { 19772, 2711 }, { 22482, 2660 }, { 25142, 2611 }, { 27754, 2564 },
+        { 30318, 2519 }, { 32837, 2475 }, { 35312, 2433 }, { 37744, 2392 },
+        { 40136, 2352 }, { 42489, 2314 }, { 44803, 2277 }, { 47080, 2241 },
+
+    };
+
+    /* --- Approximation ---
+     *
+     *   10 * log10(x^2) = 10 * log10(2) * log2(x^2)
+     *
+     *   And log2(x^2) = 2 * log2( (1 + m) * 2^e )
+     *                 = 2 * (e + log2(1 + m)) , with m in range [0..1]
+     *
+     * Split the float values in :
+     *   e2  Double value of the exponent (2 * e + k)
+     *   hi  High 5 bits of mantissa, for precalculated result `t[hi][0]`
+     *   lo  Low 16 bits of mantissa, for linear interpolation `t[hi][1]`
+     *
+     * Two cases, from the range of the mantissa :
+     *   0 to 0.5   `k = 0`, use 1st part of the table
+     *   0.5 to 1   `k = 1`, use 2nd part of the table  */
+
+    union { float f; uint32_t u; } x2 = { .f = x*x };
+
+    int e2 = (int)(x2.u >> 22) - 2*127;
+    int hi = (x2.u >> 18) & 0x1f;
+    int lo = (x2.u >>  2) & 0xffff;
+
+    return e2 * 49321 + t[hi][0] + ((t[hi][1] * lo) >> 16);
+}
+
+
+#endif /* __LC3_FASTMATH_H */