diff --git a/src/modules/events/send_event.cpp b/src/modules/events/send_event.cpp index 16d9869783..092064b658 100644 --- a/src/modules/events/send_event.cpp +++ b/src/modules/events/send_event.cpp @@ -39,10 +39,40 @@ #include 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(); diff --git a/src/modules/events/send_event.h b/src/modules/events/send_event.h index 9196bee408..607a907a21 100644 --- a/src/modules/events/send_event.h +++ b/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; };