From ccb30d44ac50263eaf44375291f341327a86c3a9 Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Wed, 19 Sep 2018 12:21:16 +0930
Subject: [PATCH] Sub: Missing Loiter Init on accel

---
 ArduSub/control_auto.cpp    | 1 +
 ArduSub/control_poshold.cpp | 3 +++
 2 files changed, 4 insertions(+)

diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp
index 7a257afb88..c38beb0647 100644
--- a/ArduSub/control_auto.cpp
+++ b/ArduSub/control_auto.cpp
@@ -646,6 +646,7 @@ bool Sub::auto_terrain_recover_start()
     mission.stop();
 
     // Reset xy target
+    loiter_nav.clear_pilot_desired_acceleration();
     loiter_nav.init_target();
 
     // Reset z axis controller
diff --git a/ArduSub/control_poshold.cpp b/ArduSub/control_poshold.cpp
index b856e5b740..f27b2d5894 100644
--- a/ArduSub/control_poshold.cpp
+++ b/ArduSub/control_poshold.cpp
@@ -24,6 +24,7 @@ bool Sub::poshold_init()
 
     // set target to current position
     // only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I
+    loiter_nav.clear_pilot_desired_acceleration();
     loiter_nav.init_target();
 
     last_pilot_heading = ahrs.yaw_sensor;
@@ -40,6 +41,7 @@ void Sub::poshold_run()
     // if not armed set throttle to zero and exit immediately
     if (!motors.armed()) {
         motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
+        loiter_nav.clear_pilot_desired_acceleration();
         loiter_nav.init_target();
         attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
         pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
@@ -64,6 +66,7 @@ void Sub::poshold_run()
     if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) {
         lateral_out = pilot_lateral;
         forward_out = pilot_forward;
+        loiter_nav.clear_pilot_desired_acceleration();
         loiter_nav.init_target(); // initialize target to current position after repositioning
     } else {
         translate_wpnav_rp(lateral_out, forward_out);