FS-9009 [mod_avmd] Amplitude estimation

Add DESA-2 estimation of signal amplitude

src/mod/applications/mod_avmd/avmd_amplitude.c

@@ -5,15 +5,8 @@
 #include "avmd_amplitude.h"
 #include "avmd_psi.h"
 
-/*! \brief
- * @author Eric des Courtis
- * @param b A circular audio sample buffer
- * @param i Position in the buffer
- * @param f Frequency estimate
- * @return The amplitude at position i 
- */
-extern double avmd_amplitude(circ_buffer_t *b, size_t i, double f)
-{
+
+double avmd_amplitude(circ_buffer_t *b, size_t i, double f) {
     double result;
     result = sqrt(PSI(b, i) / sin(f * f));
     return result;

src/mod/applications/mod_avmd/avmd_amplitude.h

 /*
  * @brief   Estimation of amplitude using DESA-2 algorithm.
- * @author Eric des Courtis
- * @par    Modifications: Piotr Gregor < piotrek.gregor gmail.com >
+ *
+ * Contributor(s):
+ *
+ * Eric des Courtis <eric.des.courtis@benbria.com>
+ * Piotr Gregor <piotrek.gregor gmail.com>:
  */
 
 
@@ -12,7 +15,7 @@
 #include "avmd_buffer.h"
 
 
-extern double avmd_amplitude(circ_buffer_t *, size_t i, double f);
+double avmd_amplitude(circ_buffer_t *, size_t i, double f) __attribute__ ((nonnull(1)));
 
 
 #endif /* __AVMD_AMPLITUDE_H__ */

src/mod/applications/mod_avmd/avmd_desa2.c

@@ -19,8 +19,8 @@ int __isnan(double);
 #include "avmd_fast_acosf.h"
 #endif
 
-extern double avmd_desa2(circ_buffer_t *b, size_t i)
-{
+
+double avmd_desa2(circ_buffer_t *b, size_t i, double *amplitude) {
     double d;
     double n;
     double x0;
@@ -30,6 +30,7 @@ extern double avmd_desa2(circ_buffer_t *b, size_t i)
     double x4;
     double x2sq;
     double result;
+    double PSI_Xn, PSI_Yn, NEEDED;
 
     x0 = GET_SAMPLE((b), (i));
     x1 = GET_SAMPLE((b), ((i) + 1));
@@ -39,8 +40,14 @@ extern double avmd_desa2(circ_buffer_t *b, size_t i)
 
     x2sq = x2 * x2;
     d = 2.0 * ((x2sq) - (x1 * x3));
-    if (d == 0.0) return 0.0;
-    n = ((x2sq) - (x0 * x4)) - ((x1 * x1) - (x0 * x2)) - ((x3 * x3) - (x2 * x4));
+    if (d == 0.0) {
+        *amplitude = 0.0;
+        return 0.0;
+    }
+    PSI_Xn = ((x2sq) - (x0 * x4));
+    NEEDED = ((x1 * x1) - (x0 * x2)) + ((x3 * x3) - (x2 * x4));
+    n = ((x2sq) - (x0 * x4)) - NEEDED;
+    PSI_Yn = NEEDED + PSI_Xn;
 
 #ifdef AVMD_FAST_MATH
     result = 0.5 * (double)fast_acosf((float)n/d);
@@ -48,7 +55,10 @@ extern double avmd_desa2(circ_buffer_t *b, size_t i)
     result = 0.5 * acos(n/d);
 #endif
 
-    if (ISNAN(result)) result = 0.0;
+    if (ISNAN(result)) {
+        result = 0.0;
+    }
+    *amplitude = 2.0 * PSI_Xn / sqrt(PSI_Yn);
 
     return result;
 

src/mod/applications/mod_avmd/avmd_desa2.h

 /*
  * @brief  DESA-2 algorithm implementation.
- * @author Eric des Courtis
- * @par    Modifications: Piotr Gregor < piotrek.gregor gmail.com >
+ *
+ * Contributor(s):
+ *
+ * Eric des Courtis <eric.des.courtis@benbria.com>
+ * Piotr Gregor <piotrek.gregor gmail.com>:
  */
 
 
@@ -12,8 +15,8 @@
 #include <math.h>
 #include "avmd_buffer.h"
 
-/* Returns digital frequency estimation. */
-extern double avmd_desa2(circ_buffer_t *b, size_t i);
+/* Returns digital frequency estimation and amplitude estimation. */
+extern double avmd_desa2(circ_buffer_t *b, size_t i, double *amplitude) __attribute__ ((nonnull(1,3)));
 
 
 #endif  /* __AVMD_DESA2_H__ */

src/mod/applications/mod_avmd/avmd_desa2_tweaked.c

@@ -19,11 +19,10 @@ int __isnan(double);
 #include "avmd_fast_acosf.h"
 #endif
 
+
 double
-avmd_desa2_tweaked(circ_buffer_t *b, size_t i)
-{
-    double d;
-    double n;
+avmd_desa2_tweaked(circ_buffer_t *b, size_t i, double *amplitude) {
+    double n, d;
     double x0;
     double x1;
     double x2;
@@ -31,6 +30,7 @@ avmd_desa2_tweaked(circ_buffer_t *b, size_t i)
     double x4;
     double x2sq;
     double result;
+    double PSI_Xn, PSI_Yn, NEEDED;
 
     x0 = GET_SAMPLE((b), (i));
     x1 = GET_SAMPLE((b), ((i) + 1));
@@ -39,8 +39,10 @@ avmd_desa2_tweaked(circ_buffer_t *b, size_t i)
     x4 = GET_SAMPLE((b), ((i) + 4));
     x2sq = x2 * x2;
     d = 2.0 * ((x2sq) - (x1 * x3));
-    n = ((x2sq) - (x0 * x4)) - ((x1 * x1) 
-            - (x0 * x2)) - ((x3 * x3) - (x2 * x4));
+    PSI_Xn = ((x2sq) - (x0 * x4));
+    NEEDED = ((x1 * x1) - (x0 * x2)) + ((x3 * x3) - (x2 * x4));
+    n = ((x2sq) - (x0 * x4)) - NEEDED;
+    PSI_Yn = NEEDED + PSI_Xn;
 
 /* instead of
 #ifdef FASTMATH
@@ -52,11 +54,14 @@ avmd_desa2_tweaked(circ_buffer_t *b, size_t i)
 
     result = n/d;
     if (ISINF(result)) {
-        if (n < 0.0)
+        *amplitude = 0.0;
+        if (n < 0.0) {
             return -10.0;
-        else
+        } else {
             return 10.0;
+        }
     }
+    *amplitude = 2.0 * PSI_Xn / sqrt(PSI_Yn);
     return result;
 }
 

src/mod/applications/mod_avmd/avmd_desa2_tweaked.h

@@ -33,9 +33,10 @@
  * to corresponding frequencies is nonlinear.
  * The actual frequency estimation can be retrieved later
  * from this partial result using
- *      0.5 * acos(n/d)
+ *      0.5 * acos(returned_value)
+ * Amplitude estimation is returned by address.
  */
-double avmd_desa2_tweaked(circ_buffer_t *b, size_t i);
+double avmd_desa2_tweaked(circ_buffer_t *b, size_t i, double *amplitude) __attribute__ ((nonnull(1,3)));
 
 
 #endif  /* __AVMD_DESA2_TWEAKED_H__ */

src/mod/applications/mod_avmd/avmd_psi.h

@@ -4,6 +4,8 @@
 
 #include "avmd_buffer.h"
 
-#define PSI(b, i) (GET_SAMPLE((b), ((i) + 1))*GET_SAMPLE((b), ((i) + 1))-GET_SAMPLE((b), ((i) + 2))*GET_SAMPLE((b), ((i) + 0)))
+
+#define PSI(b, i) (GET_SAMPLE((b), ((i) + 1)) * GET_SAMPLE((b), ((i) + 1)) - GET_SAMPLE((b), ((i) + 2)) * GET_SAMPLE((b), ((i) + 0)))
+
 
 #endif /* __AVMD_PSI_H__ */