diff --git a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo
index 994613ed51..b983f5410d 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo
+++ b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo
@@ -74,6 +74,10 @@ then
 	#param set SYS_FMU_TASK 1
 	param set SENS_BOARD_ROT 10
 
+	# EKF2
+	param set EKF2_GND_EFF_DZ 6.0
+	param set EKF2_HGT_MODE 1
+
 	# Position control
 	param set MPC_Z_P 1.00000
 	param set MPC_Z_VEL_P 0.20000
@@ -81,7 +85,6 @@ then
 	param set MPC_Z_VEL_D 0.00000
 
 	param set MPC_THR_MIN 0.06000
-	param set MPC_THR_MAX 0.40000
 	param set MPC_THR_HOVER 0.3000
 
 	param set MIS_TAKEOFF_ALT 1.1000
diff --git a/ROMFS/px4fmu_common/init.d/airframes/4072_draco b/ROMFS/px4fmu_common/init.d/airframes/4072_draco
index 661572074d..a12654d59a 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/4072_draco
+++ b/ROMFS/px4fmu_common/init.d/airframes/4072_draco
@@ -20,100 +20,84 @@
 # @maintainer Hyon Lim <lim@uvify.com>
 #
 
-set VEHICLE_TYPE mc
+sh /etc/init.d/rc.mc_defaults
+
 set MIXER quad_x
 set PWM_OUT 1234
 
 if [ $AUTOCNF = yes ]
 then
+	# use the Q attitude estimator, it works w/o mag or GPS.
+	param set SYS_MC_EST_GROUP 3
+	param set ATT_ACC_COMP 0
+	param set ATT_W_ACC 0.4000
+	param set ATT_W_GYRO_BIAS 0.0000
+	param set SYS_HAS_MAG 0
+
 	# Attitude & rate gains
-	param set MC_ROLL_P 7.00000
-	param set MC_ROLLRATE_P 0.15000
-	param set MC_ROLLRATE_I 0.90000
-	param set MC_ROLLRATE_D 0.00130
+	param set MC_ROLL_P 8.0
+	param set MC_ROLLRATE_P 0.11
+	param set MC_ROLLRATE_I 0.3
+	param set MC_ROLLRATE_D 0.0015
+	param set MC_ROLLRATE_MAX 1600.0
 
-	param set MC_PITCH_P 7.00000
-	param set MC_PITCHRATE_P 0.15000
-	param set MC_PITCHRATE_I 1.10000
-	param set MC_PITCHRATE_D 0.00160
+	param set MC_PITCH_P 8.0
+	param set MC_PITCHRATE_P 0.13
+	param set MC_PITCHRATE_I 0.35
+	param set MC_PITCHRATE_D 0.0015
+	param set MC_PITCHRATE_MAX 1600.0
 
-	param set MC_YAW_P 2.8
-	param set MC_YAWRATE_P 0.2
-	param set MC_YAWRATE_I 0.15
+	param set MC_YAW_P 4.0
+	param set MC_YAWRATE_P 0.21
+	param set MC_YAWRATE_I 0.3
 	param set MC_YAWRATE_D 0.0
 	param set MC_YAW_FF 0.5
+	param set MC_YAWRATE_MAX 600.0
 
 	param set MC_ROLL_TC 0.19
 	param set MC_PITCH_TC 0.16
+	param set MC_AIRMODE 1
 
-	# Manual mode settings: Unleash Draco R's power :)
-	param set MPC_MAN_TILT_MAX 70.00000
-	param set MC_PITCHRATE_MAX 1600.00000
-	param set MC_ROLLRATE_MAX 1600.00000
-	param set MC_YAWRATE_MAX 700.00000
-	param set MPC_MANTHR_MAX 0.90000
-	param set MPC_MANTHR_MIN 0.08000
-	param set MPC_MAN_TILT_MAX 35.0000
-	param set MPC_TILTMAX_AIR 20.0000
+	param set MPC_MAN_TILT_MAX 60.0
+	param set MPC_THR_CURVE 1
+	param set MPC_THR_HOVER 0.25
 
 	# Disable RC filtering
 	param set RC_FLT_CUTOFF 0.00000
 
-	# Filter settings
-	param set MC_DTERM_CUTOFF 90.00000
-	param set IMU_GYRO_CUTOFF 100.00000
-
 	# Thrust curve (avoids the need for TPA)
-	param set THR_MDL_FAC 0.25
+	param set THR_MDL_FAC 0.3
 
 	# System
 	param set PWM_MAX 1950
-	param set PWM_MIN 1100
+	param set PWM_MIN 1180
+
+	# enable one-shot
 	param set PWM_RATE 0
 
-	param set SYS_FMU_TASK 1
 	param set SENS_BOARD_ROT 2
 
-	# Position control
-	param set MPC_Z_P 1.00000
-	param set MPC_Z_VEL_P 0.20000
-	param set MPC_Z_VEL_I 0.02000
-	param set MPC_Z_VEL_D 0.00000
-
-	param set MPC_THR_MIN 0.06000
-	param set MPC_THR_MAX 0.40000
-	param set MPC_THR_HOVER 0.3000
-
-	param set MIS_TAKEOFF_ALT 1.1000
-	param set MPC_XY_P 1.7000
-	param set MPC_XY_VEL_P 0.1300
-	param set MPC_XY_VEL_I 0.0600
-	param set MPC_XY_VEL_D 0.0100
-	param set MPC_TKO_RAMP_T 1.0000
-	param set MPC_TKO_SPEED 1.1000
-	param set MPC_VEL_MANUAL 3.0000
-
 	param set BAT_SOURCE 0
 	param set CBRK_IO_SAFETY 22027
-	param set COM_ARM_EKF_AB 0.00500
-	param set COM_DISARM_LAND 3
+	#param set COM_ARM_EKF_AB 0.00500
+	#param set COM_DISARM_LAND 3
 
 	# Filter settings
 	param set IMU_GYRO_CUTOFF 90.00000
-	param set MC_DTERM_CUTOFF 70.00000
+	param set MC_DTERM_CUTOFF 100.00000
 
 	# Don't try to be intelligent on RC loss: just cut the motors
-	param set NAV_RCL_ACT 6
+	#param set NAV_RCL_ACT 6
 
 	# enable to use high-rate logging for better rate tracking analysis
-	# param set SDLOG_PROFILE 19
+	param set SDLOG_PROFILE 27
 
 	# TELEM1 ttyS1
-	param set MAV_0_CONFIG 101
-	param set MAV_0_MODE 2  # onboard
-	param set MAV_0_RATE 20000
-	param set SER_TEL1_BAUD 921600
+	#param set MAV_0_CONFIG 101
+	#param set MAV_0_MODE 2  # onboard
+	#param set MAV_0_RATE 20000
+	#param set SER_TEL1_BAUD 921600
 
 	# TELEM2 ttyS2
-	param set MAV_1_CONFIG 0
+	#param set MAV_1_CONFIG 0
 fi
diff --git a/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r b/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r
index 95bad2029c..c44c2f286c 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r
+++ b/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r
@@ -24,47 +24,56 @@
 #
 # @maintainer Hyon Lim <lim@uvify.com>
 #
-
-set VEHICLE_TYPE mc
+sh /etc/init.d/rc.mc_defaults
 set MIXER hexa_x
-set PWM_OUT 123456
+set PWM_OUT 12345678
 
 if [ $AUTOCNF = yes ]
 then
+	###############################################	
 	# Attitude & rate gains
+	###############################################
+	# Roll
 	param set MC_ROLL_P 6.50000
 	param set MC_ROLLRATE_P 0.15000
 	param set MC_ROLLRATE_I 0.05000
 	param set MC_ROLLRATE_D 0.00130
-
+	param set MC_ROLLRATE_MAX 800.00000
+	# Pitch
 	param set MC_PITCH_P 6.50000
 	param set MC_PITCHRATE_P 0.15000
 	param set MC_PITCHRATE_I 0.05000
 	param set MC_PITCHRATE_D 0.00160
-
+	param set MC_PITCHRATE_MAX 800.00000
+	# Yaw
 	param set MC_YAW_P 2.80000
 	param set MC_YAWRATE_P 0.20000
 	param set MC_YAWRATE_I 0.10000
 	param set MC_YAWRATE_D 0.00000
 	param set MC_YAW_FF 0.00000
-
-	#param set MC_ROLL_TC 0.19
-	#param set MC_PITCH_TC 0.16
-
-	# Manual mode settings: Unleash Draco R's power :)
-	param set MPC_MAN_TILT_MAX 70.00000
-	param set MPC_MANTHR_MAX 0.90000
-	param set MPC_MANTHR_MIN 0.08000
-	param set MPC_MAN_TILT_MAX 35.0000
-	param set MPC_TILTMAX_AIR 45.0000
-	param set MPC_POS_MODE 2
-	param set MPC_AUTO_MODE 1
-	param set MPC_ACC_HOR 8.0000
-
-	param set MC_PITCHRATE_MAX 800.00000
-	param set MC_ROLLRATE_MAX 800.00000
 	param set MC_YAWRATE_MAX 700.00000
 
+	###############################################	
+	# Multirotor Position Gains
+	###############################################
+	param set MPC_ACC_HOR 8.0000
+	param set MPC_THR_MIN 0.06000
+	param set MPC_THR_HOVER 0.3000
+	# altitude control gains
+	param set MPC_Z_P 1.00000
+	param set MPC_Z_VEL_P 0.20000
+	param set MPC_Z_VEL_I 0.02000
+	param set MPC_Z_VEL_D 0.00000
+	# position control gains
+	param set MPC_XY_P 0.9500
+	param set MPC_XY_VEL_P 0.0900
+	param set MPC_XY_VEL_I 0.0200
+	param set MPC_XY_VEL_D 0.0100
+	# etc gains
+	param set MPC_TKO_RAMP_T 0.4000
+	param set MPC_TKO_SPEED 1.5000
+	param set MPC_VEL_MANUAL 5.0000
+
 	# Disable RC filtering
 	param set RC_FLT_CUTOFF 0.00000
 
@@ -81,97 +90,58 @@ then
 	param set PWM_MAIN_DIS5 980
 	param set PWM_MAIN_DIS6 980
 
-	param set SYS_FMU_TASK 1
-	param set SENS_BOARD_ROT 2
+	# DSHOT
+	param set DSHOT_CONFIG 600
 
 	param set COM_ARM_MAG 0.2000
 
-	# Sensors
-	param set SENS_EN_LL40LS 2
-	param set SENS_FLOW_ROT 2
-
-	# Position control
-	param set MPC_Z_P 1.00000
-	param set MPC_Z_VEL_P 0.20000
-	param set MPC_Z_VEL_I 0.02000
-	param set MPC_Z_VEL_D 0.00000
-
-	param set MPC_THR_MIN 0.06000
-	param set MPC_THR_MAX 0.40000
-	param set MPC_THR_HOVER 0.3000
-
 	param set MIS_TAKEOFF_ALT 1.1000
 
-	param set MPC_XY_P 0.9500
-	param set MPC_XY_VEL_P 0.0900
-	param set MPC_XY_VEL_I 0.0200
-	param set MPC_XY_VEL_D 0.0100
-
-	param set MPC_TKO_RAMP_T 0.4000
-	param set MPC_TKO_SPEED 1.5000
-	param set MPC_VEL_MANUAL 10.0000
-
+	#####################################
 	# EKF
-	# Set baro first
+	#####################################
+	# Height mode as GPS
 	param set EKF2_HGT_MODE 1
 	# Enable optical flow and GPS
 	param set EKF2_AID_MASK 1
-	param set EKF2_RNG_AID 0
+	param set EKF2_RNG_AID 1
 	param set EKF2_MAG_TYPE 1
 	param set EKF2_OF_QMIN 80.0000
 
+	# default horizontal stablization sensors
+	param set SENS_EN_PMW3901 1
+	param set SENS_EN_PX4FLOW 1
+	param set SENS_EN_LL40LS 2
+	param set SENS_FLOW_ROT 2
+
 	#
 	param set CBRK_IO_SAFETY 22027
-	param set SYS_COMPANION 921600
 	param set COM_DISARM_LAND 3
 
-	#PWM
-	# ONESHOT
+	# PWM ONESHOT
 	param set PWM_RATE 0
 
-	# gimbal
-	#param set MNT_DO_STAB 1
-	#param set MNT_MAN_PITCH 1
-	#param set MNT_MAN_ROLL 2
-	#param set MNT_MODE_IN 1
+	# Battery
 	param set BAT_SOURCE 0
 	param set BAT_N_CELLS 4
 	param set BAT_V_DIV 10.133
 
 	# TELEM1 ttyS1
 	param set MAV_0_CONFIG 101
-	param set MAV_0_MODE 1	# onboard
+	param set MAV_0_MODE 0	# onboard
 	param set MAV_0_FORWARD 1
 	param set SER_TEL1_BAUD 57600
 
 	# TELEM2 ttyS2
 	param set MAV_1_CONFIG 102
 	param set MAV_1_MODE 2
-	param set MAV_1_RATE 800000
 	param set MAV_1_FORWARD 1
+	param set MAV_1_RATE 800000
 	param set SER_TEL2_BAUD 921600
+	
+	# Disable internal magnetometer sensor
+	param set CAL_MAG0_EN 0
+	# Enable external magnetometer sensor
+	param set CAL_MAG1_EN 1
 fi
 
-#set PWM_OUT 12345678
-
-#set MIXER_AUX mount_2axes
-#set PWM_AUX_OUT 78
-#set PWM_AUX_RATE 50
-#set OUTPUT_AUX_DEV /dev/pwm_output0
-#set OUTPUT_AUX_TO_MAIN yes
-#set MIXER_APPEND yes
-
-#if mixer append /dev/pwm_output0 /etc/mixers/mount_2axes.aux.mix
-#then
-#	echo "INFO [6002] Mixer append success"
-#else
-#	echo "ERROR [6002] Mixer append failed"
-#fi
-
-#if pwm rate -c 78 -r 50 -d /dev/pwm_output0
-#then
-#	echo "INFO [6002] PWM RATE CHANGE SUCCESS"
-#else
-#	echo "INFO [6002] PWM RATE CHANGE FAILED"
-#fi
-
diff --git a/boards/uvify/core/default.cmake b/boards/uvify/core/default.cmake
index b17507af90..e87a91d368 100644
--- a/boards/uvify/core/default.cmake
+++ b/boards/uvify/core/default.cmake
@@ -24,6 +24,7 @@ px4_add_board(
 		camera_trigger
 		#differential_pressure # all available differential pressure drivers
 		distance_sensor # all available distance sensor drivers
+		dshot
 		gps
 		#heater
 		#imu # all available imu drivers
@@ -38,15 +39,14 @@ px4_add_board(
 		magnetometer/lis3mdl
 		magnetometer/ist8310
 		#mkblctrl
-		#optical_flow # all available optical flow drivers
-		optical_flow/px4flow
+		optical_flow # all available optical flow drivers
 		pca9685
 		pwm_input
 		pwm_out_sim
 		px4fmu
 		rc_input
 		#tap_esc
-		telemetry # all available telemetry drivers
+		#telemetry # all available telemetry drivers
 		#test_ppm
 		tone_alarm
 		uavcan
diff --git a/boards/uvify/core/init/rc.board_extras b/boards/uvify/core/init/rc.board_extras
index 4625aa9e3a..39248b9002 100644
--- a/boards/uvify/core/init/rc.board_extras
+++ b/boards/uvify/core/init/rc.board_extras
@@ -8,7 +8,7 @@
 if param compare SYS_AUTOSTART 4071
 then
 	# Change rate to 400 Khz for fast barometer
-	fmu i2c 1 400000
+	#fmu i2c 1 400000
 
 	# IFO has only external i2c barometer.
 	# It does not start EKF2 in the beginning which is strange behaviour. but 3 seconds hack.
diff --git a/boards/uvify/core/init/rc.board_sensors b/boards/uvify/core/init/rc.board_sensors
index 7c4be4ad20..3422582183 100644
--- a/boards/uvify/core/init/rc.board_sensors
+++ b/boards/uvify/core/init/rc.board_sensors
@@ -4,20 +4,18 @@
 #------------------------------------------------------------------------------
 
 # Internal SPI
-ms5611 -s start
+ms5611 -T 0 -s start
 
 # Draco-R
 if param compare SYS_AUTOSTART 6002
 then
-	# Barometer
-	# Internal SPI
-	ms5611 -T 0 -s start
+	# GPS LED
+	rgbled_ncp5623c start -a 0x38
 
-	# PX4flow
-	px4flow start
-	ist8310 start
-	mpu6000 -R 2 -T 20608 start
-	mpu9250 -R 2 start
+	#mpu6000 -R 4 -T 20608 start
+	mpu9250 -R 4 start
+
+	# Default GNSS with LIS3MDL magnetometer with external i2c.
 	lis3mdl -R 2 -X start
 fi
 
diff --git a/boards/uvify/core/src/board_config.h b/boards/uvify/core/src/board_config.h
index 03f1ffb567..e1c8ae1d14 100644
--- a/boards/uvify/core/src/board_config.h
+++ b/boards/uvify/core/src/board_config.h
@@ -52,7 +52,7 @@
  ****************************************************************************************************/
 /* Configuration ************************************************************************************/
 
-/* PX4FMU GPIOs ***********************************************************************************/
+/* UVify Core GPIOs ***********************************************************************************/
 /* LEDs */
 #define GPIO_LED1                    (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11)
 #define GPIO_LED2                    (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
@@ -344,6 +344,8 @@
 
 #define BOARD_HAS_ON_RESET 1
 
+#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5};
+
 #define BOARD_ENABLE_CONSOLE_BUFFER
 #define BOARD_CONSOLE_BUFFER_SIZE (1024*3)
 
diff --git a/boards/uvify/core/src/timer_config.c b/boards/uvify/core/src/timer_config.c
index f567504324..49f5ac6216 100644
--- a/boards/uvify/core/src/timer_config.c
+++ b/boards/uvify/core/src/timer_config.c
@@ -60,6 +60,12 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
 		.last_channel_index = 3,
 		.handler = io_timer_handler0,
 		.vectorno =  STM32_IRQ_TIM1CC,
+		.dshot = {
+			.dma_base = STM32_DMA2_BASE,
+			.dmamap = DMAMAP_TIM1_UP,
+			.start_ccr_register = TIM_DMABASE_CCR1,
+			.channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */
+		}
 
 	},
 	{
@@ -71,6 +77,12 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
 		.last_channel_index = 5,
 		.handler = io_timer_handler1,
 		.vectorno =  STM32_IRQ_TIM4,
+		.dshot = {
+			.dma_base = STM32_DMA1_BASE,
+			.dmamap = DMAMAP_TIM4_UP,
+			.start_ccr_register = TIM_DMABASE_CCR2,
+			.channels_number = 2u // CCR2 and CCR3
+		}
 	}
 };
 
diff --git a/src/drivers/barometer/ms5611/ms5611.cpp b/src/drivers/barometer/ms5611/ms5611.cpp
index 30f7646571..720b38e301 100644
--- a/src/drivers/barometer/ms5611/ms5611.cpp
+++ b/src/drivers/barometer/ms5611/ms5611.cpp
@@ -1024,7 +1024,7 @@ info()
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset'");
+	warnx("missing command: try 'start', 'info', 'test', 'reset'");
 	warnx("options:");
 	warnx("    -X    (external I2C bus)");
 	warnx("    -I    (intternal I2C bus)");