From 69081a72d740fee7307767bbfb96c4b02881c3a9 Mon Sep 17 00:00:00 2001
From: Alan Carvalho de Assis <acassis@gmail.com>
Date: Tue, 6 Jun 2023 21:01:57 -0300
Subject: [PATCH] libdsp: Add average filter

This commit add average filter to DSP library
---
 include/dsp.h         | 14 +++++++
 libs/libdsp/Makefile  |  1 +
 libs/libdsp/lib_avg.c | 94 +++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 109 insertions(+)
 create mode 100644 libs/libdsp/lib_avg.c

diff --git a/include/dsp.h b/include/dsp.h
index 64142be66d..62672adfa1 100644
--- a/include/dsp.h
+++ b/include/dsp.h
@@ -447,6 +447,14 @@ struct pmsm_model_f32_s
   float                         iq_int; /* Iq integral part */
 };
 
+/* Average filter */
+
+struct avg_filter_data_s
+{
+  float prev_avg;      /* Previous average */
+  float k;             /* k counter */
+};
+
 /****************************************************************************
  * Public Functions Prototypes
  ****************************************************************************/
@@ -601,6 +609,12 @@ int pmsm_model_elec(FAR struct pmsm_model_f32_s *model,
                     FAR ab_frame_f32_t *vab);
 int pmsm_model_mech(FAR struct pmsm_model_f32_s *model, float load);
 
+/* Average filter */
+
+void avg_filter_data_init(FAR struct avg_filter_data_s *data,
+                          float prev_avg, float k);
+float avg_filter(FAR struct avg_filter_data_s *data, float x);
+
 #undef EXTERN
 #if defined(__cplusplus)
 }
diff --git a/libs/libdsp/Makefile b/libs/libdsp/Makefile
index cb2c72e6f7..0d110be4c8 100644
--- a/libs/libdsp/Makefile
+++ b/libs/libdsp/Makefile
@@ -21,6 +21,7 @@
 include $(TOPDIR)/Make.defs
 
 ifeq ($(CONFIG_LIBDSP),y)
+CSRCS += lib_avg.c
 CSRCS += lib_pid.c
 CSRCS += lib_svm.c
 CSRCS += lib_transform.c
diff --git a/libs/libdsp/lib_avg.c b/libs/libdsp/lib_avg.c
new file mode 100644
index 0000000000..bb8fd171c0
--- /dev/null
+++ b/libs/libdsp/lib_avg.c
@@ -0,0 +1,94 @@
+/****************************************************************************
+ * libs/libdsp/lib_avg.c
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you 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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <assert.h>
+#include <dsp.h>
+#include <string.h>
+
+/* Based on video explanation of Dr. Shane Ross:
+ * https://www.youtube.com/watch?v=HCd-leV8OkU
+ */
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: avg_filter_data_init
+ *
+ * Description:
+ *   Initialize the data struct used to store prev_avg and k parameter.
+ *
+ * Input Parameters:
+ *   data     - pointer to avg_filter_data_s
+ *   prev_avg - initial value of prev_avg
+ *   k        - initial value of k counter
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+void avg_filter_data_init(FAR struct avg_filter_data_s *data,
+                          float prev_avg, float k)
+{
+  LIBDSP_DEBUGASSERT(k > 0.0f)
+
+  data->prev_avg = prev_avg;
+  data->k        = k;
+}
+
+/****************************************************************************
+ * Name: avg_filter
+ *
+ * Description:
+ *   Calculate the recurring average of a signal in the time
+ *
+ * Input Parameters:
+ *   prev_avg - pointer to previous average variable
+ *   k - pointer to k counter variable
+ *   x - current signal value
+ *
+ * Returned Value:
+ *   Average value
+ *
+ ****************************************************************************/
+
+float avg_filter(FAR struct avg_filter_data_s *data, float x)
+{
+  float alpha;
+  float avg;
+
+  LIBDSP_DEBUGASSERT(data != NULL);
+
+  alpha = (data->k - 1.0f) / data->k;
+
+  avg = (alpha * data->prev_avg) + ((1.0f - alpha) * x);
+  data->k += 1.0f;
+
+  data->prev_avg = avg;
+
+  return avg;
+}
+