From 2a1013e8965e2a1e877b546837c062380be7ca63 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 5 Jan 2022 13:35:23 +0900
Subject: [PATCH] AR_WPNav: set desired lat accel and crosstrack to zero when
 disarmed

---
 libraries/AR_WPNav/AR_WPNav.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp
index 0b5ce3c54c..368baa5371 100644
--- a/libraries/AR_WPNav/AR_WPNav.cpp
+++ b/libraries/AR_WPNav/AR_WPNav.cpp
@@ -133,7 +133,9 @@ void AR_WPNav::update(float dt)
     float speed;
     if (!hal.util->get_soft_armed() || !_orig_and_dest_valid || !AP::ahrs().get_location(current_loc) || !_atc.get_forward_speed(speed)) {
         _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt);
+        _desired_lat_accel = 0.0f;
         _desired_turn_rate_rads = 0.0f;
+        _cross_track_error = 0;
         return;
     }