mavlink: memory leaks on exit fixed, minor fixes

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