Distance was calculated vs EKF origin instead of AHRS home
Pair-Programmed-With: Randy Mackay <rmackay9@yahoo.com>