static void
CalculatePirker(DistanceStat &pirker, const DistanceStat &planned,
const DistanceStat &remaining_effective)
{
if (planned.IsDefined() && remaining_effective.IsDefined())
pirker.set_distance(planned.get_distance() -
remaining_effective.get_distance());
else
pirker.Reset();
}
void
DistanceStatComputer::ResetIncrementalSpeed(DistanceStat &data)
{
fixed distance = data.IsDefined() ? data.GetDistance() : fixed_zero;
fixed speed = data.IsDefined() ? data.GetSpeed() : fixed_zero;
df.Reset(distance, (is_positive ? -1 : 1) * speed);
v_lpf.Reset((is_positive ? -1 : 1) * speed);
data.speed_incremental = fixed_zero; // data.speed;
av_dist.Reset();
}
void
IncrementalSpeedComputer::Reset(DistanceStat &data)
{
fixed distance = data.IsDefined() ? data.GetDistance() : fixed(0);
fixed speed = data.IsDefined() ? data.GetSpeed() : fixed(0);
df.Reset(distance, (is_positive ? -1 : 1) * speed);
v_lpf.Reset((is_positive ? -1 : 1) * speed);
data.speed_incremental = fixed(0); // data.speed;
av_dist.Reset();
last_time = fixed(-1);
}
void
DistanceStatComputer::CalcSpeed(DistanceStat &data, fixed time)
{
if (positive(time) && data.IsDefined())
data.speed = data.GetDistance() / time;
else
data.speed = fixed_zero;
}
void
DistanceStatComputer::CalcIncrementalSpeed(DistanceStat &data, const fixed dt)
{
if ((dt + fixed_half >= fixed_one) && data.IsDefined()) {
if (av_dist.Update(data.distance)) {
const fixed d_av = av_dist.Average() / N_AV;
av_dist.Reset();
fixed v_f = fixed_zero;
for (unsigned i = 0; i < (unsigned)(dt + fixed_half); ++i) {
const fixed v = df.Update(d_av);
v_f = v_lpf.Update(v);
}
data.speed_incremental = (is_positive ? -v_f : v_f);
}
} else if (!positive(dt) || !data.IsDefined()) {
ResetIncrementalSpeed(data);
}
}
void
IncrementalSpeedComputer::Compute(DistanceStat &data, const fixed time)
{
if (!data.IsDefined() || negative(time) ||
(!negative(last_time) && (time < last_time ||
time > last_time + fixed(60)))) {
Reset(data);
return;
}
if (negative(last_time)) {
last_time = time;
return;
}
const fixed dt = time - last_time;
const unsigned seconds = uround(dt);
if (seconds == 0)
return;
if (!av_dist.Update(data.distance))
return;
const fixed d_av = av_dist.Average();
av_dist.Reset();
fixed v_f = fixed(0);
for (unsigned i = 0; i < seconds; ++i) {
const fixed v = df.Update(d_av);
v_f = v_lpf.Update(v);
}
last_time += fixed(seconds);
data.speed_incremental = (is_positive ? -v_f : v_f);
}