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