diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index cb620349d9..f1ec6e8dce 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -153,6 +153,7 @@ namespace mavlink
 
 Mavlink::Mavlink() :
 //	_mavlink_fd(-1),
+	_next(nullptr),
 	_task_should_exit(false),
 	thread_running(false),
 	_mavlink_task(-1),
@@ -1448,6 +1449,8 @@ Mavlink::task_main(int argc, char *argv[])
 	_baudrate = 57600;
 	_chan = MAVLINK_COMM_0;
 
+	_mode = MODE_OFFBOARD;
+
 	/* work around some stupidity in task_create's argv handling */
 	argc -= 2;
 	argv += 2;
@@ -1488,7 +1491,23 @@ Mavlink::task_main(int argc, char *argv[])
 	warnx("MAVLink v1.0 serial interface starting...");
 
 	/* inform about mode */
-	warnx((_mode == MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
+	switch (_mode) {
+		case MODE_TX_HEARTBEAT_ONLY:
+			warnx("MODE_TX_HEARTBEAT_ONLY");
+			break;
+		case MODE_OFFBOARD:
+			warnx("MODE_OFFBOARD");
+			break;
+		case MODE_ONBOARD:
+			warnx("MODE_ONBOARD");
+			break;
+		case MODE_HIL:
+			warnx("MODE_HIL");
+			break;
+		default:
+			warnx("Error: Unknown mode");
+			break;
+	}
 
 	/* Flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
@@ -1506,12 +1525,12 @@ Mavlink::task_main(int argc, char *argv[])
 	mavlink_update_system();
 
 	/* start the MAVLink receiver */
-	// MavlinkReceiver rcv(this);
-	//receive_thread = MavlinkReceiver::receive_start(this);
+//	MavlinkReceiver rcv(this);
+	receive_thread = MavlinkReceiver::receive_start(this);
 
 	/* start the ORB receiver */
-	//MavlinkOrbListener listener(this);
-	//uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this);
+//	MavlinkOrbListener listener(this);
+	uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this);
 
 	/* initialize waypoint manager */
 	mavlink_wpm_init(wpm);
diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp
index 6d64569de5..418e1c1219 100644
--- a/src/modules/mavlink/mavlink_orb_listener.cpp
+++ b/src/modules/mavlink/mavlink_orb_listener.cpp
@@ -36,6 +36,7 @@
  * Monitors ORB topics and sends update messages as appropriate.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 // XXX trim includes
@@ -80,7 +81,7 @@ cm_uint16_from_m_float(float m)
 	return (uint16_t)(m * 100.0f);
 }
 
-MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : 
+MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) :
 
 	thread_should_exit(false),
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")),
@@ -88,31 +89,31 @@ MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) :
 	_listeners(nullptr),
 	_n_listeners(0)
 {
-	add_listener(MavlinkOrbListener::l_sensor_combined,		&_mavlink->get_subs()->sensor_sub,			0);
-	add_listener(MavlinkOrbListener::l_vehicle_attitude,		&_mavlink->get_subs()->att_sub,			0);
-	add_listener(MavlinkOrbListener::l_vehicle_gps_position,	&_mavlink->get_subs()->gps_sub,				0);
-	add_listener(MavlinkOrbListener::l_vehicle_status,		&status_sub,						0);
-	add_listener(MavlinkOrbListener::l_rc_channels,			&rc_sub,			0);
-	add_listener(MavlinkOrbListener::l_input_rc,			&_mavlink->get_subs()->input_rc_sub,		0);
-	add_listener(MavlinkOrbListener::l_global_position,		&_mavlink->get_subs()->global_pos_sub,			0);
-	add_listener(MavlinkOrbListener::l_local_position,		&_mavlink->get_subs()->local_pos_sub,			0);
-	add_listener(MavlinkOrbListener::l_global_position_setpoint,	&_mavlink->get_subs()->triplet_sub,		0);
-	add_listener(MavlinkOrbListener::l_local_position_setpoint,	&_mavlink->get_subs()->spl_sub,				0);
-	add_listener(MavlinkOrbListener::l_attitude_setpoint,		&_mavlink->get_subs()->spa_sub,			0);
-	add_listener(MavlinkOrbListener::l_actuator_outputs,		&_mavlink->get_subs()->act_0_sub,		0);
-	add_listener(MavlinkOrbListener::l_actuator_outputs,		&_mavlink->get_subs()->act_1_sub,		1);
-	add_listener(MavlinkOrbListener::l_actuator_outputs,		&_mavlink->get_subs()->act_2_sub,		2);
-	add_listener(MavlinkOrbListener::l_actuator_outputs,		&_mavlink->get_subs()->act_3_sub,		3);
-	add_listener(MavlinkOrbListener::l_actuator_armed,		&_mavlink->get_subs()->armed_sub,			0);
-	add_listener(MavlinkOrbListener::l_manual_control_setpoint,	&_mavlink->get_subs()->man_control_sp_sub, 		0);
-	add_listener(MavlinkOrbListener::l_vehicle_attitude_controls,	&_mavlink->get_subs()->actuators_sub,		0);
-	add_listener(MavlinkOrbListener::l_debug_key_value,		&_mavlink->get_subs()->debug_key_value,			0);
-	add_listener(MavlinkOrbListener::l_optical_flow,		&_mavlink->get_subs()->optical_flow,			0);
-	add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint,	&_mavlink->get_subs()->rates_setpoint_sub,		0);
-	add_listener(MavlinkOrbListener::l_home,			&_mavlink->get_subs()->home_sub,			0);
-	add_listener(MavlinkOrbListener::l_airspeed,		&_mavlink->get_subs()->airspeed_sub,			0);
-	add_listener(MavlinkOrbListener::l_nav_cap,			&_mavlink->get_subs()->navigation_capabilities_sub,	0);
-	add_listener(MavlinkOrbListener::l_control_mode,		&_mavlink->get_subs()->control_mode_sub,		0);
+	add_listener(MavlinkOrbListener::l_sensor_combined,		&(_mavlink->get_subs()->sensor_sub),			0);
+	add_listener(MavlinkOrbListener::l_vehicle_attitude,		&(_mavlink->get_subs()->att_sub),			0);
+	add_listener(MavlinkOrbListener::l_vehicle_gps_position,	&(_mavlink->get_subs()->gps_sub),			0);
+	add_listener(MavlinkOrbListener::l_vehicle_status,		&(_mavlink->get_subs()->status_sub),			0);
+	add_listener(MavlinkOrbListener::l_home,			&(_mavlink->get_subs()->home_sub),			0);
+	add_listener(MavlinkOrbListener::l_control_mode,		&(_mavlink->get_subs()->control_mode_sub),		0);
+	add_listener(MavlinkOrbListener::l_rc_channels,			&(_mavlink->get_subs()->rc_sub),			0);
+	add_listener(MavlinkOrbListener::l_input_rc,			&(_mavlink->get_subs()->input_rc_sub),			0);
+	add_listener(MavlinkOrbListener::l_global_position,		&(_mavlink->get_subs()->global_pos_sub),		0);
+	add_listener(MavlinkOrbListener::l_local_position,		&(_mavlink->get_subs()->local_pos_sub),			0);
+	add_listener(MavlinkOrbListener::l_global_position_setpoint,	&(_mavlink->get_subs()->triplet_sub),			0);
+	add_listener(MavlinkOrbListener::l_local_position_setpoint,	&(_mavlink->get_subs()->spl_sub),			0);
+	add_listener(MavlinkOrbListener::l_attitude_setpoint,		&(_mavlink->get_subs()->spa_sub),			0);
+	add_listener(MavlinkOrbListener::l_actuator_outputs,		&(_mavlink->get_subs()->act_0_sub),			0);
+	add_listener(MavlinkOrbListener::l_actuator_outputs,		&(_mavlink->get_subs()->act_1_sub),			1);
+	add_listener(MavlinkOrbListener::l_actuator_outputs,		&(_mavlink->get_subs()->act_2_sub),			2);
+	add_listener(MavlinkOrbListener::l_actuator_outputs,		&(_mavlink->get_subs()->act_3_sub),			3);
+	add_listener(MavlinkOrbListener::l_actuator_armed,		&(_mavlink->get_subs()->armed_sub),			0);
+	add_listener(MavlinkOrbListener::l_manual_control_setpoint,	&(_mavlink->get_subs()->man_control_sp_sub), 		0);
+	add_listener(MavlinkOrbListener::l_vehicle_attitude_controls,	&(_mavlink->get_subs()->actuators_sub),			0);
+	add_listener(MavlinkOrbListener::l_debug_key_value,		&(_mavlink->get_subs()->debug_key_value),		0);
+	add_listener(MavlinkOrbListener::l_optical_flow,		&(_mavlink->get_subs()->optical_flow),			0);
+	add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint,	&(_mavlink->get_subs()->rates_setpoint_sub),		0);
+	add_listener(MavlinkOrbListener::l_airspeed,			&(_mavlink->get_subs()->airspeed_sub),			0);
+	add_listener(MavlinkOrbListener::l_nav_cap,			&(_mavlink->get_subs()->navigation_capabilities_sub),	0);
 
 }
 
@@ -124,15 +125,18 @@ void MavlinkOrbListener::add_listener(void (*callback)(const struct listener *l)
 	nl->subp = subp;
 	nl->arg = arg;
 	nl->next = nullptr;
+	nl->mavlink = _mavlink;
+	nl->listener = this;
 
-	// Register it
-	struct listener *next = _listeners;
-	while (next->next != nullptr) {
-		next = next->next;
+	if (_listeners == nullptr) {
+		_listeners = nl;
+	} else {
+		struct listener *next = _listeners;
+		while (next->next != nullptr) {
+			next = next->next;
+		}
+		next->next = nl;
 	}
-
-	// Attach
-	next->next = nl;
 	_n_listeners++;
 }
 
@@ -271,7 +275,7 @@ void
 MavlinkOrbListener::l_vehicle_status(const struct listener *l)
 {
 	/* immediately communicate state _mavlink->get_chan()ges back to user */
-	orb_copy(ORB_ID(vehicle_status), l->listener->status_sub, &l->listener->v_status);
+	orb_copy(ORB_ID(vehicle_status), l->mavlink->get_subs()->status_sub, &(l->mavlink->v_status));
 	orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed);
 
 	/* enable or disable HIL */
diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h
index c9f35a1fbd..349a11f2e9 100644
--- a/src/modules/mavlink/mavlink_orb_listener.h
+++ b/src/modules/mavlink/mavlink_orb_listener.h
@@ -36,6 +36,7 @@
  * MAVLink 1.0 protocol interface definition.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #include <systemlib/perf_counter.h>
@@ -106,7 +107,7 @@ public:
 		void	(*callback)(const struct listener *l);
 		int		*subp;
 		uintptr_t	arg;
-		struct listener* next;
+		struct listener *next;
 		Mavlink		*mavlink;
 		MavlinkOrbListener* listener;
 	};
@@ -172,9 +173,6 @@ private:
 	struct airspeed_s airspeed;
 	struct home_position_s home;
 
-	int status_sub;
-	int rc_sub;
-
 	unsigned int sensors_raw_counter;
 	unsigned int attitude_counter;
 	unsigned int gps_counter;
diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h
index e5a35ff9bc..40328af146 100755
--- a/src/modules/uORB/topics/vehicle_attitude.h
+++ b/src/modules/uORB/topics/vehicle_attitude.h
@@ -76,7 +76,7 @@ struct vehicle_attitude_s {
 	float rate_offsets[3];	/**< Offsets of the body angular rates from zero		*/
 	float R[3][3];		/**< Rotation matrix body to world, (Tait-Bryan, NED)		*/
 	float q[4];		/**< Quaternion (NED)						*/
-	float g_comp[3];	/**< Compensated gravity vector				*/
+	float g_comp[3];	/**< Compensated gravity vector					*/
 	bool R_valid;		/**< Rotation matrix valid					*/
 	bool q_valid;		/**< Quaternion valid						*/