diff options
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 49 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 17 |
2 files changed, 50 insertions, 16 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4c79c67b4..5bdf6c262 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -221,6 +221,8 @@ Mavlink::Mavlink() : Mavlink::~Mavlink() { + perf_free(_loop_perf); + if (_task_running) { /* task wakes up every 10ms or so at the longest */ _task_should_exit = true; @@ -393,14 +395,12 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); - Mavlink *inst = ::_mavlink_instances; - - while (inst != nullptr) { - - mavlink_logbuffer_write(&inst->_logbuffer, &msg); - inst->_total_counter++; - inst = inst->next; - + Mavlink *inst; + LL_FOREACH(_mavlink_instances, inst) { + if (!inst->_task_should_exit) { + mavlink_logbuffer_write(&inst->_logbuffer, &msg); + inst->_total_counter++; + } } return OK; @@ -1557,9 +1557,6 @@ Mavlink::task_main(int argc, char *argv[]) warnx("start"); fflush(stdout); - /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&_logbuffer, 5); - int ch; _baudrate = 57600; _datarate = 0; @@ -1706,6 +1703,9 @@ Mavlink::task_main(int argc, char *argv[]) return ERROR; } + /* initialize mavlink text message buffering */ + mavlink_logbuffer_init(&_logbuffer, 5); + /* create the device node that's used for sending text log messages, etc. */ register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); @@ -1867,6 +1867,30 @@ Mavlink::task_main(int argc, char *argv[]) delete _subscribe_to_stream; _subscribe_to_stream = nullptr; + /* delete streams */ + MavlinkStream *stream_to_del = nullptr; + MavlinkStream *stream_next = _streams; + + while (stream_next != nullptr) { + stream_to_del = stream_next; + stream_next = stream_to_del->next; + delete stream_to_del; + } + + _streams = nullptr; + + /* delete subscriptions */ + MavlinkOrbSubscription *sub_to_del = nullptr; + MavlinkOrbSubscription *sub_next = _subscriptions; + + while (sub_next != nullptr) { + sub_to_del = sub_next; + sub_next = sub_to_del->next; + delete sub_to_del; + } + + _subscriptions = nullptr; + warnx("waiting for UART receive thread"); /* wait for threads to complete */ @@ -1878,6 +1902,9 @@ Mavlink::task_main(int argc, char *argv[]) /* close UART */ close(_uart_fd); + /* close mavlink logging device */ + close(_mavlink_fd); + /* destroy log buffer */ mavlink_logbuffer_destroy(&_logbuffer); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c9fcc4cd8..fa63e06c5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -83,7 +83,7 @@ __BEGIN_DECLS __END_DECLS -static const float mg2ms2 = 9.8f / 1000.0f; +static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), @@ -118,6 +118,10 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : memset(&hil_local_pos, 0, sizeof(hil_local_pos)); } +MavlinkReceiver::~MavlinkReceiver() +{ +} + void MavlinkReceiver::handle_message(mavlink_message_t *msg) { @@ -828,9 +832,9 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) memset(&accel, 0, sizeof(accel)); accel.timestamp = timestamp; - accel.x_raw = hil_state.xacc / 9.81f * 1e3f; - accel.y_raw = hil_state.yacc / 9.81f * 1e3f; - accel.z_raw = hil_state.zacc / 9.81f * 1e3f; + accel.x_raw = hil_state.xacc / CONSTANTS_ONE_G * 1e3f; + accel.y_raw = hil_state.yacc / CONSTANTS_ONE_G * 1e3f; + accel.z_raw = hil_state.zacc / CONSTANTS_ONE_G * 1e3f; accel.x = hil_state.xacc; accel.y = hil_state.yacc; accel.z = hil_state.zacc; @@ -925,7 +929,10 @@ void MavlinkReceiver::print_status() void *MavlinkReceiver::start_helper(void *context) { MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); - return rcv->receive_thread(NULL); + + rcv->receive_thread(NULL); + + delete rcv; } pthread_t |