2019-12-29 10:01:27 +00:00
|
|
|
/*
|
|
|
|
* 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"
|
2019-12-29 10:06:25 +00:00
|
|
|
#include "time-util.h"
|
2019-12-29 10:01:27 +00:00
|
|
|
|
|
|
|
#include <math.h>
|
|
|
|
|
|
|
|
double smooth(struct smooth* self, double input)
|
|
|
|
{
|
2019-12-29 10:06:25 +00:00
|
|
|
double now = gettime_s();
|
2019-12-29 10:01:27 +00:00
|
|
|
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;
|
|
|
|
}
|