LPE: Don't use home as local origin (#5067)

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -32,7 +32,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// status updates 2 hz
 	_sub_param_update(ORB_ID(parameter_update), 1000 / 2, 0, &getSubscriptions()),
 	_sub_manual(ORB_ID(manual_control_setpoint), 1000 / 2, 0, &getSubscriptions()),
-	_sub_home(ORB_ID(home_position), 1000 / 2, 0, &getSubscriptions()),
 	// gps 10 hz
 	_sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()),
 	// vision 5 hz
@@ -87,9 +86,9 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_pn_b_noise_density(this, "PN_B"),
 	_t_max_grade(this, "T_MAX_GRADE"),
 
-	// init home
+	// init origin
-	_init_home_lat(this, "LAT"),
+	_init_origin_lat(this, "LAT"),
-	_init_home_lon(this, "LON"),
+	_init_origin_lon(this, "LON"),
 
 	// flow gyro
 	_flow_gyro_x_high_pass(this, "FGYRO_HP"),
@@ -140,12 +139,12 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_mocapInitialized(false),
 
 	// reference altitudes
-	_altHome(0),
+	_altOrigin(0),
-	_altHomeInitialized(false),
+	_altOriginInitialized(false),
-	_baroAltHome(0),
+	_baroAltOrigin(0),
-	_gpsAltHome(0),
+	_gpsAltOrigin(0),
-	_visionHome(),
+	_visionOrigin(),
-	_mocapHome(),
+	_mocapOrigin(),
 
 	// flow integration
 	_flowX(0),
@@ -256,12 +255,10 @@ void BlockLocalPositionEstimator::update()
 	// reset pos, vel, and terrain on arming
 	if (!_lastArmedState && armedState) {
 
-		// we just armed, we are at home position on the ground
+		// we just armed, we are at origin on the ground
 		_x(X_x) = 0;
 		_x(X_y) = 0;
-
+		_x(X_z) = 0;
-		// the pressure altitude of home may have drifted, so we don't
-		// reset z to zero
 
 		// reset flow integral
 		_flowX = 0;
@@ -287,7 +284,6 @@ void BlockLocalPositionEstimator::update()
 	bool paramsUpdated = _sub_param_update.updated();
 	bool baroUpdated = _sub_sensor.updated();
 	bool gpsUpdated = _gps_on.get() && _sub_gps.updated();
-	bool homeUpdated = _sub_home.updated();
 	bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated();
 	bool mocapUpdated = _sub_mocap.updated();
 	bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated();
@@ -302,11 +298,6 @@ void BlockLocalPositionEstimator::update()
 		updateSSParams();
 	}
 
-	// update home position projection
-	if (homeUpdated) {
-		updateHome();
-	}
-
 	// is xy valid?
 	bool xy_stddev_ok = sqrtf(math::max(_P(X_x, X_x), _P(X_y, X_y))) < _xy_pub_thresh.get();
 
@@ -370,8 +361,8 @@ void BlockLocalPositionEstimator::update()
 	// if we have no lat, lon initialize projection at 0,0
 	if (_validXY && !_map_ref.init_done) {
 		map_projection_init(&_map_ref,
-				    _init_home_lat.get(),
+				    _init_origin_lat.get(),
-				    _init_home_lon.get());
+				    _init_origin_lon.get());
 	}
 
 	// reinitialize x if necessary
@@ -484,7 +475,7 @@ void BlockLocalPositionEstimator::update()
 		}
 	}
 
-	if (_altHomeInitialized) {
+	if (_altOriginInitialized) {
 		// update all publications if possible
 		publishLocalPos();
 		publishEstimatorStatus();
@@ -570,6 +561,7 @@ void BlockLocalPositionEstimator::correctionLogic(Vector<float, n_x> &dx)
 		dx(X_bz) = 0;
 	}
 
+	// if xy not valid, stop estimating
 	if (!_validXY) {
 		dx(X_x) = 0;
 		dx(X_y) = 0;
@@ -579,12 +571,14 @@ void BlockLocalPositionEstimator::correctionLogic(Vector<float, n_x> &dx)
 		dx(X_by) = 0;
 	}
 
+	// if z not valid, stop estimating
 	if (!_validZ) {
 		dx(X_z) = 0;
 		dx(X_vz) = 0;
 		dx(X_bz) = 0;
 	}
 
+	// if terrain not valid, stop estimating
 	if (!_validTZ) {
 		dx(X_tz) = 0;
 	}
@@ -629,28 +623,6 @@ void BlockLocalPositionEstimator::detectDistanceSensors()
 	}
 }
 
-void BlockLocalPositionEstimator::updateHome()
-{
-	double lat = _sub_home.get().lat;
-	double lon = _sub_home.get().lon;
-	float alt = _sub_home.get().alt;
-
-	// updating home causes absolute measurements
-	// like gps and baro to be off, need to allow it
-	// to reset by resetting covariance
-	initP();
-	mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] home "
-				     "lat %6.2f lon %6.2f alt %5.1f m",
-				     lat, lon, double(alt));
-	map_projection_init(&_map_ref, lat, lon);
-	float delta_alt = alt - _altHome;
-	_altHomeInitialized = true;
-	_altHome = alt;
-	_gpsAltHome += delta_alt;
-	_baroAltHome +=  delta_alt;
-	_visionHome(2) += delta_alt;
-	_mocapHome(2) += delta_alt;
-}
 
 void BlockLocalPositionEstimator::publishLocalPos()
 {
@@ -672,12 +644,12 @@ void BlockLocalPositionEstimator::publishLocalPos()
 		_pub_lpos.get().vy = xLP(X_vy); // east
 		_pub_lpos.get().vz = xLP(X_vz); // down
 		_pub_lpos.get().yaw = _sub_att.get().yaw;
-		_pub_lpos.get().xy_global = _sub_home.get().timestamp != 0; // need home for reference
+		_pub_lpos.get().xy_global = _validXY;
 		_pub_lpos.get().z_global = _baroInitialized;
-		_pub_lpos.get().ref_timestamp = _sub_home.get().timestamp;
+		_pub_lpos.get().ref_timestamp = _timeStamp;
 		_pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI;
 		_pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
-		_pub_lpos.get().ref_alt = _sub_home.get().alt;
+		_pub_lpos.get().ref_alt = _altOrigin;
 		_pub_lpos.get().dist_bottom = _aglLowPass.getState();
 		_pub_lpos.get().dist_bottom_rate = - xLP(X_vz);
 		_pub_lpos.get().surface_bottom_timestamp = _timeStamp;
@@ -725,7 +697,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 	double lon = 0;
 	const Vector<float, n_x> &xLP = _xLowPass.getState();
 	map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon);
-	float alt = -xLP(X_z) + _altHome;
+	float alt = -xLP(X_z) + _altOrigin;
 
 	if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) &&
 	    PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) &&
@@ -741,7 +713,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 		_pub_gpos.get().yaw = _sub_att.get().yaw;
 		_pub_gpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
 		_pub_gpos.get().epv = sqrtf(_P(X_z, X_z));
-		_pub_gpos.get().terrain_alt = _altHome - xLP(X_tz);
+		_pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz);
 		_pub_gpos.get().terrain_alt_valid = _validTZ;
 		_pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout;
 		_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter;

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -19,7 +19,6 @@
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vision_position_estimate.h>
 #include <uORB/topics/att_pos_mocap.h>
@@ -205,7 +204,6 @@ private:
 	float agl();
 	void correctionLogic(Vector<float, n_x> &dx);
 	void detectDistanceSensors();
-	void updateHome();
 
 	// publications
 	void publishLocalPos();
@@ -222,7 +220,6 @@ private:
 	uORB::Subscription<sensor_combined_s> _sub_sensor;
 	uORB::Subscription<parameter_update_s> _sub_param_update;
 	uORB::Subscription<manual_control_setpoint_s> _sub_manual;
-	uORB::Subscription<home_position_s> _sub_home;
 	uORB::Subscription<vehicle_gps_position_s> _sub_gps;
 	uORB::Subscription<vision_position_estimate_s> _sub_vision_pos;
 	uORB::Subscription<att_pos_mocap_s> _sub_mocap;
@@ -292,9 +289,9 @@ private:
 	BlockParamFloat  _pn_b_noise_density;
 	BlockParamFloat  _t_max_grade;
 
-	// init home
+	// init origin
-	BlockParamFloat  _init_home_lat;
+	BlockParamFloat  _init_origin_lat;
-	BlockParamFloat  _init_home_lon;
+	BlockParamFloat  _init_origin_lon;
 
 	// flow gyro filter
 	BlockHighPass _flow_gyro_x_high_pass;
@@ -344,12 +341,12 @@ private:
 	bool _mocapInitialized;
 
 	// reference altitudes
-	float _altHome;
+	float _altOrigin;
-	bool _altHomeInitialized;
+	bool _altOriginInitialized;
-	float _baroAltHome;
+	float _baroAltOrigin;
-	float _gpsAltHome;
+	float _gpsAltOrigin;
-	Vector3f _visionHome;
+	Vector3f _visionOrigin;
-	Vector3f _mocapHome;
+	Vector3f _mocapOrigin;
 
 	// flow integration
 	float _flowX;

src/modules/local_position_estimator/params.c

@@ -308,7 +308,7 @@ PARAM_DEFINE_FLOAT(LPE_T_MAX_GRADE, 1.0f);
 PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.1f);
 
 /**
- * Home latitude for nav w/o GPS
+ * Local origin latitude for nav w/o GPS
  *
  * @group Local Position Estimator
  * @unit deg
@@ -319,7 +319,7 @@ PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.1f);
 PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f);
 
 /**
- * Home longitude for nav w/o GPS
+ * Local origin longitude for nav w/o GPS
  *
  * @group Local Position Estimator
  * @unit deg

src/modules/local_position_estimator/sensors/baro.cpp

@@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::baroInit()
 
 	// if finished
 	if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) {
-		_baroAltHome = _baroStats.getMean()(0);
+		_baroAltOrigin = _baroStats.getMean()(0);
 		mavlink_and_console_log_info(&mavlink_log_pub,
 					     "[lpe] baro init %d m std %d cm",
 					     (int)_baroStats.getMean()(0),
@@ -29,10 +29,10 @@ void BlockLocalPositionEstimator::baroInit()
 		_baroInitialized = true;
 		_baroFault = FAULT_NONE;
 
-		// only initialize alt home with baro if gps is disabled
+		// only initialize alt origin with baro and no gps
-		if (!_altHomeInitialized && !_gps_on.get()) {
+		if (!_altOriginInitialized && !_gps_on.get()) {
-			_altHomeInitialized = true;
+			_altOriginInitialized = true;
-			_altHome = _baroAltHome;
+			_altOrigin = _baroAltOrigin;
 		}
 	}
 }
@@ -54,8 +54,8 @@ void BlockLocalPositionEstimator::baroCorrect()
 
 	if (baroMeasure(y) != OK) { return; }
 
-	// subtract baro home alt
+	// subtract baro origin alt
-	y -= _baroAltHome;
+	y -= _baroAltOrigin;
 
 	// baro measurement matrix
 	Matrix<float, n_y_baro, n_x> C;

src/modules/local_position_estimator/sensors/gps.cpp

@@ -37,27 +37,27 @@ void BlockLocalPositionEstimator::gpsInit()
 
 	// if finished
 	if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) {
-		double gpsLatHome = _gpsStats.getMean()(0);
+		double gpsLatOrigin = _gpsStats.getMean()(0);
-		double gpsLonHome = _gpsStats.getMean()(1);
+		double gpsLonOrigin = _gpsStats.getMean()(1);
 
 		if (!_receivedGps) {
 			_receivedGps = true;
-			map_projection_init(&_map_ref, gpsLatHome, gpsLonHome);
+			map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);
 		}
 
-		_gpsAltHome = _gpsStats.getMean()(2);
+		_gpsAltOrigin = _gpsStats.getMean()(2);
 		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps init "
 					     "lat %6.2f lon %6.2f alt %5.1f m",
-					     gpsLatHome,
+					     gpsLatOrigin,
-					     gpsLonHome,
+					     gpsLonOrigin,
-					     double(_gpsAltHome));
+					     double(_gpsAltOrigin));
 		_gpsInitialized = true;
 		_gpsFault = FAULT_NONE;
 		_gpsStats.reset();
 
-		if (!_altHomeInitialized) {
+		if (!_altOriginInitialized) {
-			_altHomeInitialized = true;
+			_altOriginInitialized = true;
-			_altHome = _gpsAltHome;
+			_altOrigin = _gpsAltOrigin;
 		}
 	}
 }
@@ -92,7 +92,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	float  alt = y_global(2);
 	float px = 0;
 	float py = 0;
-	float pz = -(alt - _gpsAltHome);
+	float pz = -(alt - _gpsAltOrigin);
 	map_projection_project(&_map_ref, lat, lon, &px, &py);
 	Vector<float, 6> y;
 	y.setZero();

src/modules/local_position_estimator/sensors/mocap.cpp

@@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::mocapInit()
 
 	// if finished
 	if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) {
-		_mocapHome = _mocapStats.getMean();
+		_mocapOrigin = _mocapStats.getMean();
 		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap position init: "
 					     "%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m",
 					     double(_mocapStats.getMean()(0)),
@@ -33,9 +33,9 @@ void BlockLocalPositionEstimator::mocapInit()
 		_mocapInitialized = true;
 		_mocapFault = FAULT_NONE;
 
-		if (!_altHomeInitialized) {
+		if (!_altOriginInitialized) {
-			_altHomeInitialized = true;
+			_altOriginInitialized = true;
-			_altHome = _mocapHome(2);
+			_altOrigin = _mocapOrigin(2);
 		}
 	}
 }
@@ -58,8 +58,8 @@ void BlockLocalPositionEstimator::mocapCorrect()
 
 	if (mocapMeasure(y) != OK) { return; }
 
-	// make measurement relative to home
+	// make measurement relative to origin
-	y -= _mocapHome;
+	y -= _mocapOrigin;
 
 	// mocap measurement matrix, measures position
 	Matrix<float, n_y_mocap, n_x> C;

src/modules/local_position_estimator/sensors/vision.cpp

@@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::visionInit()
 
 	// increament sums for mean
 	if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) {
-		_visionHome = _visionStats.getMean();
+		_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)),
@@ -33,9 +33,9 @@ void BlockLocalPositionEstimator::visionInit()
 		_visionInitialized = true;
 		_visionFault = FAULT_NONE;
 
-		if (!_altHomeInitialized) {
+		if (!_altOriginInitialized) {
-			_altHomeInitialized = true;
+			_altOriginInitialized = true;
-			_altHome = _visionHome(2);
+			_altOrigin = _visionOrigin(2);
 		}
 	}
 }
@@ -58,8 +58,8 @@ void BlockLocalPositionEstimator::visionCorrect()
 
 	if (visionMeasure(y) != OK) { return; }
 
-	// make measurement relative to home
+	// make measurement relative to origin
-	y -= _visionHome;
+	y -= _visionOrigin;
 
 	// vision measurement matrix, measures position
 	Matrix<float, n_y_vision, n_x> C;

src/modules/uORB/Subscription.cpp

@@ -63,6 +63,7 @@
 #include "topics/att_pos_mocap.h"
 #include "topics/vision_position_estimate.h"
 #include "topics/control_state.h"
+#include "topics/vehicle_land_detected.h"
 
 #include <px4_defines.h>
 
@@ -179,5 +180,6 @@ template class __EXPORT Subscription<distance_sensor_s>;
 template class __EXPORT Subscription<att_pos_mocap_s>;
 template class __EXPORT Subscription<vision_position_estimate_s>;
 template class __EXPORT Subscription<control_state_s>;
+template class __EXPORT Subscription<vehicle_land_detected_s>;
 
 } // namespace uORB