double filter(double x)
{
static double l = 0.0;
- l = 0.001 * x + 0.999 * l;
+ l = 0.01 * x + 0.99 * l;
return l;
}
double speed = 1.0;
while (!feof(in1) && !feof(in2)) {
+ short refs;
+
// read sample from reference
- if (fread(&sample, sizeof(short), 1, in1) != 1) {
+ if (fread(&refs, sizeof(short), 1, in1) != 1) {
exit(0);
}
// linear interpolation (works well since delta_p varies so slowly)
double t = frac(p);
- short intp_sample = prev_sample * (1.0 - t) + sample * t;
+ double intp_sample = prev_sample * (1.0 - t) + sample * t;
// subtract the two samples and pull it through a filter. we assume
// the sines are normalized for now (and that there's no dc skew)
- double offset = -0.001 * filter(intp_sample - sample);
- speed += offset;
+ double offset = filter(intp_sample - refs);
+ speed -= 0.00000001 * offset;
fwrite(&speed, sizeof(double), 1, skew);
- printf("%f (offset=%f)\n", speed, offset);
+ printf("%f (offset=%f)\n", speed, intp_sample - sample);
}
}