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); }