roboforum.ru

SonicSpeed = (331.4 + (0.6 * SonicTemperature));

D = SonicSpeed * SonicOffsetNorthSouth;
Vn = D / SonicTimeNorthSouth;
Vn = Vn - SonicSpeed;

D = SonicSpeed * SonicOffsetEastWest;
Ve = D / SonicTimeEastWest;
Ve = Ve - SonicSpeed;

MeterPerSecond = sqrt(pow(Vn,2) + pow(Ve,2));
Tmp = abs(MeterPerSecond * 3.6);
SonicKmh = (Tmp + (4 * SonicKmh)) / 5;

if (SonicKmh > 2) SonicAngle = abs(atan2(Vn,Ve) * 180 / _PI);