lpe : changes to allow hybrid GPS-denied navigation

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -94,6 +94,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_t_max_grade(this, "T_MAX_GRADE"),
 
 	// init origin
+	_fake_origin(this, "FAKE_ORIGIN"),
 	_init_origin_lat(this, "LAT"),
 	_init_origin_lon(this, "LON"),
 
@@ -374,11 +375,15 @@ void BlockLocalPositionEstimator::update()
 	// check timeouts
 	checkTimeouts();
 
-	// if we have no lat, lon initialize projection at 0,0
-	if ((_estimatorInitialized & EST_XY) && !_map_ref.init_done) {
+	// if we have no lat, lon initialize projection to LPE_LAT, LPE_LON parameters
+	if (!_map_ref.init_done && (_estimatorInitialized & EST_XY) && _fake_origin.get()) {
 		map_projection_init(&_map_ref,
 				    _init_origin_lat.get(),
 				    _init_origin_lon.get());
+
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m",
+					     double(_init_origin_lat.get()), double(_init_origin_lon.get()), double(_altOrigin));
+
 	}
 
 	// reinitialize x if necessary
@@ -520,7 +525,7 @@ void BlockLocalPositionEstimator::update()
 		publishEstimatorStatus();
 		_pub_innov.update();
 
-		if ((_estimatorInitialized & EST_XY)) {
+		if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _fake_origin.get())) {
 			publishGlobalPos();
 		}
 	}

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -316,6 +316,7 @@ private:
 	BlockParamFloat  _t_max_grade;
 
 	// init origin
+	BlockParamInt  	 _fake_origin;
 	BlockParamFloat  _init_origin_lat;
 	BlockParamFloat  _init_origin_lon;
 

src/modules/local_position_estimator/params.c

@@ -337,6 +337,16 @@ PARAM_DEFINE_FLOAT(LPE_T_MAX_GRADE, 1.0f);
  */
 PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.001f);
 
+/**
+ * Enable publishing of a fake global position (e.g for AUTO missions using Optical Flow)
+ * by initializing the estimator to the LPE_LAT/LON parameters when global information is unavailable
+ *
+ * @group Local Position Estimator
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(LPE_FAKE_ORIGIN, 1);
+
 /**
  * Local origin latitude for nav w/o GPS
  *
@@ -441,4 +451,4 @@ PARAM_DEFINE_FLOAT(LPE_LAND_VXY, 0.05f);
  * @bit 6 flow gyro compensation
  * @bit 7 fuse baro
  */
-PARAM_DEFINE_INT32(LPE_FUSION, 247);
+PARAM_DEFINE_INT32(LPE_FUSION, 247);

src/modules/local_position_estimator/sensors/gps.cpp

@@ -57,7 +57,7 @@ void BlockLocalPositionEstimator::gpsInit()
 			// find lat, lon of current origin by subtracting x and y
 			// if not using vision position since vision will
 			// have it's own origin, not necessarily where vehicle starts
-			if (!(_fusion.get() & FUSE_VIS_POS)) {
+			if (!_map_ref.init_done && !(_fusion.get() & FUSE_VIS_POS)) {
 				double gpsLatOrigin = 0;
 				double gpsLonOrigin = 0;
 				// reproject at current coordinates
@@ -71,6 +71,9 @@ void BlockLocalPositionEstimator::gpsInit()
 				// possible baro offset in global altitude at init
 				_altOrigin = _gpsAltOrigin;
 				_altOriginInitialized = true;
+
+				mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (gps) : lat %6.2f lon %6.2f alt %5.1f m",
+							     gpsLatOrigin, gpsLonOrigin, double(_gpsAltOrigin));
 			}
 
 			PX4_INFO("[lpe] gps init "

src/modules/local_position_estimator/sensors/vision.cpp

@@ -4,15 +4,15 @@
 
 extern orb_advert_t mavlink_log_pub;
 
-// required number of samples for sensor
-// to initialize
-
-// 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
+// required number of samples for sensor to initialize.
+// This is a vision based position measurement so we assume
+// as soon as we get one measurement it is initialized.
 static const uint32_t 		REQ_VISION_INIT_COUNT = 1;
-static const uint32_t 		VISION_TIMEOUT =    10000000;	// 10 s
+
+// We 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 0.5 seconds
+static const uint32_t 		VISION_TIMEOUT =    500000;	// 0.5 s
 
 void BlockLocalPositionEstimator::visionInit()
 {
@@ -37,9 +37,16 @@ void BlockLocalPositionEstimator::visionInit()
 		_sensorTimeout &= ~SENSOR_VISION;
 		_sensorFault &= ~SENSOR_VISION;
 
+		if (!_map_ref.init_done && _sub_vision_pos.get().xy_global) {
+			// initialize global origin using the visual estimator reference
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (vision) : lat %6.2f lon %6.2f alt %5.1f m",
+						     double(_sub_vision_pos.get().ref_lat), double(_sub_vision_pos.get().ref_lon), double(_sub_vision_pos.get().ref_alt));
+			map_projection_init(&_map_ref, _sub_vision_pos.get().ref_lat, _sub_vision_pos.get().ref_lon);
+		}
+
 		if (!_altOriginInitialized) {
 			_altOriginInitialized = true;
-			_altOrigin = 0;
+			_altOrigin = _sub_vision_pos.get().z_global ? _sub_vision_pos.get().ref_alt : 0.0f;
 		}
 	}
 }