|
|
|
@ -32,6 +32,7 @@ struct TimeFilter { |
|
|
|
|
double feedback2_factor; |
|
|
|
|
double feedback3_factor; |
|
|
|
|
double integrator2_state; |
|
|
|
|
int count; |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
TimeFilter * ff_timefilter_new(double feedback2_factor, double feedback3_factor) |
|
|
|
@ -51,10 +52,12 @@ void ff_timefilter_destroy(TimeFilter *self) |
|
|
|
|
void ff_timefilter_reset(TimeFilter *self) |
|
|
|
|
{ |
|
|
|
|
self->cycle_time = 0; |
|
|
|
|
self->count = 0; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
double ff_timefilter_update(TimeFilter *self, double system_time, double period) |
|
|
|
|
{ |
|
|
|
|
self->count++; |
|
|
|
|
if (!self->cycle_time) { |
|
|
|
|
/// init loop
|
|
|
|
|
self->cycle_time = system_time; |
|
|
|
@ -65,7 +68,7 @@ double ff_timefilter_update(TimeFilter *self, double system_time, double period) |
|
|
|
|
loop_error = system_time - self->cycle_time; |
|
|
|
|
|
|
|
|
|
/// update loop
|
|
|
|
|
self->cycle_time += self->feedback2_factor * loop_error; |
|
|
|
|
self->cycle_time += FFMAX(self->feedback2_factor, 1.0/(self->count)) * loop_error; |
|
|
|
|
self->integrator2_state += self->feedback3_factor * loop_error / period; |
|
|
|
|
} |
|
|
|
|
return self->cycle_time; |
|
|
|
|