send_event: separate initialization and wait until started for 'send_event start_listening'

src/modules/events/send_event.cpp

@@ -39,10 +39,40 @@
 #include <drivers/drv_hrt.h>
 
 static SendEvent *send_event_obj = nullptr;
+struct work_s SendEvent::_work = {};
 
 // Run it at 30 Hz.
 const unsigned SEND_EVENT_INTERVAL_US = 33000;
 
+
+int SendEvent::initialize()
+{
+	int ret = work_queue(LPWORK, &_work, (worker_t)&SendEvent::initialize_trampoline, nullptr, 0);
+
+	if (ret < 0) {
+		return ret;
+	}
+
+	int i = 0;
+
+	do {
+		/* wait up to 1s */
+		usleep(2500);
+
+	} while ((!send_event_obj || !send_event_obj->is_running()) && ++i < 400);
+
+	if (i == 400) {
+		PX4_ERR("failed to start");
+		return -1;
+	}
+
+	return 0;
+}
+
+SendEvent::SendEvent()
+{
+}
+
 int SendEvent::start()
 {
 	if (_task_is_running) {
@@ -52,8 +82,12 @@ int SendEvent::start()
 	_task_is_running = true;
 	_task_should_exit = false;
 
-	/* Schedule a cycle to start things. */
-	return work_queue(LPWORK, &_work, (worker_t)&SendEvent::cycle_trampoline, this, 0);
+	_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
+
+	// Kick off the cycling. We can call it directly because we're already in the work queue context
+	cycle();
+
+	return 0;
 }
 
 void SendEvent::stop()
@@ -77,6 +111,18 @@ void SendEvent::stop()
 	}
 }
 
+void SendEvent::initialize_trampoline(void *arg)
+{
+	send_event_obj = new SendEvent();
+
+	if (!send_event_obj) {
+		PX4_ERR("alloc failed");
+		return;
+	}
+
+	send_event_obj->start();
+}
+
 void
 SendEvent::cycle_trampoline(void *arg)
 {
@@ -97,11 +143,6 @@ void SendEvent::cycle()
 		return;
 	}
 
-	// check if not yet initialized. we have to do it here, because it's running in a different context than initialisation
-	if (_vehicle_command_sub < 0) {
-		_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
-	}
-
 	process_commands();
 
 	work_queue(LPWORK, &_work, (worker_t)&SendEvent::cycle_trampoline, this,
@@ -206,18 +247,10 @@ int send_event_main(int argc, char *argv[])
 		if (send_event_obj) {
 			PX4_INFO("already running");
 			return -1;
-
-		} else {
-			send_event_obj = new SendEvent();
-
-			if (!send_event_obj) {
-				PX4_ERR("alloc failed");
-				return -1;
-			}
-
-			return send_event_obj->start();
 		}
 
+		return SendEvent::initialize();
+
 	} else if (!strcmp(argv[1], "stop_listening")) {
 		if (send_event_obj) {
 			send_event_obj->stop();

src/modules/events/send_event.h

@@ -42,10 +42,12 @@ extern "C" __EXPORT int send_event_main(int argc, char *argv[]);
 class SendEvent
 {
 public:
-	/** Start background listening for commands
+	SendEvent();
+
+	/** Initialize class in the same context as the work queue. And start the background listener.
 	 *
-	 * @return 0 if successfull, -1 on error. */
-	int start();
+	 * @return 0 if successful, <0 on error */
+	static int initialize();
 
 	/** Stop background listener */
 	void stop();
@@ -55,6 +57,15 @@ public:
 	void print_status();
 
 private:
+
+	/** Start background listening for commands
+	 *
+	 * @return 0 if successful, <0 on error. */
+	int start();
+
+
+	/** Trampoline for initialisation. */
+	static void initialize_trampoline(void *arg);
 	/** Trampoline for the work queue. */
 	static void cycle_trampoline(void *arg);
 
@@ -69,7 +80,7 @@ private:
 
 	volatile bool _task_should_exit = false;
 	volatile bool _task_is_running = false;
-	struct work_s _work = {};
+	static struct work_s _work;
 	int _vehicle_command_sub = -1;
 	orb_advert_t _command_ack_pub = nullptr;
 };