1 files changed, 129 insertions, 0 deletions
diff --git a/os/various/median.c b/os/various/median.c
new file mode 100644
index 0000000..6e2f99f
--- /dev/null
+++ b/os/various/median.c
@@ -0,0 +1,129 @@
+/*
+    ChibiOS-Contrib - Copyright (C) 2014...2019 Fabien Poussin
+
+    Licensed 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.
+*/
+
+#include "median.h"
+
+void median_init(median_t* conf, uint16_t stopper, pair_t* buffer, uint16_t size)
+{
+  conf->stopper = stopper;
+  conf->buffer = buffer;
+  conf->size = size;
+  pair_t small_tmp = {NULL, conf->stopper};
+  pair_t big_tmp = {&conf->small, 0};
+  conf->datpoint = conf->buffer;                  /* Pointer into circular buffer of data */
+  conf->small = small_tmp;                        /* Chain stopper */
+  conf->big = big_tmp;                            /* Pointer to head (largest) of linked list.*/
+}
+
+uint16_t median_filter(median_t* conf, uint16_t datum)
+{
+  pair_t *successor;                              /* Pointer to successor of replaced data item */
+  pair_t *scan;                                   /* Pointer used to scan down the sorted list */
+  pair_t *scanold;                                /* Previous value of scan */
+  pair_t *median;                                 /* Pointer to median */
+  uint16_t i;
+
+  if (datum == conf->stopper)
+  {
+    datum = conf->stopper + 1;                    /* No stoppers allowed. */
+  }
+
+  if ( (++conf->datpoint - conf->buffer) >= conf->size)
+  {
+    conf->datpoint = conf->buffer;               /* Increment and wrap data in pointer.*/
+  }
+
+  conf->datpoint->value = datum;                 /* Copy in new datum */
+  successor = conf->datpoint->point;             /* Save pointer to old value's successor */
+  median = &conf->big;                           /* Median initially to first in chain */
+  scanold = NULL;                                /* Scanold initially null. */
+  scan = &conf->big;                             /* Points to pointer to first (largest) datum in chain */
+
+  /* Handle chain-out of first item in chain as special case */
+  if (scan->point == conf->datpoint)
+  {
+    scan->point = successor;
+  }
+  scanold = scan;                                     /* Save this pointer and   */
+  scan = scan->point ;                                /* step down chain */
+
+  /* Loop through the chain, normal loop exit via break. */
+  for (i = 0 ; i < conf->size; ++i)
+  {
+    /* Handle odd-numbered item in chain  */
+    if (scan->point == conf->datpoint)
+    {
+      scan->point = successor;                      /* Chain out the old datum.*/
+    }
+
+    if (scan->value < datum)                        /* If datum is larger than scanned value,*/
+    {
+      conf->datpoint->point = scanold->point;             /* Chain it in here.  */
+      scanold->point = conf->datpoint;                    /* Mark it chained in. */
+      datum = conf->stopper;
+    };
+
+    /* Step median pointer down chain after doing odd-numbered element */
+    median = median->point;                       /* Step median pointer.  */
+    if (scan == &conf->small)
+    {
+        break;                                      /* Break at end of chain  */
+    }
+    scanold = scan;                               /* Save this pointer and   */
+    scan = scan->point;                           /* step down chain */
+
+    /* Handle even-numbered item in chain.  */
+    if (scan->point == conf->datpoint)
+    {
+      scan->point = successor;
+    }
+
+    if (scan->value < datum)
+    {
+      conf->datpoint->point = scanold->point;
+      scanold->point = conf->datpoint;
+      datum = conf->stopper;
+    }
+
+    if (scan == &conf->small)
+    {
+      break;
+    }
+
+    scanold = scan;
+    scan = scan->point;
+  }
+  return median->value;
+}
+
+uint16_t middle_of_3(uint16_t a, uint16_t b, uint16_t c)
+{
+  uint16_t middle;
+
+  if ((a <= b) && (a <= c))
+  {
+    middle = (b <= c) ? b : c;
+  }
+  else if ((b <= a) && (b <= c))
+  {
+    middle = (a <= c) ? a : c;
+  }
+  else
+  {
+    middle = (a <= b) ? a : b;
+  }
+  return middle;
+}