private float FilterAbs(ISensorModel model, float brake)
{
// convert speed to m/s
float speed = (float)(model.GetSpeed() / 3.6f);
// when speed lower than min speed for abs do nothing
if (speed < AbsMinSpeed)
{
return brake;
}
// compute the speed of wheels in m/s
float slip = 0.0f;
for(int i = 0; i < 4; i++)
{
slip += (float)(model.GetWheelSpinVelocity()[i] * WheelRadius[i]);
}
// slip is the difference between actual speed of car and average speed of wheels
slip = speed - slip / 4.0f;
// when slip too high apply ABS
if (slip > AbsSlip)
{
brake = brake - (slip - AbsSlip) / AbsRange;
}
if(brake < 0)
{
return 0;
}
else
{
return brake;
}
}
