was drifting toward home by some distance as (0,0,0) was hard-coded in
control_rtl.pde. Fixed, tested in SITL