From 4dc35d10175d7a1052e06cf94f1912196a25bcaa Mon Sep 17 00:00:00 2001
From: Carlos <karli_o@gmx.at>
Date: Mon, 3 May 2021 07:29:36 +0200
Subject: [PATCH] - add buffer full function to Medianfilter module - stabilize
 playback control loop

Signed-off-by: Carlos <karli_o@gmx.at>
---
 components/libmedian/MedianFilter.c         | 28 +++++++++++++++++++--
 components/libmedian/include/MedianFilter.h |  2 ++
 main/main.c                                 | 14 +++++++----
 3 files changed, 37 insertions(+), 7 deletions(-)

diff --git a/components/libmedian/MedianFilter.c b/components/libmedian/MedianFilter.c
index 064ef6a..e35671e 100644
--- a/components/libmedian/MedianFilter.c
+++ b/components/libmedian/MedianFilter.c
@@ -17,10 +17,13 @@
 
 #include "MedianFilter.h"
 
+/**
+ *
+ */
 int MEDIANFILTER_Init(sMedianFilter_t *medianFilter)
 {
-    if(medianFilter && medianFilter->medianBuffer &&
-        (medianFilter->numNodes % 2) && (medianFilter->numNodes > 1))
+    if (medianFilter && medianFilter->medianBuffer &&
+       (medianFilter->numNodes % 2) && (medianFilter->numNodes > 1))
     {
         //initialize buffer nodes
         for(unsigned int i = 0; i < medianFilter->numNodes; i++)
@@ -35,12 +38,17 @@ int MEDIANFILTER_Init(sMedianFilter_t *medianFilter)
         medianFilter->valueHead = medianFilter->medianBuffer;
         medianFilter->medianHead = &medianFilter->medianBuffer[medianFilter->numNodes / 2];
 
+        medianFilter->bufferCnt = 0;
+
         return 0;
     }
 
     return -1;
 }
 
+/**
+ *
+ */
 int64_t MEDIANFILTER_Insert(sMedianFilter_t *medianFilter, int64_t sample)
 {
     unsigned int i;
@@ -88,6 +96,10 @@ int64_t MEDIANFILTER_Insert(sMedianFilter_t *medianFilter, int64_t sample)
     it->prevValue = newNode;
     newNode->nextValue = it;
 
+    if (medianFilter->bufferCnt < medianFilter->numNodes) {
+    	medianFilter->bufferCnt++;
+    }
+
     //adjust median node
     if(i >= (medianFilter->numNodes / 2))
     {
@@ -97,3 +109,15 @@ int64_t MEDIANFILTER_Insert(sMedianFilter_t *medianFilter, int64_t sample)
     return medianFilter->medianHead->value;
 }
 
+/**
+ *
+ */
+uint8_t MEDIANFILTER_isFull(sMedianFilter_t *medianFilter) {
+    if (medianFilter->bufferCnt < medianFilter->numNodes) {
+    	return 0;
+    }
+    else {
+    	return 1;
+    }
+}
+
diff --git a/components/libmedian/include/MedianFilter.h b/components/libmedian/include/MedianFilter.h
index 5acbcde..fcb452c 100644
--- a/components/libmedian/include/MedianFilter.h
+++ b/components/libmedian/include/MedianFilter.h
@@ -33,10 +33,12 @@ typedef struct
     sMedianNode_t *ageHead;         //pointer to oldest value
     sMedianNode_t *valueHead;       //pointer to smallest value
     sMedianNode_t *medianHead;      //pointer to median value
+    unsigned int bufferCnt;
 } sMedianFilter_t;
 
 int MEDIANFILTER_Init(sMedianFilter_t *medianFilter);
 int64_t MEDIANFILTER_Insert(sMedianFilter_t *medianFilter, int64_t sample);
+uint8_t MEDIANFILTER_isFull(sMedianFilter_t *medianFilter);
 
 #ifdef __cplusplus
 }
diff --git a/main/main.c b/main/main.c
index e94f7dc..b077cf8 100644
--- a/main/main.c
+++ b/main/main.c
@@ -100,7 +100,7 @@ TaskHandle_t syncTaskHandle = NULL;
 
 #define CONFIG_USE_SNTP		0
 
-#define DAC_OUT_BUFFER_TIME_US		0//3500		// determined this by comparing a 180bpm metronome signal on a scope which is played by esp32 and ubuntu client
+#define DAC_OUT_BUFFER_TIME_US		3000		// determined this by comparing a 180bpm metronome signal on a scope which is played by esp32 and ubuntu client
 												// not sure why I need this though... And why it is so high. I'd expect something in the µs range??!
 
 static const char *TAG = "SC";
@@ -1120,6 +1120,7 @@ static void snapcast_sync_task(void *pvParameters) {
 //					size = chnk->size;
 					i2s_stop(i2s_cfg.i2s_port);
 //					i2s_zero_dma_buffer(i2s_cfg.i2s_port);
+					xQueueReset(i2s_event_queue);
 
 					i2sDmaBufferCnt = 0;
 
@@ -1137,7 +1138,7 @@ static void snapcast_sync_task(void *pvParameters) {
 					//p_payload += writtenBytes;	// TODO: produces heap error???
 
 					if (size == 0) {
-//						ESP_LOGI(TAG, "I2S can take more");
+						ESP_LOGI(TAG, "I2S can take more");
 
 						if (chnk != NULL) {
 							free(chnk->payload);
@@ -1260,12 +1261,14 @@ static void snapcast_sync_task(void *pvParameters) {
 
 			if (initialSync == 1) {
 				const uint8_t enableControlLoop = 1;
-				const int64_t age_expect = 48000;
+				const int64_t age_expect = -24000;
 				const int64_t maxOffset = 100;
 				const int64_t maxOffset_dropSample = 1000;
 
-				avg = MEDIANFILTER_Insert(&shortMedianFilter, age + age_expect);
-				//avg = age + age_expect;
+				avg = MEDIANFILTER_Insert(&shortMedianFilter, age + (-age_expect));
+				if ( MEDIANFILTER_isFull(&shortMedianFilter) == 0 ) {
+					avg = age + (-age_expect);
+				}
 
 				// resync hard if we are off too far
 				if ((avg < -1 * chunkDuration_us / 4) || (avg > 1 * chunkDuration_us / 4) || (initialSync == 0)) {
@@ -2163,6 +2166,7 @@ void setup_dsp_i2s(uint32_t sample_rate, i2s_port_t i2sNum)
   };
 
   i2s_driver_install(i2sNum, &i2s_config0, 7, &i2s_event_queue);
+  i2s_stop(i2sNum);
 //  i2s_zero_dma_buffer(I2S_NUM_0);
   i2s_set_pin(i2sNum, &pin_config0);
 }