From 1130bc129cfa7f1c5427ce5021a61dc797a3fb58 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=83=C2=BCng?= <beat-kueng@gmx.net>
Date: Tue, 18 Jul 2017 15:27:14 +0200
Subject: [PATCH] fix fmu: make sure init() is called on the new task

This was only a problem when running as a task not on the work queue.
The problem was that init() opened the RC serial device, which was then
read in the main loop, which is a different context when run as a task.
---
 src/drivers/px4fmu/fmu.cpp | 29 +++++++++++++++++++----------
 1 file changed, 19 insertions(+), 10 deletions(-)

diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index ed17f4980e..7c6ba3a829 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -171,6 +171,11 @@ public:
 	/** @see ModuleBase::run() */
 	void run() override;
 
+	/**
+	 * run the main loop: if running as task, continuously iterate, otherwise execute only one single cycle
+	 */
+	void cycle();
+
 	/** @see ModuleBase::print_status() */
 	int print_status() override;
 
@@ -1062,7 +1067,7 @@ PX4FMU::cycle_trampoline(void *arg)
 		_object = dev;
 	}
 
-	dev->run();
+	dev->cycle();
 }
 
 void
@@ -1181,6 +1186,18 @@ PX4FMU::update_pwm_out_state(bool on)
 
 void
 PX4FMU::run()
+{
+	if (init() != 0) {
+		PX4_ERR("init failed");
+		exit_and_cleanup();
+		return;
+	}
+
+	cycle();
+}
+
+void
+PX4FMU::cycle()
 {
 	while (true) {
 
@@ -3355,15 +3372,7 @@ PX4FMU::fake(int argc, char *argv[])
 PX4FMU *PX4FMU::instantiate(int argc, char *argv[])
 {
 	// No arguments to parse. We also know that we should run as task
-	PX4FMU *dev = new PX4FMU(true);
-
-	if (dev && dev->init() != 0) {
-		PX4_ERR("init failed");
-		delete dev;
-		dev = nullptr;
-	}
-
-	return dev;
+	return new PX4FMU(true);
 }
 
 int PX4FMU::custom_command(int argc, char *argv[])