From 49a3d9b0f7867f7684ff50b03b40a77e4b801bd9 Mon Sep 17 00:00:00 2001 From: Fabien Poussin Date: Thu, 10 Jan 2019 11:12:14 +0100 Subject: Adding median library --- os/various/median.c | 129 ++++++++++++++++++++++++++++++++++++++++++++++++++++ os/various/median.h | 42 +++++++++++++++++ 2 files changed, 171 insertions(+) create mode 100644 os/various/median.c create mode 100644 os/various/median.h (limited to 'os') 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; +} diff --git a/os/various/median.h b/os/various/median.h new file mode 100644 index 0000000..c74cb66 --- /dev/null +++ b/os/various/median.h @@ -0,0 +1,42 @@ +/* + 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. +*/ + +#ifndef MEDIAN_H_ +#define MEDIAN_H_ + +#include "ch.h" + +typedef struct pair_ +{ + struct pair_* point; /* Pointers forming list linked in sorted order */ + uint16_t value; /* Values to sort */ +} pair_t; + +typedef struct +{ + uint16_t stopper; /* Smaller than any datum */ + uint16_t size; /* 3 or more */ + pair_t* buffer; /* Buffer of nwidth pairs */ + pair_t* datpoint; /* Pointer into circular buffer of data */ + pair_t small; /* Chain stopper */ + pair_t big; /* Pointer to head (largest) of linked list.*/ +} median_t; + +void median_init(median_t* conf, uint16_t stopper, pair_t* buffer, uint16_t size); +uint16_t median_filter(median_t* conf, uint16_t datum); +uint16_t middle_of_3(uint16_t a, uint16_t b, uint16_t c); + +#endif /* MEDIAN_H_ */ -- cgit v1.2.3