Modify MedianFilter to work with non-full buffer. Fix get_median. Remove unneeded code from http_task (#49)

components/libmedian/MedianFilter.c

@@ -14,9 +14,7 @@
  */
 
 #include <stdint.h>
-
 #include "MedianFilter.h"
-
 /**
  *
  */
@@ -25,7 +23,7 @@ int MEDIANFILTER_Init(sMedianFilter_t *medianFilter) {
       (medianFilter->numNodes % 2) && (medianFilter->numNodes > 1)) {
     // initialize buffer nodes
     for (unsigned int i = 0; i < medianFilter->numNodes; i++) {
-      medianFilter->medianBuffer[i].value = 0;
+      medianFilter->medianBuffer[i].value = INT64_MAX;
       medianFilter->medianBuffer[i].nextAge =
           &medianFilter->medianBuffer[(i + 1) % medianFilter->numNodes];
       medianFilter->medianBuffer[i].nextValue =
@@ -36,8 +34,7 @@ int MEDIANFILTER_Init(sMedianFilter_t *medianFilter) {
     // initialize heads
     medianFilter->ageHead = medianFilter->medianBuffer;
     medianFilter->valueHead = medianFilter->medianBuffer;
-    medianFilter->medianHead =
-        &medianFilter->medianBuffer[medianFilter->numNodes / 2];
+    medianFilter->medianHead = medianFilter->medianBuffer;
 
     medianFilter->bufferCnt = 0;
 
@@ -53,6 +50,10 @@ int MEDIANFILTER_Init(sMedianFilter_t *medianFilter) {
 int64_t MEDIANFILTER_Insert(sMedianFilter_t *medianFilter, int64_t sample) {
   unsigned int i;
   sMedianNode_t *newNode, *it;
+  if (medianFilter->bufferCnt < medianFilter->numNodes) {
+    medianFilter->bufferCnt++;
+  }
+
 
   // if oldest node is also the smallest node,
   // increment value head
@@ -60,8 +61,9 @@ int64_t MEDIANFILTER_Insert(sMedianFilter_t *medianFilter, int64_t sample) {
     medianFilter->valueHead = medianFilter->valueHead->nextValue;
   }
 
-  if ((medianFilter->ageHead == medianFilter->medianHead) ||
-      (medianFilter->ageHead->value > medianFilter->medianHead->value)) {
+  if (((medianFilter->ageHead == medianFilter->medianHead) ||
+      (medianFilter->ageHead->value > medianFilter->medianHead->value)) &&
+      (medianFilter->bufferCnt >= medianFilter->numNodes)) {
     // prepare for median correction
     medianFilter->medianHead = medianFilter->medianHead->prevValue;
   }
@@ -80,15 +82,15 @@ int64_t MEDIANFILTER_Insert(sMedianFilter_t *medianFilter, int64_t sample) {
 
   // find new node position
   it = medianFilter->valueHead;  // set iterator as value head
-  for (i = 0; i < medianFilter->numNodes - 1; i++) {
+  for (i = 0; i < medianFilter->bufferCnt - 1; i++) {
     if (sample < it->value) {
-      if (i == 0) {  // replace value head if new node is the smallest
-        medianFilter->valueHead = newNode;
-      }
       break;
     }
     it = it->nextValue;
   }
+  if (i == 0) {  // replace value head if new node is the smallest
+    medianFilter->valueHead = newNode;
+  }
 
   // insert new node in list
   it->prevValue->nextValue = newNode;
@@ -96,12 +98,17 @@ 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)) {
+  if ((medianFilter->bufferCnt < medianFilter->numNodes)){
+      if (medianFilter->bufferCnt % 2 != 0 && medianFilter->bufferCnt != 1) {
+          medianFilter->medianHead = medianFilter->medianHead->prevValue;
+      }
+      if (((i > (medianFilter->bufferCnt / 2)) && (medianFilter->bufferCnt % 2 != 0)) ||
+        ((i >= (medianFilter->bufferCnt / 2)) && (medianFilter->bufferCnt % 2 == 0))) {
+        medianFilter->medianHead = medianFilter->medianHead->nextValue;
+      }
+  }
+  else if (i >= (medianFilter->bufferCnt / 2) ) {
     medianFilter->medianHead = medianFilter->medianHead->nextValue;
   }
 
@@ -115,30 +122,31 @@ int64_t MEDIANFILTER_get_median(sMedianFilter_t *medianFilter, uint32_t n) {
   int64_t avgMedian = 0;
   sMedianNode_t *it;
   int32_t i;
+  if (n >= medianFilter->bufferCnt) {
+      n = (((medianFilter->bufferCnt-1)<<1)>>1);
+  }
+ 
+  // n should not include the center value
+  if ((n % 2) != 0) {
+      n--;
+  }
 
-  if ((n % 2) == 0) {
-    it = medianFilter->medianHead
-             ->prevValue;  // set iterator as value head previous
-    // first add previous values
-    for (i = 0; i < n / 2; i++) {
-      avgMedian += it->value;
-      it = medianFilter->medianHead->prevValue;
-    }
+  it = medianFilter->medianHead->prevValue;  // set iterator as value head previous
+  // first add previous values
+  for (i = 0; i < n / 2; i++) {
+    avgMedian += it->value;
+    it = it->prevValue;
+  }
 
-    it =
-        medianFilter->medianHead->nextValue;  // set iterator as value head next
-    // second add next values
-    for (i = 0; i < n / 2; i++) {
-      avgMedian += it->value;
-      it = medianFilter->medianHead->nextValue;
-    }
+  it = medianFilter->medianHead->nextValue;  // set iterator as value head next
+  // second add next values
+  for (i = 0; i < n / 2; i++) {
+    avgMedian += it->value;
+    it = it->nextValue;
   }
 
   avgMedian += medianFilter->medianHead->value;
-
-  if (n > 0) {
-    avgMedian /= (n + 1);
-  }
+  avgMedian /= (n + 1);
 
   return avgMedian;
 }
@@ -146,8 +154,11 @@ int64_t MEDIANFILTER_get_median(sMedianFilter_t *medianFilter, uint32_t n) {
 /**
  *
  */
-uint32_t MEDIANFILTER_isFull(sMedianFilter_t *medianFilter) {
-  if (medianFilter->bufferCnt >= medianFilter->numNodes) {
+uint32_t MEDIANFILTER_isFull(sMedianFilter_t *medianFilter, uint32_t n) {
+  if (n < 1 || n > medianFilter->numNodes) {
+      n = medianFilter->numNodes;
+  }
+  if (medianFilter->bufferCnt >= n) {
     return 1;
   } else {
     return 0;

components/libmedian/include/MedianFilter.h

@@ -37,7 +37,8 @@ typedef struct {
 int MEDIANFILTER_Init(sMedianFilter_t *medianFilter);
 int64_t MEDIANFILTER_Insert(sMedianFilter_t *medianFilter, int64_t sample);
 int64_t MEDIANFILTER_get_median(sMedianFilter_t *medianFilter, uint32_t n);
-uint32_t MEDIANFILTER_isFull(sMedianFilter_t *medianFilter);
+uint32_t MEDIANFILTER_isFull(sMedianFilter_t *medianFilter, uint32_t n);
+
 
 #ifdef __cplusplus
 }

components/lightsnapcast/include/player.h

@@ -14,6 +14,7 @@
 #define CHNK_CTRL_CNT 2
 
 #define LATENCY_MEDIAN_FILTER_LEN 199
+#define LATENCY_MEDIAN_FILTER_FULL 19
 
 // set to 0 if you do not wish to be the median an average around actual
 // median average will be (LATENCY_MEDIAN_FILTER_LEN /

components/lightsnapcast/player.c

@@ -365,7 +365,7 @@ int32_t player_latency_insert(int64_t newValue) {
 
   medianValue = MEDIANFILTER_Insert(&latencyMedianFilter, newValue);
   if (xSemaphoreTake(latencyBufSemaphoreHandle, pdMS_TO_TICKS(0)) == pdTRUE) {
-    if (MEDIANFILTER_isFull(&latencyMedianFilter)) {
+    if (MEDIANFILTER_isFull(&latencyMedianFilter, LATENCY_MEDIAN_FILTER_FULL)) {
       latencyBuffFull = true;
 
       //      ESP_LOGI(TAG, "(full) latency median: %lldus", medianValue);
@@ -1417,7 +1417,7 @@ static void player_task(void *pvParameters) {
 
         // resync hard if we are getting very late / early.
         // rest gets tuned in through apll speed control
-        if ((msgWaiting == 0) || (MEDIANFILTER_isFull(&shortMedianFilter) &&
+        if ((msgWaiting == 0) || (MEDIANFILTER_isFull(&shortMedianFilter,0) &&
                                   (abs(shortMedian) > hardResyncThreshold)))
         //        if (msgWaiting == 0)
         {
@@ -1467,7 +1467,7 @@ static void player_task(void *pvParameters) {
 
 #if USE_SAMPLE_INSERTION  // WIP: insert samples to adjust sync
         if ((enableControlLoop == true) &&
-            (MEDIANFILTER_isFull(&shortMedianFilter))) {
+            (MEDIANFILTER_isFull(&shortMedianFilter,0))) {
           if (avg < -miniOffset) {  // we are early
             dir = -1;
             dir_insert_sample = -1;
@@ -1478,7 +1478,7 @@ static void player_task(void *pvParameters) {
         }
 #else  // use APLL to adjust sync
         if ((enableControlLoop == true) &&
-            (MEDIANFILTER_isFull(&shortMedianFilter))) {
+            (MEDIANFILTER_isFull(&shortMedianFilter,0))) {
           if ((shortMedian < -shortOffset) && (miniMedian < -miniOffset) &&
               (avg < -miniOffset)) {  // we are early
             dir = -1;