Exemple #1
0
void 
DistanceStatComputer::calc_incremental_speed(const fixed dt)
{  
  if (positive(dt) && positive(data.distance)) {
    if (av_dist.update(data.distance)) {
      fixed d_av = av_dist.average();
      av_dist.reset();

      for (unsigned i=0; i<(unsigned)(dt); i++) {
        fixed v = df.update(d_av) / N_AV;
        fixed v_f = v_lpf.update(v);
        data.speed_incremental = (is_positive? -v_f:v_f);
      }
    }
  } else {    
    reset_incremental_speed();
  }
}
void 
DistanceStatComputer::calc_incremental_speed(const fixed dt)
{  
  if ((dt+fixed_half>=fixed_one) && positive(data.distance)) {
    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) || !positive(data.distance)) {    
    reset_incremental_speed();
  }
}