From ca92f0505e4edbb52c1bc5d300f32dd066a02747 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 4 Oct 2019 10:24:51 +0900
Subject: [PATCH] Copter: follow mode restores offsets to zero on exit

---
 ArduCopter/mode.cpp        | 6 ++++++
 ArduCopter/mode.h          | 1 +
 ArduCopter/mode_follow.cpp | 6 ++++++
 3 files changed, 13 insertions(+)

diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp
index ae2e74bb4a..3fbb41e20d 100644
--- a/ArduCopter/mode.cpp
+++ b/ArduCopter/mode.cpp
@@ -314,6 +314,12 @@ void Copter::exit_mode(Mode *&old_flightmode,
     }
 #endif
 
+#if MODE_FOLLOW_ENABLED == ENABLED
+    if (old_flightmode == &mode_follow) {
+        mode_follow.exit();
+    }
+#endif
+
 #if FRAME_CONFIG == HELI_FRAME
     // firmly reset the flybar passthrough to false when exiting acro mode.
     if (old_flightmode == &mode_acro) {
diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index 66cee84621..eb71ac9ef0 100644
--- a/ArduCopter/mode.h
+++ b/ArduCopter/mode.h
@@ -1256,6 +1256,7 @@ public:
     using ModeGuided::Mode;
 
     bool init(bool ignore_checks) override;
+    void exit();
     void run() override;
 
     bool requires_GPS() const override { return true; }
diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp
index a770829fcf..a17212c465 100644
--- a/ArduCopter/mode_follow.cpp
+++ b/ArduCopter/mode_follow.cpp
@@ -24,6 +24,12 @@ bool ModeFollow::init(const bool ignore_checks)
     return ModeGuided::init(ignore_checks);
 }
 
+// perform cleanup required when leaving follow mode
+void ModeFollow::exit()
+{
+    g2.follow.clear_offsets_if_required();
+}
+
 void ModeFollow::run()
 {
     // if not armed set throttle to zero and exit immediately