49 lines
892 B
C
49 lines
892 B
C
|
|
#include <stdint.h>
|
|
#include <ti_msp_dl_config.h>
|
|
|
|
float y = 0;
|
|
float Lowpassfilter_sim(float x)
|
|
{
|
|
float out = 0.9 * x + 0.1 * y;
|
|
y = x;
|
|
return out;
|
|
}
|
|
|
|
uint32_t Last_Timesamp = 0.0;
|
|
float Last_y = 0.0;
|
|
float Lowpassfilter(float Tf, float x)
|
|
{
|
|
float dt = 0.0;
|
|
|
|
uint32_t Timesamp = DL_SYSTICK_getValue();
|
|
if(Timesamp < Last_Timesamp)
|
|
{
|
|
dt = (float)(Last_Timesamp - Timesamp) / 80 * 1e-6;
|
|
}
|
|
else
|
|
{
|
|
dt = (float)(0xFFFFFF - Timesamp + Last_Timesamp) / 80 * 1e-6;
|
|
}
|
|
|
|
if(dt < 0.0 || dt == 0)
|
|
{
|
|
dt = 0.0015f;
|
|
}
|
|
else if(dt > 0.005f)
|
|
{
|
|
Last_y = x;
|
|
Last_Timesamp = Timesamp;
|
|
return x;
|
|
}
|
|
float alpha = Tf / (Tf + dt);
|
|
float y = alpha * Last_y + (1.0f - alpha) * x;
|
|
|
|
Last_y = y;
|
|
Last_Timesamp = Timesamp;
|
|
|
|
|
|
return y;
|
|
}
|
|
|