示例#1
0
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();
}
示例#2
0
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);
}
示例#4
0
void
DistanceStatComputer::CalcSpeed(DistanceStat &data, fixed time)
{
  if (positive(time) && data.IsDefined())
    data.speed = data.GetDistance() / time;
  else
    data.speed = fixed_zero;
}
示例#5
0
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);
}