From 346ae5b9f4fc2da13e6d890521f48768b6b6e8c2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Feb 2014 19:13:10 +0100 Subject: Mavlink: allow to stop (WIP) --- src/modules/mavlink/mavlink_main.cpp | 62 +++++++++++++++++++++------- src/modules/mavlink/mavlink_main.h | 5 ++- src/modules/mavlink/mavlink_orb_listener.cpp | 3 +- src/modules/mavlink/mavlink_orb_listener.h | 2 - src/modules/mavlink/mavlink_receiver.cpp | 5 +-- src/modules/mavlink/mavlink_receiver.h | 2 - 6 files changed, 54 insertions(+), 25 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cbbe3c31f..0d9d06190 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -250,6 +250,43 @@ Mavlink* Mavlink::get_instance(unsigned instance) return inst; } +int Mavlink::destroy_all_instances() +{ + /* start deleting from the end */ + Mavlink *inst_to_del = nullptr; + Mavlink *next_inst = ::_head; + + unsigned iterations = 0; + + warnx("waiting for instances to stop"); + while (next_inst != nullptr) { + + inst_to_del = next_inst; + next_inst = inst_to_del->_next; + + /* set flag to stop thread and wait for all threads to finish */ + inst_to_del->_task_should_exit = true; + while (inst_to_del->thread_running) { + printf("."); + usleep(10000); + iterations++; + + if (iterations > 10000) { + warnx("ERROR: Couldn't stop all mavlink instances."); + return ERROR; + } + } + delete inst_to_del; + } + + /* reset head */ + ::_head = nullptr; + + printf("\n"); + warnx("all instances stopped"); + return OK; +} + bool Mavlink::instance_exists(const char *device_name, Mavlink *self) { Mavlink* inst = ::_head; @@ -1495,9 +1532,9 @@ Mavlink::task_main(int argc, char *argv[]) device_name = optarg; break; - case 'e': - mavlink_link_termination_allowed = true; - break; +// case 'e': +// mavlink_link_termination_allowed = true; +// break; case 'o': _mode = MODE_ONBOARD; @@ -1749,7 +1786,7 @@ Mavlink::task_main(int argc, char *argv[]) tcsetattr(_uart, TCSANOW, &uart_config_original); /* destroy log buffer */ - //mavlink_logbuffer_destroy(&lb); + mavlink_logbuffer_destroy(&lb); thread_running = false; @@ -1782,7 +1819,7 @@ Mavlink::status() static void usage() { - errx(1, "usage: mavlink {start|stop|status}"); + errx(1, "usage: mavlink {start|stop|status} [-d device] [-b baudrate] [-o]"); } int mavlink_main(int argc, char *argv[]) @@ -1830,21 +1867,16 @@ int mavlink_main(int argc, char *argv[]) // } return 0; - } - - // if (mavlink::g_mavlink == nullptr) - // errx(1, "not running"); - // if (!strcmp(argv[1], "stop")) { - // delete mavlink::g_mavlink; - // mavlink::g_mavlink = nullptr; + } else if (!strcmp(argv[1], "stop")) { + return Mavlink::destroy_all_instances(); // } else if (!strcmp(argv[1], "status")) { // mavlink::g_mavlink->status(); - // } else { - // usage(); - // } + } else { + usage(); + } return 0; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index bf7675267..d5bbb746b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -168,6 +168,8 @@ public: static Mavlink* get_instance(unsigned instance); + static int destroy_all_instances(); + static bool instance_exists(const char *device_name, Mavlink *self); static int get_uart_fd(unsigned index); @@ -263,6 +265,8 @@ public: /** Position setpoint triplet */ struct position_setpoint_triplet_s pos_sp_triplet; + bool _task_should_exit; /**< if true, mavlink task should exit */ + protected: /** * Pointer to the default cdev file operations table; useful for @@ -273,7 +277,6 @@ protected: private: int _mavlink_fd; - bool _task_should_exit; /**< if true, mavlink task should exit */ bool thread_running; int _mavlink_task; /**< task handle for sensor task */ diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index cd9408f2f..a2b71c931 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -83,7 +83,6 @@ cm_uint16_from_m_float(float m) MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : - thread_should_exit(false), _loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")), _mavlink(parent), _listeners(nullptr), @@ -678,7 +677,7 @@ MavlinkOrbListener::uorb_receive_thread(void *arg) /* Invoke callback to set initial state */ //listeners[i].callback(&listener[i]); - while (!thread_should_exit) { + while (!_mavlink->_task_should_exit) { int poll_ret = poll(fds, _n_listeners, timeout); diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index 560b47423..317a489d4 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -116,8 +116,6 @@ public: private: - bool thread_should_exit; /**< if true, sensor task should exit */ - perf_counter_t _loop_perf; /**< loop performance counter */ Mavlink* _mavlink; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 89dcb1d7b..16a1b9aff 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -106,8 +106,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : telemetry_status_pub(-1), lat0(0), lon0(0), - alt0(0), - thread_should_exit(false) + alt0(0) { } @@ -807,7 +806,7 @@ MavlinkReceiver::receive_thread(void *arg) ssize_t nread = 0; - while (!thread_should_exit) { + while (!_mavlink->_task_should_exit) { if (poll(fds, 1, timeout) > 0) { if (nread < sizeof(buf)) { /* to avoid reading very small chunks wait for data before reading */ diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index d8d3b5452..6614e13f4 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -101,8 +101,6 @@ public: private: - bool thread_should_exit; /**< if true, sensor task should exit */ - perf_counter_t _loop_perf; /**< loop performance counter */ Mavlink* _mavlink; -- cgit v1.2.3