42 lines
1.3 KiB
C
42 lines
1.3 KiB
C
|
/*
|
||
|
|
* Copyright (c) 2019 Andri Yngvason
|
||
|
|
*
|
||
|
|
* Permission to use, copy, modify, and/or distribute this software for any
|
||
|
|
* purpose with or without fee is hereby granted, provided that the above
|
||
|
|
* copyright notice and this permission notice appear in all copies.
|
||
|
|
*
|
||
|
|
* THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH
|
||
|
|
* REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
|
||
|
|
* AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT,
|
||
|
|
* INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
|
||
|
|
* LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE
|
||
|
|
* OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
|
||
|
|
* PERFORMANCE OF THIS SOFTWARE.
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "smooth.h"
|
||
|
|
|
||
|
|
#include <math.h>
|
||
|
|
#include <time.h>
|
||
|
|
|
||
|
|
static inline double smooth__gettime(void)
|
||
|
|
{
|
||
|
|
struct timespec ts = { 0 };
|
||
|
|
clock_gettime(CLOCK_MONOTONIC, &ts);
|
||
|
|
return (double)ts.tv_sec + (double)ts.tv_nsec / 1.0e-9;
|
||
|
|
}
|
||
|
|
|
||
|
|
double smooth(struct smooth* self, double input)
|
||
|
|
{
|
||
|
|
double now = smooth__gettime();
|
||
|
|
double dt = now - self->last_time;
|
||
|
|
self->last_time = now;
|
||
|
|
|
||
|
|
double factor = 1.0 - exp(-dt / self->time_constant);
|
||
|
|
|
||
|
|
double result = factor * input + (1.0 - factor) * self->last_result;
|
||
|
|
self->last_result = result;
|
||
|
|
|
||
|
|
return result;
|
||
|
|
}
|