Bug fixes, timing, some sensor fixes, PID coefficients