Fixed some mathematical issues

Dependents:   GPSNavigationNew

Fork of PID by Aaron Berk

no such method: tip