From 22598b1a4b16d6b21748ec0103fc0999aac19737 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Fri, 28 Jun 2019 14:31:44 -0700 Subject: [PATCH] Plane: Reduce inerital nav dependence --- ArduPlane/ArduPlane.cpp | 2 +- ArduPlane/quadplane.cpp | 5 ++++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 45f5eb7d6f..32a0ae6b95 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -166,7 +166,7 @@ void Plane::ahrs_update() quadplane.check_yaw_reset(); // update inertial_nav for quadplane - quadplane.inertial_nav.update(G_Dt); + quadplane.inertial_nav.update(); } /* diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 3838c776e1..a0a63fedb9 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2254,7 +2254,10 @@ void QuadPlane::vtol_position_controller(void) void QuadPlane::setup_target_position(void) { const Location &loc = plane.next_WP_loc; - Location origin = inertial_nav.get_origin(); + Location origin; + if (!ahrs.get_origin(origin)) { + origin.zero(); + } motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); const Vector2f diff2d = origin.get_distance_NE(loc);