|
|
@ -59,15 +59,15 @@ double ff_timefilter_update(TimeFilter *self, double system_time, double period) |
|
|
|
self->count++; |
|
|
|
self->count++; |
|
|
|
if (self->count==1) { |
|
|
|
if (self->count==1) { |
|
|
|
/// init loop
|
|
|
|
/// init loop
|
|
|
|
self->cycle_time = system_time; |
|
|
|
self->cycle_time = system_time; |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
double loop_error; |
|
|
|
double loop_error; |
|
|
|
self->cycle_time += self->clock_period * period; |
|
|
|
self->cycle_time += self->clock_period * period; |
|
|
|
/// calculate loop error
|
|
|
|
/// calculate loop error
|
|
|
|
loop_error = system_time - self->cycle_time; |
|
|
|
loop_error = system_time - self->cycle_time; |
|
|
|
|
|
|
|
|
|
|
|
/// update loop
|
|
|
|
/// update loop
|
|
|
|
self->cycle_time += FFMAX(self->feedback2_factor, 1.0/(self->count)) * loop_error; |
|
|
|
self->cycle_time += FFMAX(self->feedback2_factor, 1.0/(self->count)) * loop_error; |
|
|
|
self->clock_period += self->feedback3_factor * loop_error / period; |
|
|
|
self->clock_period += self->feedback3_factor * loop_error / period; |
|
|
|
} |
|
|
|
} |
|
|
|
return self->cycle_time; |
|
|
|
return self->cycle_time; |
|
|
|