Copter: follow mode restores offsets to zero on exit

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) {

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; }

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