Fixed everything except yaw tuning.