diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index c4b25987be..411eb0d892 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -564,6 +564,10 @@ then
 		then
 			mavlink start -d /dev/ttyS2 -b 57600 -m osd -r 1000
 		fi
+		if param compare SYS_COMPANION 257600
+		then
+			mavlink start -d /dev/ttyS2 -b 57600 -m magic -r 5000 -x
+		fi
 		# Sensors on the PWM interface bank
 		# clear pins 5 and 6
 		if param compare SENS_EN_LL40LS 1
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 3a2f1d583b..1b04eea1a0 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1523,6 +1523,9 @@ Mavlink::task_main(int argc, char *argv[])
 			} else if (strcmp(myoptarg, "osd") == 0) {
 				_mode = MAVLINK_MODE_OSD;
 
+			} else if (strcmp(myoptarg, "magic") == 0) {
+				_mode = MAVLINK_MODE_MAGIC;
+
 			} else if (strcmp(myoptarg, "config") == 0) {
 				_mode = MAVLINK_MODE_CONFIG;
 			}
@@ -1709,7 +1712,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("SYSTEM_TIME", 1.0f);
 		configure_stream("TIMESYNC", 10.0f);
 		configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f);
-		/* camera trigger is rate limited at the source, do not limit here */
+		//camera trigger is rate limited at the source, do not limit here
 		configure_stream("CAMERA_TRIGGER", 500.0f);
 		configure_stream("EXTENDED_SYS_STATE", 2.0f);
 		break;
@@ -1729,6 +1732,10 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("EXTENDED_SYS_STATE", 1.0f);
 		break;
 
+	case MAVLINK_MODE_MAGIC:
+		//stream nothing
+		break;
+
 	case MAVLINK_MODE_CONFIG:
 		// Enable a number of interesting streams we want via USB
 		configure_stream("SYS_STATUS", 1.0f);
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 95e0fdb91a..37c66b5963 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -149,6 +149,7 @@ public:
 		MAVLINK_MODE_CUSTOM,
 		MAVLINK_MODE_ONBOARD,
 		MAVLINK_MODE_OSD,
+		MAVLINK_MODE_MAGIC,
 		MAVLINK_MODE_CONFIG
 	};
 
diff --git a/src/modules/position_estimator_inav/CMakeLists.txt b/src/modules/position_estimator_inav/CMakeLists.txt
index 2d5315b92b..60429f5d22 100644
--- a/src/modules/position_estimator_inav/CMakeLists.txt
+++ b/src/modules/position_estimator_inav/CMakeLists.txt
@@ -31,7 +31,7 @@
 #
 ############################################################################
 if(${OS} STREQUAL "nuttx")
-	list(APPEND MODULE_CFLAGS -Wframe-larger-than=3890)
+	list(APPEND MODULE_CFLAGS -Wframe-larger-than=4000)
 endif()
 px4_add_module(
 	MODULE modules__position_estimator_inav
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 aa7fe0d972..d8f29f1082 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -586,8 +586,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
 					flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
 							fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
-					//this flag is not working -->
-					flow_accurate = true; //already checked if flow_q > 0.3
 
 
 					/*calculate offset of flow-gyro using already calibrated gyro from autopilot*/
@@ -840,7 +838,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 						} else if (t > ref_init_start + ref_init_delay) {
 							ref_inited = true;
-
+							
 							/* set position estimate to (0, 0, 0), use GPS velocity for XY */
 							x_est[0] = 0.0f;
 							x_est[1] = gps.vel_n_m_s;
@@ -953,9 +951,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		t_prev = t;
 
 		/* increase EPH/EPV on each step */
+		if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0
+			eph = 0.001;
+		}
 		if (eph < max_eph_epv) {
 			eph *= 1.0f + dt;
 		}
+		if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0
+			epv = 0.001;
+		}
 		if (epv < max_eph_epv) {
 			epv += 0.005f * dt;	// add 1m to EPV each 200s (baro drift)
 		}
@@ -968,9 +972,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;
 		/* use MOCAP if it's valid and has a valid weight parameter */
 		bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W;
+		if(params.disable_mocap) { //disable mocap if fake gps is used
+			use_mocap = false;
+		}
 		/* use flow if it's valid and (accurate or no GPS available) */
 		bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
 
+		
 		bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;
 
 		bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout);
@@ -1166,7 +1174,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 				inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
 				inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
-				//mavlink_log_info(mavlink_fd, "w_flow = %2.4f\t w_xy_flow = %2.4f\n", (double)w_flow, (double)params.w_xy_flow);
 			}
 
 			if (use_gps_xy) {
@@ -1264,6 +1271,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				buf_ptr = 0;
 			}
 
+			
 			/* publish local position */
 			local_pos.xy_valid = can_estimate_xy;
 			local_pos.v_xy_valid = can_estimate_xy;
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
index be8a136c4d..f303abaf6d 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -158,10 +158,10 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f);
  * Weight (cutoff frequency) for optical flow (velocity) measurements.
  *
  * @min 0.0
- * @max 30.0
+ * @max 10.0
  * @group Position Estimator INAV
  */
-PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 10.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 9.0f);
 
 /**
  * XY axis weight for resetting velocity
@@ -313,6 +313,17 @@ PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f);
  */
 PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f);
 
+/**
+ * Disable mocap (set 0 if using fake gps)
+ *
+ * Disable mocap
+ *
+ * @min 0
+ * @max 1
+ * @group Position Estimator INAV
+ */
+PARAM_DEFINE_FLOAT(INAV_DISAB_MOCAP, 0);
+
 /**
  * Disable vision input
  *
@@ -364,6 +375,7 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h)
 	h->delay_gps = param_find("INAV_DELAY_GPS");
 	h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X");
 	h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y");
+	h->disable_mocap = param_find("INAV_DISAB_MOCAP");
 
 	return 0;
 }
@@ -394,6 +406,7 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h
 	param_get(h->delay_gps, &(p->delay_gps));
 	param_get(h->flow_module_offset_x, &(p->flow_module_offset_x));
 	param_get(h->flow_module_offset_y, &(p->flow_module_offset_y));
+	param_get(h->disable_mocap, &(p->disable_mocap));
 
 	return 0;
 }
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h
index 43601568f8..07b78104bf 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.h
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -67,6 +67,7 @@ struct position_estimator_inav_params {
 	float delay_gps;
 	float flow_module_offset_x;
 	float flow_module_offset_y;
+	int32_t disable_mocap;
 };
 
 struct position_estimator_inav_param_handles {
@@ -95,6 +96,7 @@ struct position_estimator_inav_param_handles {
 	param_t delay_gps;
 	param_t flow_module_offset_x;
 	param_t flow_module_offset_y;
+	param_t disable_mocap;
 };
 
 #define CBRK_NO_VISION_KEY	328754