LPE vision estimation fixes. (#5505)

src/modules/local_position_estimator/sensors/vision.cpp

@@ -6,8 +6,13 @@ extern orb_advert_t mavlink_log_pub;
 
 // required number of samples for sensor
 // to initialize
-static const uint32_t 		REQ_VISION_INIT_COUNT = 20;
-static const uint32_t 		VISION_TIMEOUT =    500000;	// 0.5 s
+
+// this is a vision based position measurement so we assume as soon as we get one
+// measurement it is initialized, we also don't want to deinitialize it because
+// this will throw away a correction before it starts using the data so we
+// set the timeout to 10 seconds
+static const uint32_t 		REQ_VISION_INIT_COUNT = 1;
+static const uint32_t 		VISION_TIMEOUT =    10000000;	// 10 s
 
 void BlockLocalPositionEstimator::visionInit()
 {
@@ -21,7 +26,6 @@ void BlockLocalPositionEstimator::visionInit()
 
 	// increament sums for mean
 	if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) {
-		_visionOrigin = _visionStats.getMean();
 		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position init: "
 					     "%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m",
 					     double(_visionStats.getMean()(0)),
@@ -35,7 +39,7 @@ void BlockLocalPositionEstimator::visionInit()
 
 		if (!_altOriginInitialized) {
 			_altOriginInitialized = true;
-			_altOrigin = _visionOrigin(2);
+			_altOrigin = 0;
 		}
 	}
 }
@@ -58,9 +62,6 @@ void BlockLocalPositionEstimator::visionCorrect()
 
 	if (visionMeasure(y) != OK) { return; }
 
-	// make measurement relative to origin
-	y -= _visionOrigin;
-
 	// vision measurement matrix, measures position
 	Matrix<float, n_y_vision, n_x> C;
 	C.setZero();