diff --git a/makefiles/posix/config_posix_sitl.mk b/makefiles/posix/config_posix_sitl.mk
index c3172641436..806e59e7cb0 100644
--- a/makefiles/posix/config_posix_sitl.mk
+++ b/makefiles/posix/config_posix_sitl.mk
@@ -40,6 +40,7 @@ MODULES		+= modules/position_estimator_inav
 MODULES 	+= modules/navigator
 MODULES 	+= modules/mc_pos_control
 MODULES		+= modules/mc_att_control
+MODULES 	+= modules/land_detector
 
 #
 # Library modules
diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS
index e8f257195aa..d9f94c5ce00 100644
--- a/posix-configs/SITL/init/rcS
+++ b/posix-configs/SITL/init/rcS
@@ -22,6 +22,9 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
+param set MPC_XY_P 0.4
+param set MPC_XY_VEL_P 0.2
+param set MPC_XY_VEL_D 0.005
 rgbled start
 tone_alarm start
 gyrosim start
@@ -32,8 +35,14 @@ gpssim start
 hil mode_pwm
 commander start
 sensors start
+land_detector start multicopter
+navigator start
 attitude_estimator_q start
 position_estimator_inav start
 mc_pos_control start
 mc_att_control start
 mixer load /dev/pwm_output0 ../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
+mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
+mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556
+mavlink stream -r 50 -s ATTITUDE -u 14556
+mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556
diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp
index 22cebd207cb..71f41347906 100644
--- a/src/drivers/ll40ls/ll40ls.cpp
+++ b/src/drivers/ll40ls/ll40ls.cpp
@@ -329,6 +329,8 @@ test(const bool use_i2c, const int bus)
 		}
 
 		warnx("periodic read %u", i);
+		warnx("valid %u", (float)report.current_distance > report.min_distance
+			&& (float)report.current_distance < report.max_distance ? 1 : 0);
 		warnx("measurement: %0.3f m", (double)report.current_distance);
 		warnx("time:        %lld", report.timestamp);
 	}
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index a67db0bd1f4..45ce7200bac 100644
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -832,6 +832,7 @@ test()
 	}
 
 	warnx("single read");
+	warnx("measurement: %0.2f m", (double)report.current_distance);
 	warnx("time:        %llu", report.timestamp);
 
 	/* start the sensor polling at 2Hz */
@@ -860,6 +861,9 @@ test()
 		}
 
 		warnx("periodic read %u", i);
+		warnx("valid %u", (float)report.current_distance > report.min_distance
+			&& (float)report.current_distance < report.max_distance ? 1 : 0);
+		warnx("measurement: %0.3f", (double)report.current_distance);
 		warnx("time:        %llu", report.timestamp);
 	}
 
diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp
index 730fd9b9689..11baafd23bf 100644
--- a/src/drivers/ms5611/ms5611_i2c.cpp
+++ b/src/drivers/ms5611/ms5611_i2c.cpp
@@ -71,8 +71,8 @@ public:
 	virtual ~MS5611_I2C();
 
 	virtual int	init();
-	virtual int	dev_read(unsigned offset, void *data, unsigned count);
-	virtual int	dev_ioctl(unsigned operation, unsigned &arg);
+	virtual int	read(unsigned offset, void *data, unsigned count);
+	virtual int	ioctl(unsigned operation, unsigned &arg);
 
 #ifdef __PX4_NUTTX
 protected:
@@ -139,7 +139,7 @@ MS5611_I2C::init()
 }
 
 int
-MS5611_I2C::dev_read(unsigned offset, void *data, unsigned count)
+MS5611_I2C::read(unsigned offset, void *data, unsigned count)
 {
 	union _cvt {
 		uint8_t	b[4];
@@ -162,7 +162,7 @@ MS5611_I2C::dev_read(unsigned offset, void *data, unsigned count)
 }
 
 int
-MS5611_I2C::dev_ioctl(unsigned operation, unsigned &arg)
+MS5611_I2C::ioctl(unsigned operation, unsigned &arg)
 {
 	int ret;
 
diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp
index 5b1b37f5633..b331aff08b0 100644
--- a/src/drivers/sf0x/sf0x.cpp
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -854,7 +854,7 @@ test()
 	}
 
 	warnx("single read");
-	warnx("val:  %0.2f m", (double)report.current_distance);
+	warnx("measurement:  %0.2f m", (double)report.current_distance);
 	warnx("time: %llu", report.timestamp);
 
 	/* start the sensor polling at 2 Hz rate */
@@ -885,7 +885,9 @@ test()
 		}
 
 		warnx("read #%u", i);
-		warnx("val:  %0.3f m", (double)report.current_distance);
+		warnx("valid %u", (float)report.current_distance > report.min_distance
+			&& (float)report.current_distance < report.max_distance ? 1 : 0);
+		warnx("measurement:  %0.3f m", (double)report.current_distance);
 		warnx("time: %llu", report.timestamp);
 	}
 
diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp
index 70455b7d77a..606f37ba21c 100644
--- a/src/drivers/trone/trone.cpp
+++ b/src/drivers/trone/trone.cpp
@@ -125,6 +125,7 @@ private:
 	work_s				_work;
 	ringbuffer::RingBuffer		*_reports;
 	bool				_sensor_ok;
+	uint8_t				_valid;
 	int					_measure_ticks;
 	bool				_collect_phase;
 	int				_class_instance;
@@ -211,7 +212,7 @@ static const uint8_t crc_table[] = {
 	0xfa, 0xfd, 0xf4, 0xf3
 };
 
-/* static uint8_t crc8(uint8_t *p, uint8_t len) {
+ static uint8_t crc8(uint8_t *p, uint8_t len) {
 	uint16_t i;
 	uint16_t crc = 0x0;
 
@@ -221,7 +222,7 @@ static const uint8_t crc_table[] = {
 	}
 
 	return crc & 0xFF;
-}*/
+}
 
 /*
  * Driver 'main' command.
@@ -234,6 +235,7 @@ TRONE::TRONE(int bus, int address) :
 	_max_distance(TRONE_MAX_DISTANCE),
 	_reports(nullptr),
 	_sensor_ok(false),
+	_valid(0),
 	_measure_ticks(0),
 	_collect_phase(false),
 	_class_instance(-1),
@@ -592,6 +594,10 @@ TRONE::collect()
 	/* TODO: set proper ID */
 	report.id = 0;
 
+	// This validation check can be used later
+	_valid = crc8(val, 2) == val[2] && (float)report.current_distance > report.min_distance
+		&& (float)report.current_distance < report.max_distance ? 1 : 0;
+
 	/* publish it, if we are the primary */
 	if (_distance_sensor_topic != nullptr) {
 		orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp
index 741bc02ad44..fc473ebabda 100644
--- a/src/modules/land_detector/FixedwingLandDetector.cpp
+++ b/src/modules/land_detector/FixedwingLandDetector.cpp
@@ -87,12 +87,12 @@ bool FixedwingLandDetector::update()
 	if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) {
 		float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx *
 					_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
-		if (isfinite(val)) {
+		if (PX4_ISFINITE(val)) {
 			_velocity_xy_filtered = val;
 		}
 		val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz);
 
-		if (isfinite(val)) {
+		if (PX4_ISFINITE(val)) {
 			_velocity_z_filtered = val;
 		}
 	}
diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp
index 1ca319ce63f..34355c6b111 100644
--- a/src/modules/land_detector/land_detector_main.cpp
+++ b/src/modules/land_detector/land_detector_main.cpp
@@ -38,6 +38,10 @@
  * @author Johan Jansen <jnsn.johan@gmail.com>
  */
 
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
 #include <unistd.h>					//usleep
 #include <stdio.h>
 #include <string.h>
@@ -80,7 +84,7 @@ static void land_detector_deamon_thread(int argc, char *argv[])
 static void land_detector_stop()
 {
 	if (land_detector_task == nullptr || _landDetectorTaskID == -1) {
-		errx(1, "not running");
+		warnx("not running");
 		return;
 	}
 
@@ -95,7 +99,7 @@ static void land_detector_stop()
 
 		/* if we have given up, kill it */
 		if (++i > 50) {
-			task_delete(_landDetectorTaskID);
+			px4_task_delete(_landDetectorTaskID);
 			break;
 		}
 	} while (land_detector_task->isRunning());
@@ -104,7 +108,7 @@ static void land_detector_stop()
 	delete land_detector_task;
 	land_detector_task = nullptr;
 	_landDetectorTaskID = -1;
-	errx(0, "land_detector has been stopped");
+	warnx("land_detector has been stopped");
 }
 
 /**
@@ -113,7 +117,7 @@ static void land_detector_stop()
 static int land_detector_start(const char *mode)
 {
 	if (land_detector_task != nullptr || _landDetectorTaskID != -1) {
-		errx(1, "already running");
+		warnx("already running");
 		return -1;
 	}
 
@@ -125,13 +129,13 @@ static int land_detector_start(const char *mode)
 		land_detector_task = new MulticopterLandDetector();
 
 	} else {
-		errx(1, "[mode] must be either 'fixedwing' or 'multicopter'");
+		warnx("[mode] must be either 'fixedwing' or 'multicopter'");
 		return -1;
 	}
 
 	//Check if alloc worked
 	if (land_detector_task == nullptr) {
-		errx(1, "alloc failed");
+		warnx("alloc failed");
 		return -1;
 	}
 
@@ -140,11 +144,11 @@ static int land_detector_start(const char *mode)
 					     SCHED_DEFAULT,
 					     SCHED_PRIORITY_DEFAULT,
 					     1000,
-					     (main_t)&land_detector_deamon_thread,
+					     (px4_main_t)&land_detector_deamon_thread,
 					     nullptr);
 
 	if (_landDetectorTaskID < 0) {
-		errx(1, "task start failed: %d", -errno);
+		warnx("task start failed: %d", -errno);
 		return -1;
 	}
 
@@ -163,9 +167,9 @@ static int land_detector_start(const char *mode)
 			usleep(50000);
 
 			if (hrt_absolute_time() > timeout) {
-				err(1, "start failed - timeout");
+				warnx("start failed - timeout");
 				land_detector_stop();
-				exit(1);
+				return 1;
 			}
 		}
 		printf("\n");
@@ -174,7 +178,6 @@ static int land_detector_start(const char *mode)
 	//Remember current active mode
 	strncpy(_currentMode, mode, 12);
 
-	exit(0);
 	return 0;
 }
 
@@ -189,12 +192,16 @@ int land_detector_main(int argc, char *argv[])
 	}
 
 	if (argc >= 2 && !strcmp(argv[1], "start")) {
-		land_detector_start(argv[2]);
+		if (land_detector_start(argv[2]) != 0) {
+			warnx("land_detector start failed");
+			return 1;
+		}
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "stop")) {
 		land_detector_stop();
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "status")) {
@@ -204,13 +211,14 @@ int land_detector_main(int argc, char *argv[])
 				warnx("running (%s): %s", _currentMode, (land_detector_task->isLanded()) ? "LANDED" : "IN AIR");
 
 			} else {
-				errx(1, "exists, but not running (%s)", _currentMode);
+				warnx("exists, but not running (%s)", _currentMode);
 			}
 
-			exit(0);
+			return 0;
 
 		} else {
-			errx(1, "not running");
+			warnx("not running");
+			return 1;
 		}
 	}
 
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index cc96b535ee6..7b4bfccf33f 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -310,7 +310,7 @@ Navigator::task_main()
 	const hrt_abstime mavlink_open_interval = 500000;
 
 	/* wakeup source(s) */
-	struct pollfd fds[8];
+	px4_pollfd_struct_t fds[8];
 
 	/* Setup of loop */
 	fds[0].fd = _global_pos_sub;
@@ -333,7 +333,7 @@ Navigator::task_main()
 	while (!_task_should_exit) {
 
 		/* wait for up to 100ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		if (pret == 0) {
 			/* timed out - periodic check for _task_should_exit, etc. */
diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk
index c92ba3007e5..45c87629966 100644
--- a/src/modules/position_estimator_inav/module.mk
+++ b/src/modules/position_estimator_inav/module.mk
@@ -42,5 +42,5 @@ SRCS		 	= position_estimator_inav_main.c \
 
 MODULE_STACKSIZE = 1200
 
-EXTRACFLAGS = -Wframe-larger-than=3500 -Wno-unused
+EXTRACFLAGS = -Wframe-larger-than=3500
 
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 6f60e4878d3..327276c5c16 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -1071,25 +1071,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
 		}
 
-		// if (inav_verbose_mode) {
-		// 	/* print updates rate */
-		// 	if (t > updates_counter_start + updates_counter_len) {
-		// 		float updates_dt = (t - updates_counter_start) * 0.000001f;
-		// 		warnx(
-		// 			"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s",
-		// 			(double)(accel_updates / updates_dt),
-		// 			(double)(baro_updates / updates_dt),
-		// 			(double)(gps_updates / updates_dt),
-		// 			(double)(attitude_updates / updates_dt),
-		// 			(double)(flow_updates / updates_dt));
-		// 		updates_counter_start = t;
-		// 		accel_updates = 0;
-		// 		baro_updates = 0;
-		// 		gps_updates = 0;
-		// 		attitude_updates = 0;
-		// 		flow_updates = 0;
-		// 	}
-		// }
+		if (inav_verbose_mode) {
+			/* print updates rate */
+			if (t > updates_counter_start + updates_counter_len) {
+				float updates_dt = (t - updates_counter_start) * 0.000001f;
+				warnx(
+					"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s",
+					(double)(accel_updates / updates_dt),
+					(double)(baro_updates / updates_dt),
+					(double)(gps_updates / updates_dt),
+					(double)(attitude_updates / updates_dt),
+					(double)(flow_updates / updates_dt));
+				updates_counter_start = t;
+				accel_updates = 0;
+				baro_updates = 0;
+				gps_updates = 0;
+				attitude_updates = 0;
+				flow_updates = 0;
+			}
+		}
 
 		if (t > pub_last + PUB_INTERVAL) {
 			pub_last = t;
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index afde16ed395..b0e96b1f031 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -292,9 +292,9 @@ dsm_bind(uint16_t cmd, int pulses)
 
 		/*Pulse RX pin a number of times*/
 		for (int i = 0; i < pulses; i++) {
-			up_udelay(25);
+			up_udelay(120);
 			stm32_gpiowrite(usart1RxAsOutp, false);
-			up_udelay(25);
+			up_udelay(120);
 			stm32_gpiowrite(usart1RxAsOutp, true);
 		}
 		break;
diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp
index 8b535e5992d..9108a6f2281 100644
--- a/src/platforms/posix/drivers/gpssim/gpssim.cpp
+++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp
@@ -272,8 +272,8 @@ GPSSIM::receive(int timeout) {
 	_report_gps_pos.lon = gps.lon;
 	_report_gps_pos.alt = gps.alt;
 	_report_gps_pos.timestamp_variance = hrt_absolute_time();
-	_report_gps_pos.eph = (float)gps.eph;
-	_report_gps_pos.epv = (float)gps.epv;
+	_report_gps_pos.eph = (float)gps.eph * 1e-2f;
+	_report_gps_pos.epv = (float)gps.epv * 1e-2f;
 	_report_gps_pos.vel_m_s = (float)(gps.vel)/100.0f;
 	_report_gps_pos.vel_n_m_s = (float)(gps.vn)/100.0f;
 	_report_gps_pos.vel_e_m_s = (float)(gps.ve)/100.0f;