From 1cfa9d924d5eb58818f5c874e49085b6b8dfc025 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 22 Jan 2016 12:07:17 +0100
Subject: [PATCH] Fixed ekf2 stop / start routine

---
 src/modules/ekf2/ekf2_main.cpp | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 6c5793ecf0..521193336c 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -115,6 +115,8 @@ public:
 
 	void print_status();
 
+	void exit() { _task_should_exit = true; }
+
 private:
 	static constexpr float _dt_max = 0.02;
 	bool		_task_should_exit = false;		/**< if true, task should exit */
@@ -471,6 +473,9 @@ void Ekf2::task_main()
 			}
 		}
 	}
+
+	delete ekf2::instance;
+	ekf2::instance = nullptr;
 }
 
 void Ekf2::task_main_trampoline(int argc, char *argv[])
@@ -535,8 +540,13 @@ int ekf2_main(int argc, char *argv[])
 			return 1;
 		}
 
-		delete ekf2::instance;
-		ekf2::instance = nullptr;
+		ekf2::instance->exit();
+
+		// wait for the destruction of the instance
+		while (ekf2::instance != nullptr) {
+			usleep(50000);
+		}
+
 		return 0;
 	}