diff --git a/ArduCopter/position_vector.pde b/ArduCopter/position_vector.pde index addd9888ad..967f1fb326 100644 --- a/ArduCopter/position_vector.pde +++ b/ArduCopter/position_vector.pde @@ -10,8 +10,8 @@ // pv_location_to_vector - convert lat/lon coordinates to a position vector Vector3f pv_location_to_vector(const Location& loc) { - const struct Location &temp_home = ahrs.get_home(); - Vector3f tmp((loc.lat-temp_home.lat) * LATLON_TO_CM, (loc.lng-temp_home.lng) * LATLON_TO_CM * scaleLongDown, loc.alt); + const struct Location &origin = inertial_nav.get_origin(); + Vector3f tmp((loc.lat-origin.lat) * LATLON_TO_CM, (loc.lng-origin.lng) * LATLON_TO_CM * scaleLongDown, loc.alt); return tmp; }