update_v1.5: Code cleanup, continued PID tuning