fw pos ctrl: Only return from start handler with all allocations done

1 files changed, 23 insertions, 3 deletions

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 98ccd09a5..08c996ebc 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: 	Lorenz Meier
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -120,10 +119,18 @@ public:
 	 */
 	int		start();
 
+	/**
+	 * Task status
+	 *
+	 * @return	true if the mainloop is running
+	 */
+	bool		task_running() { return _task_running; }
+
 private:
 	int		_mavlink_fd;
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	bool		_task_running;			/**< if true, task is running in its mainloop */
 	int		_control_task;			/**< task handle for sensor task */
 
 	int		_global_pos_sub;
@@ -391,13 +398,14 @@ namespace l1_control
 #endif
 static const int ERROR = -1;
 
-FixedwingPositionControl	*g_control;
+FixedwingPositionControl	*g_control = nullptr;
 }
 
 FixedwingPositionControl::FixedwingPositionControl() :
 
 	_mavlink_fd(-1),
 	_task_should_exit(false),
+	_task_running(false),
 	_control_task(-1),
 
 /* subscriptions */
@@ -1290,6 +1298,8 @@ FixedwingPositionControl::task_main()
 	fds[1].fd = _global_pos_sub;
 	fds[1].events = POLLIN;
 
+	_task_running = true;
+
 	while (!_task_should_exit) {
 
 		/* wait for up to 500ms for data */
@@ -1390,6 +1400,8 @@ FixedwingPositionControl::task_main()
 		perf_end(_loop_perf);
 	}
 
+	_task_running = false;
+
 	warnx("exiting.\n");
 
 	_control_task = -1;
@@ -1478,6 +1490,14 @@ int fw_pos_control_l1_main(int argc, char *argv[])
 			err(1, "start failed");
 		}
 
+		/* avoid memory fragmentation by not exiting start handler until the task has fully started */
+		while (l1_control::g_control == nullptr || !l1_control::g_control->task_running()) {
+			usleep(50000);
+			printf(".");
+			fflush(stdout);
+		}
+		printf("\n");
+
 		exit(0);
 	}