commit
db99ac760a
|
@ -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;
|
||||
}
|
|
@ -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_ */
|
Loading…
Reference in New Issue