From 869d076808f9d3b36a2ee63909c8a7f0b75a76e2 Mon Sep 17 00:00:00 2001
From: zhaohe <h_zhaohe@163.com>
Date: Sun, 2 Jul 2023 19:56:37 +0800
Subject: [PATCH] update

---
 spisound.c | 108 +++++++++++++++++++++++++++++++++++++++++++++----------------
 1 file changed, 80 insertions(+), 28 deletions(-)

diff --git a/spisound.c b/spisound.c
index 55f5c0a..8a61960 100644
--- a/spisound.c
+++ b/spisound.c
@@ -45,7 +45,12 @@ static void *sound_read_thread_func(void *v);
 static void  gpio_set_value(int value) { write(m_gpiofd, value ? "1\n" : "0\n", 2); }
 static void  readvoice();
 
-static int32_t *multiplierX2(int32_t *voice, int32_t size, int32_t *outsize) {
+typedef struct {
+  int32_t *voice;
+  int32_t  size;
+} audio_t;
+
+static audio_t multiplierX2(int32_t *voice, int32_t size) {
   static int32_t voicebuf[MAX_ONE_FRAME_VOICE_SIZE * 2];  // 50ms
   if (size > MAX_ONE_FRAME_VOICE_SIZE) {
     size = MAX_ONE_FRAME_VOICE_SIZE;
@@ -57,37 +62,82 @@ static int32_t *multiplierX2(int32_t *voice, int32_t size, int32_t *outsize) {
       voicebuf[i * 2 + 1] = (voice[i] + voice[i + 1]) / 2;
     }
   }
-  *outsize = size * 2;
-  return voicebuf;
+  audio_t ret;
+  ret.size  = size * 2;
+  ret.voice = voicebuf;
+  return ret;
 }
 
-static void filterVoice(int32_t *voice, int32_t size) {
-  /**
-   * @brief 简单的过滤器，如果当前点大于上一个点和下一个点的平均值的1.5倍，则将当前点设置为平均值
-   */
-  static int32_t lastvoice = 0;
-  lastvoice                = voice[size - 1];
-
-  {  //
-    int32_t avg = (lastvoice + voice[1]) / 2;
-    if (abs(voice[0]) > abs(avg * 1.5)) {
-      voice[0] = avg;
-    }
+static audio_t multiplierX2_2(int32_t *voice, int32_t size) {
+  static int32_t voicebuf[MAX_ONE_FRAME_VOICE_SIZE * 4];  // 50ms
+  if (size > MAX_ONE_FRAME_VOICE_SIZE) {
+    size = MAX_ONE_FRAME_VOICE_SIZE;
   }
 
-  for (size_t i = 1; i < size - 1; i++) {
-    int32_t avg = (voice[i - 1] + voice[i + 1]) / 2;
-    if (abs(voice[i]) > abs(avg * 1.5)) {
-      voice[i] = avg;
+  for (size_t i = 0; i < size; i++) {
+    voicebuf[i * 2] = voice[i];
+    if (i + 1 < size) {
+      voicebuf[i * 2 + 1] = (voice[i] + voice[i + 1]) / 2;
     }
   }
+  audio_t ret;
+  ret.size  = size * 2;
+  ret.voice = voicebuf;
+  return ret;
+}
 
-  {
-    int32_t expectendvoice = voice[size - 2] + (voice[size - 2] - voice[size - 3]);
-    if (abs(voice[size - 1]) > abs(expectendvoice * 1.5)) {
-      voice[size - 1] = expectendvoice;
+static int32_t filter(int32_t input, int32_t v1, int32_t v2, int32_t v3, int32_t v4, int32_t v5, int32_t v6) {
+  int32_t v[6];
+  v[0] = v1;
+  v[1] = v2;
+  v[2] = v3;
+  v[3] = v4;
+  v[4] = v5;
+  v[5] = v6;
+
+  // 对v进行排序
+  for (size_t i = 0; i < 6; i++) {
+    for (size_t j = i + 1; j < 6; j++) {
+      if (v[i] > v[j]) {
+        int32_t tmp = v[i];
+        v[i]        = v[j];
+        v[j]        = tmp;
+      }
     }
   }
+
+  int expect = (v[2] + v[3]) / 2;
+  if (abs(input) > abs(expect * 2) || abs(input) < abs(expect / 2)) {
+    return expect;
+  } else {
+    return input;
+  }
+}
+
+static bool filterVoice(int32_t *voice, int32_t size) {
+#if 0
+  static int32_t vbuf[MAX_ONE_FRAME_VOICE_SIZE * 3];  // 50ms
+  static int32_t vbufnum = 0;
+  vbufnum++;
+
+  memmove(vbuf, vbuf + size, 2 * size * sizeof(int32_t));
+  memcpy(vbuf + 2 * size, voice, size * sizeof(int32_t));
+
+  if (vbufnum < 3) {
+    return false;
+  }
+#endif
+  static int32_t vbuf[MAX_ONE_FRAME_VOICE_SIZE];  // 50ms
+  memcpy(vbuf, voice, size * sizeof(int32_t));
+
+  int32_t *cachebegin = vbuf;
+
+  for (int32_t i = 3; i < size - 3; i++) {
+    voice[i] = filter(voice[i], *(cachebegin + i - 1), *(cachebegin + i - 2), *(cachebegin + i - 3), *(cachebegin + i + 1), *(cachebegin + i + 2),
+                      *(cachebegin + i + 3));
+    // voice[i] = cachebegin[i];
+  }
+  return true;
 }
 
 error_code_t spisound_init(const char *deviceName, int csgpionameId, spisound_cb_t callback) {
@@ -167,6 +217,7 @@ static void readvoice() {
   gpio_set_value(0);
   // usleep(1500);  // CS信号触发后，给单片机足够的时间准备音频数据
   int ret = read(m_fd, &rxdatabytesize, 4);
+  // printf("%d\n",rxdatabytesize);
   // printf("read rxdatabytesize:%d\n", rxdatabytesize);
   if (rxdatabytesize <= 0) {
     gpio_set_value(1);
@@ -181,11 +232,12 @@ static void readvoice() {
   ret = read(m_fd, voicebuf, rxdatabytesize);
   gpio_set_value(1);
 
-  {
-    int32_t voiceframesize = 0;
-    filterVoice(voicebuf, rxdatabytesize / 4);
-    int32_t *voicereport = multiplierX2(voicebuf, rxdatabytesize / 4, &voiceframesize);
-    if (m_callback) m_callback(voicereport, voiceframesize);
+  int32_t voiceframesize = 0;
+  bool    ready          = filterVoice(voicebuf, rxdatabytesize / 4);
+  if (ready) {
+    audio_t v1 = multiplierX2(voicebuf, rxdatabytesize / 4);
+    audio_t v2 = multiplierX2_2(v1.voice, v1.size);
+    if (m_callback) m_callback(v2.voice, v2.size);
   }
 
   return;