diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9bc92c91d1..b3b7efe008 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -152,6 +152,17 @@ set(target_name "${OS}-${BOARD}-${LABEL}")
 
 message(STATUS "${target_name}")
 
+# Define GNU standard installation directories
+include(GNUInstallDirs)
+
+# Setup install paths
+if(NOT CMAKE_INSTALL_PREFIX)
+	if (${OS} STREQUAL "posix")
+		set(CMAKE_INSTALL_PREFIX "/usr" CACHE PATH "Install path prefix" FORCE)
+	endif()
+endif()
+message(STATUS "CMAKE_INSTALL_PREFIX: ${CMAKE_INSTALL_PREFIX}")
+
 # switch to ros CMake file if building ros
 if (${OS} STREQUAL "ros")
 	include("cmake/ros-CMakeLists.txt")
@@ -202,7 +213,7 @@ if (NOT ${CMAKE_VERSION} VERSION_LESS 3.1.0)
 endif()
 
 set(version_major 1)
-set(version_minor 0)
+set(version_minor 4)
 set(version_patch 1)
 set(version "${version_major}.${version_minor}.${version_patch}")
 set(package-contact "px4users@googlegroups.com")
@@ -371,10 +382,27 @@ px4_create_git_hash_header(HEADER ${CMAKE_BINARY_DIR}/build_git_version.h)
 #
 # Important to having packaging at end of cmake file.
 #
+set(CPACK_PACKAGE_NAME ${PROJECT_NAME}-${CONFIG})
 set(CPACK_PACKAGE_VERSION ${version})
-set(CPACK_PACKAGE_CONTACT ${package_contact})
+set(CPACK_PACKAGE_CONTACT ${package-contact})
+set(CPACK_DEBIAN_PACKAGE_SHLIBDEPS ON)
+set(CPACK_DEBIAN_PACKAGE_SECTION "devel")
+set(CPACK_DEBIAN_PACKAGE_PRIORITY "optional")
+set(short-description "The px4 autopilot.")
+set(CPACK_DEBIAN_PACKAGE_DESCRIPTION ${short-description})
 set(CPACK_GENERATOR "ZIP")
-set(CPACK_SOURCE_GENERATOR "ZIP")
+set(CPACK_PACKAGE_FILE_NAME "${PROJECT_NAME}-${CONFIG}-${version}")
+set(CPACK_SOURCE_PACKAGE_FILE_NAME "${PROJECT_NAME}-${version}")
+set(CPACK_SOURCE_GENERATOR "ZIP;TBZ2")
+if ("${CMAKE_SYSTEM}" MATCHES "Linux")
+  find_program(DPKG_PROGRAM dpkg)
+  if (EXISTS ${DPKG_PROGRAM})
+    list (APPEND CPACK_GENERATOR "DEB")
+  endif()
+endif()
+if (${OS} STREQUAL "posix")
+	set(CPACK_SET_DESTDIR "ON")
+endif()
 include(CPack)
 
 endif() # ros alternative endif
diff --git a/Makefile b/Makefile
index 5435ef7c70..f65ac29c1a 100644
--- a/Makefile
+++ b/Makefile
@@ -115,16 +115,16 @@ endif
 # --------------------------------------------------------------------
 # describe how to build a cmake config
 define cmake-build
-+@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@$(BUILD_DIR_SUFFIX)/Makefile ]; then rm -rf $(PWD)/build_$@$(BUILD_DIR_SUFFIX); fi
-+@if [ ! -e $(PWD)/build_$@$(BUILD_DIR_SUFFIX)/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p $(PWD)/build_$@$(BUILD_DIR_SUFFIX) && cd $(PWD)/build_$@$(BUILD_DIR_SUFFIX) && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1) || (cd .. && rm -rf $(PWD)/build_$@$(BUILD_DIR_SUFFIX)); fi
++@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e ./build_$@$(BUILD_DIR_SUFFIX)/Makefile ]; then rm -rf ./build_$@$(BUILD_DIR_SUFFIX); fi
++@if [ ! -e ./build_$@$(BUILD_DIR_SUFFIX)/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p ./build_$@$(BUILD_DIR_SUFFIX) && cd ./build_$@$(BUILD_DIR_SUFFIX) && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1) || (cd .. && rm -rf ./build_$@$(BUILD_DIR_SUFFIX)); fi
 +@Tools/check_submodules.sh
-+@(echo "PX4 CONFIG: $@$(BUILD_DIR_SUFFIX)" && cd $(PWD)/build_$@$(BUILD_DIR_SUFFIX) && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
++@(echo "PX4 CONFIG: $@$(BUILD_DIR_SUFFIX)" && cd ./build_$@$(BUILD_DIR_SUFFIX) && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
 endef
 
 define cmake-build-other
-+@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@/Makefile ]; then rm -rf $(PWD)/build_$@; fi
-+@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake $(2) -G$(PX4_CMAKE_GENERATOR) || (cd .. && rm -rf $(PWD)/build_$@); fi
-+@(cd $(PWD)/build_$@ && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
++@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e ./build_$@/Makefile ]; then rm -rf ./build_$@; fi
++@if [ ! -e ./build_$@/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p ./build_$@ && cd ./build_$@ && cmake $(2) -G$(PX4_CMAKE_GENERATOR) || (cd .. && rm -rf ./build_$@); fi
++@(cd ./build_$@ && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
 endef
 
 # create empty targets to avoid msgs for targets passed to cmake
@@ -241,7 +241,7 @@ run_sitl_ros: sitl_deprecation
 
 gazebo_build:
 	@mkdir -p build_gazebo
-	@if [ ! -e $(PWD)/build_gazebo/CMakeCache.txt ];then cd build_gazebo && cmake -Wno-dev -G$(PX4_CMAKE_GENERATOR) $(PWD)/Tools/sitl_gazebo; fi
+	@if [ ! -e ./build_gazebo/CMakeCache.txt ];then cd build_gazebo && cmake -Wno-dev -G$(PX4_CMAKE_GENERATOR) ../Tools/sitl_gazebo; fi
 	@cd build_gazebo && $(PX4_MAKE) $(PX4_MAKE_ARGS)
 	@cd build_gazebo && $(PX4_MAKE) $(PX4_MAKE_ARGS) sdf
 
@@ -339,7 +339,7 @@ distclean: submodulesclean
 	@git clean -ff -x -d -e ".project" -e ".cproject"
 
 # targets handled by cmake
-cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \
+cmake_targets = install test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \
 	run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim replay \
 	jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_tailsitter gazebo_iris gazebo_iris_opt_flow gazebo_tailsitter \
 	gazebo_gdb_standard_vtol gazebo_lldb_standard_vtol gazebo_standard_vtol gazebo_plane gazebo_solo gazebo_typhoon_h480
diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 82b3548750..0e3e246f9e 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 82b354875040d97a3f598f0dc260ddc9ca4a95b3
+Subproject commit 0e3e246f9e00ab2281721b72b0ab934a066148f3
diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris b/posix-configs/SITL/init/rcS_lpe_gazebo_iris
index a72c2c432a..9c84018e98 100644
--- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris
@@ -10,34 +10,40 @@ param set CAL_ACC0_ID 1376264
 param set CAL_ACC1_ID 1310728
 param set CAL_MAG0_ID 196616
 param set CAL_GYRO0_XOFF 0.01
-param set CAL_ACC0_XOFF 1
-param set CAL_ACC0_YOFF 2
-param set CAL_ACC0_ZOFF 3
+param set CAL_ACC0_XOFF 0.01
+param set CAL_ACC0_YOFF -0.01
+param set CAL_ACC0_ZOFF 0.01
 param set CAL_ACC0_XSCALE 1.01
 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 MC_ROLLRATE_P 0.3
-param set MC_PITCHRATE_P 0.3
-param set MC_PITCH_P 5.5
-param set MC_ROLL_P 5.5
-param set MC_ROLLRATE_I 0.1
-param set MC_PITCHRATE_I 0.1
-param set MPC_XY_P 0.15
-param set MPC_XY_VEL_P 0.05
-param set MPC_XY_VEL_D 0.005
-param set MPC_XY_FF 0.1
 param set SENS_BOARD_ROT 8
 param set SENS_BOARD_X_OFF 0.000001
-
-# changes for LPE
 param set COM_RC_IN_MODE 1
 param set NAV_DLL_ACT 2
-param set LPE_BETA_MAX 10000
+param set COM_DISARM_LAND 3
+param set NAV_ACC_RAD 2.0
+param set COM_OF_LOSS_T 5
+param set COM_OBL_ACT 2
+param set COM_OBL_RC_ACT 0
+param set RTL_RETURN_ALT 30.0
+param set RTL_DESCEND_ALT 5.0
+param set RTL_LAND_DELAY 5
+param set MIS_TAKEOFF_ALT 2.5
+param set MC_ROLLRATE_P 0.2
+param set MC_PITCHRATE_P 0.2
+param set MC_PITCH_P 6
+param set MC_ROLL_P 6
+param set MPC_HOLD_MAX_Z 2.0
+param set MPC_HOLD_XY_DZ 0.1
+param set MPC_Z_VEL_P 0.6
+param set MPC_Z_VEL_I 0.15
+param set EKF2_GBIAS_INIT 0.01
+param set EKF2_ANGERR_INIT 0.01
 replay tryapplyparams
 simulator start -s
-rgbled start
+rgbledsim start
 tone_alarm start
 gyrosim start
 accelsim start
@@ -51,24 +57,22 @@ commander start
 land_detector start multicopter
 navigator start
 attitude_estimator_q start
+local_position_estimator start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
-mavlink start -u 14556 -r 2000000
-mavlink start -u 14557 -r 2000000 -m onboard -o 14540
-mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
-mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
-mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
-mavlink stream -r 80 -s ATTITUDE -u 14556
-mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556
-mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
+mixer load /dev/pwm_output0 /usr/share/px4/ROMFS/px4fmu_common/mixers/quad_w.main.mix
+mavlink start -u 14556 -r 4000000
+mavlink start -u 14557 -r 4000000 -m onboard -o 14540
+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 GLOBAL_POSITION_INT -u 14556
+mavlink stream -r 50 -s ATTITUDE -u 14556
+mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14556
+mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556
+mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556
 mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
 sdlog2 start -r 100 -e -t -a
-
-# start LPE at end, when we know it is ok to init sensors
-sleep 5
-local_position_estimator start
 mavlink boot_complete
 replay trystart
diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
index 536f7790b8..c1a6a2eae5 100644
--- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
+++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
@@ -18,27 +18,38 @@ 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 SENS_BOARD_ROT 8
+param set SENS_BOARD_X_OFF 0.000001
+param set COM_RC_IN_MODE 1
+param set NAV_DLL_ACT 2
+param set COM_DISARM_LAND 3
+param set NAV_ACC_RAD 12.0
+param set RTL_RETURN_ALT 30.0
+param set RTL_DESCEND_ALT 10.0
+param set RTL_LAND_DELAY 0
+param set MIS_TAKEOFF_ALT 2.5
 param set MC_ROLLRATE_P 0.3
 param set MC_PITCHRATE_P 0.3
 param set MC_PITCH_P 5.5
 param set MC_ROLL_P 5.5
 param set MC_ROLLRATE_I 0.1
 param set MC_PITCHRATE_I 0.1
-param set MPC_XY_P 0.15
-param set MPC_XY_VEL_P 0.05
-param set MPC_XY_VEL_D 0.005
-param set MPC_XY_FF 0.1
-param set SENS_BOARD_ROT 8
-param set SENS_BOARD_X_OFF 0.000001
-param set COM_RC_IN_MODE 1
-param set NAV_ACC_RAD 2.0
-param set RTL_RETURN_ALT 30.0
-param set RTL_DESCEND_ALT 10.0
+param set MPC_HOLD_MAX_Z 2.0
+param set MPC_HOLD_XY_DZ 0.1
+param set MPC_Z_VEL_P 0.8
+param set MPC_Z_VEL_I 0.15
+param set MPC_XY_VEL_P 0.15
+param set MPC_XY_VEL_I 0.2
+
+# changes for LPE indoor flight
+param set LPE_GPS_ON 0
+param set MPC_ALT_MODE 1
+param set LPE_T_MAX_GRADE 0
+param set MPC_XY_VEL_MAX 2
+param set MPC_Z_VEL_MAX 2
+param set MPC_XY_P 0.5
+param set MIS_TAKEOFF_ALT 2
 
-# changes for LPE
-param set COM_RC_IN_MODE 1
-param set NAV_DLL_ACT 2
-param set LPE_BETA_MAX 10000
 replay tryapplyparams
 simulator start -s
 rgbledsim start
@@ -55,13 +66,14 @@ commander start
 land_detector start multicopter
 navigator start
 attitude_estimator_q start
+local_position_estimator start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
+mixer load /dev/pwm_output0 /usr/share/px4/ROMFS/px4fmu_common/mixers/quad_w.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
-mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
+mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
 mavlink stream -r 80 -s ATTITUDE -u 14556
 mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556
@@ -70,9 +82,5 @@ mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
 sdlog2 start -r 100 -e -t -a
-
-# start LPE at end, when we know it is ok to init sensors
-sleep 5
-local_position_estimator start
 mavlink boot_complete
 replay trystart
diff --git a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
index 6192b2e434..106d032275 100644
--- a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
@@ -18,21 +18,30 @@ 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 MC_ROLLRATE_P 0.2
-param set MC_PITCHRATE_P 0.2
-param set MC_PITCH_P 6
-param set MC_ROLL_P 6
 param set MPC_XY_P 0.4
 param set MPC_XY_VEL_P 0.2
 param set MPC_XY_VEL_D 0.005
 param set SENS_BOARD_ROT 0
 param set SENS_BOARD_X_OFF 0.000001
 param set COM_RC_IN_MODE 1
+param set NAV_DLL_ACT 2
+param set COM_DISARM_LAND 3
 param set NAV_ACC_RAD 2.0
 param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
-param set COM_RC_IN_MODE 1
-param set NAV_DLL_ACT 2
+param set RTL_LAND_DELAY 0
+param set MIS_TAKEOFF_ALT 2.5
+param set MC_ROLLRATE_P 0.2
+param set MC_PITCHRATE_P 0.2
+param set MC_PITCH_P 6
+param set MC_ROLL_P 6
+param set MPC_HOLD_MAX_Z 2.0
+param set MPC_HOLD_XY_DZ 0.1
+param set MPC_Z_VEL_MAX 2.0
+param set MPC_Z_VEL_P 0.4
+param set EKF2_GBIAS_INIT 0.01
+param set EKF2_ANGERR_INIT 0.01
+replay tryapplyparams
 simulator start -s
 rgbledsim start
 tone_alarm start
@@ -41,18 +50,22 @@ accelsim start
 barosim start
 adcsim start
 gpssim start
+#gps start -d /dev/ttyACM0 -s
 pwm_out_sim mode_pwm
 sleep 1
 sensors start
 commander start
 land_detector start multicopter
 navigator start
+attitude_estimator_q start
+local_position_estimator start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
+mixer load /dev/pwm_output0 /usr/share/px4/ROMFS/px4fmu_common/mixers/quad_x.main.mix
 mavlink start -u 14556 -r 2000000
+mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
-mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
+mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
 mavlink stream -r 80 -s ATTITUDE -u 14556
 mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556
@@ -60,7 +73,8 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
 mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
+mavlink stream -r 20 -s MANUAL_CONTROL -u 14556
 sdlog2 start -r 100 -e -t -a
-attitude_estimator_q start
-local_position_estimator start
+logger start -e -t
 mavlink boot_complete
+replay trystart
diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt
index ff348c9b45..6a43db5c11 100644
--- a/src/firmware/posix/CMakeLists.txt
+++ b/src/firmware/posix/CMakeLists.txt
@@ -154,4 +154,12 @@ foreach(viewer none jmavsim gazebo replay)
 	endforeach()
 endforeach()
 
+#=============================================================================
+# install
+#
+
+install(TARGETS mainapp DESTINATION bin)
+install(DIRECTORY ${PROJECT_SOURCE_DIR}/ROMFS DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME})
+install(DIRECTORY ${PROJECT_SOURCE_DIR}/posix-configs DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME})
+
 # vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 584efc77e1..c226560482 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -25,8 +25,8 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// subscriptions, set rate, add to list
 	_sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()),
 	_sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()),
-	// flow 10 hz
-	_sub_flow(ORB_ID(optical_flow), 1000 / 10, 0, &getSubscriptions()),
+	// set flow max update rate higher than expected to we don't lose packets
+	_sub_flow(ORB_ID(optical_flow), 1000 / 100, 0, &getSubscriptions()),
 	// main prediction loop, 100 hz
 	_sub_sensor(ORB_ID(sensor_combined), 1000 / 100, 0, &getSubscriptions()),
 	// status updates 2 hz
diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp
index 2e45333f21..13b646d9e0 100644
--- a/src/modules/local_position_estimator/sensors/flow.cpp
+++ b/src/modules/local_position_estimator/sensors/flow.cpp
@@ -82,6 +82,9 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 	float gyro_y_rad = _flow_gyro_y_high_pass.update(
 				   _sub_flow.get().gyro_y_rate_integral);
 
+	//warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f",
+	//double(flow_x_rad), double(flow_y_rad), double(gyro_x_rad), double(gyro_y_rad), double(d));
+
 	// compute velocities in camera frame using ground distance
 	// assume camera frame is body frame
 	Vector3f delta_b(
diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index 104101060f..fd6fc267c6 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -55,6 +55,7 @@
 #include <systemlib/battery.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/optical_flow.h>
+#include <uORB/topics/distance_sensor.h>
 #include <v1.0/mavlink_types.h>
 #include <v1.0/common/mavlink.h>
 namespace simulator
@@ -238,6 +239,7 @@ private:
 		_gyro_pub(nullptr),
 		_mag_pub(nullptr),
 		_flow_pub(nullptr),
+		_dist_pub(nullptr),
 		_battery_pub(nullptr),
 		_initialized(false)
 #ifndef __PX4_QURT
@@ -288,6 +290,7 @@ private:
 	orb_advert_t _gyro_pub;
 	orb_advert_t _mag_pub;
 	orb_advert_t _flow_pub;
+	orb_advert_t _dist_pub;
 	orb_advert_t _battery_pub;
 
 	bool _initialized;
@@ -298,6 +301,7 @@ private:
 	// class methods
 	int publish_sensor_topics(mavlink_hil_sensor_t *imu);
 	int publish_flow_topic(mavlink_hil_optical_flow_t *flow);
+	int publish_distance_topic(mavlink_distance_sensor_t *dist);
 
 #ifndef __PX4_QURT
 	// uORB publisher handlers
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 6b01a22909..feccfeca45 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -283,6 +283,12 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 		publish_flow_topic(&flow);
 		break;
 
+	case MAVLINK_MSG_ID_DISTANCE_SENSOR:
+		mavlink_distance_sensor_t dist;
+		mavlink_msg_distance_sensor_decode(msg, &dist);
+		publish_distance_topic(&dist);
+		break;
+
 	case MAVLINK_MSG_ID_HIL_GPS:
 		mavlink_hil_gps_t gps_sim;
 		mavlink_msg_hil_gps_decode(msg, &gps_sim);
@@ -863,7 +869,7 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink)
 	flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro;
 	flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro;
 	flow.pixel_flow_x_integral = flow_mavlink->integrated_x;
-	flow.pixel_flow_x_integral = flow_mavlink->integrated_y;
+	flow.pixel_flow_y_integral = flow_mavlink->integrated_y;
 	flow.quality = flow_mavlink->quality;
 
 	int flow_multi;
@@ -871,3 +877,25 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink)
 
 	return OK;
 }
+
+int Simulator::publish_distance_topic(mavlink_distance_sensor_t *dist_mavlink)
+{
+	uint64_t timestamp = hrt_absolute_time();
+
+	struct distance_sensor_s dist;
+	memset(&dist, 0, sizeof(dist));
+
+	dist.timestamp = timestamp;
+	dist.min_distance = dist_mavlink->min_distance / 100.0f;
+	dist.max_distance = dist_mavlink->max_distance / 100.0f;
+	dist.current_distance = dist_mavlink->current_distance / 100.0f;
+	dist.type = dist_mavlink->type;
+	dist.id = dist_mavlink->id;
+	dist.orientation = dist_mavlink->orientation;
+	dist.covariance = dist_mavlink->covariance / 100.0f;
+
+	int dist_multi;
+	orb_publish_auto(ORB_ID(distance_sensor), &_dist_pub, &dist, &dist_multi, ORB_PRIO_HIGH);
+
+	return OK;
+}