track angle working, still debugging cross track error - do not fly this
version!