LPE land bug fix and switch to fusion bit mask.

posix-configs/SITL/init/lpe/iris_opt_flow

@@ -23,7 +23,7 @@ param set SENS_BOARD_X_OFF 0.000001
 param set COM_RC_IN_MODE 1
 param set NAV_DLL_ACT 2
 param set COM_DISARM_LAND 3
-param set NAV_ACC_RAD 12.0
+param set NAV_ACC_RAD 2.0
 param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
@@ -41,14 +41,26 @@ param set MPC_Z_VEL_I 0.15
 param set MPC_XY_VEL_P 0.15
 param set MPC_XY_VEL_I 0.2
 
-# changes for LPE indoor flight
+# changes for optical flow navigation
+param set MC_PITCH_P 5.0
+param set MC_ROLL_P 5.0
+param set MC_ROLLRATE_P 0.2
+param set MC_PITCHRATE_P 0.2
+param set MC_ROLLRATE_I 0.05
+param set MC_PITCHRATE_I 0.05
 param set LPE_GPS_ON 0
 param set MPC_ALT_MODE 1
 param set LPE_T_MAX_GRADE 0
 param set MPC_XY_VEL_MAX 2
 param set MPC_Z_VEL_MAX 2
+param set MPC_TILTMAX_AIR 10
+param set MPC_TILTMAX_LND 10
 param set MPC_XY_P 0.5
 param set MIS_TAKEOFF_ALT 2
+param set NAV_ACC_RAD 1.0
+param set CBRK_GPSFAIL 240024
+param set LPE_FUSION 246
+# 11110110 no vis (1 << 3) yaw and no gps (1 << 0)
 
 replay tryapplyparams
 simulator start -s
@@ -81,6 +93,6 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
 mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
-sdlog2 start -r 100 -e -t -a
+logger start -e -t
 mavlink boot_complete
 replay trystart

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -3,7 +3,6 @@
 #include <px4_posix.h>
 #include <controllib/blocks.hpp>
 #include <mathlib/mathlib.h>
-#include <systemlib/perf_counter.h>
 #include <lib/geo/geo.h>
 #include <matrix/Matrix.hpp>
 
@@ -40,28 +39,6 @@ static const float BIAS_MAX = 1e-1f;
 static const size_t HIST_LEN = 10; // DELAY_MAX / HIST_STEP;
 static const size_t N_DIST_SUBS = 4;
 
-enum fault_t {
-	FAULT_NONE = 0,
-	FAULT_MINOR,
-	FAULT_SEVERE
-};
-
-enum sensor_t {
-	SENSOR_BARO = 0,
-	SENSOR_GPS,
-	SENSOR_LIDAR,
-	SENSOR_FLOW,
-	SENSOR_SONAR,
-	SENSOR_VISION,
-	SENSOR_MOCAP,
-	SENSOR_LAND,
-};
-
-// change this to set when
-// the system will abort correcting a measurement
-// given a fault has been detected
-static const fault_t fault_lvl_disable = FAULT_SEVERE;
-
 // for fault detection
 // chi squared distribution, false alarm probability 0.0001
 // see fault_table.py
@@ -135,15 +112,39 @@ public:
 	enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz, n_y_gps};
 	enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, n_y_vision};
 	enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z, n_y_mocap};
-	enum {Y_land_vx, Y_land_vy, Y_land_agl = 0, n_y_land};
+	enum {Y_land_vx = 0, Y_land_vy, Y_land_agl, n_y_land};
-	enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM, n_poll};
+	enum {POLL_FLOW = 0, POLL_SENSORS, POLL_PARAM, n_poll};
+	enum {
+		FUSE_GPS = 1 << 0,
+		FUSE_FLOW = 1 << 1,
+		FUSE_VIS_POS = 1 << 2,
+		FUSE_VIS_YAW = 1 << 3,
+		FUSE_LAND = 1 << 4,
+		FUSE_PUB_AGL_Z = 1 << 5,
+		FUSE_FLOW_GYRO_COMP = 1 << 6,
+		FUSE_BARO = 1 << 7,
+	};
 
+	enum sensor_t {
+		SENSOR_BARO = 1 << 0,
+		SENSOR_GPS = 1 << 1,
+		SENSOR_LIDAR = 1 << 2,
+		SENSOR_FLOW = 1 << 3,
+		SENSOR_SONAR = 1 << 4,
+		SENSOR_VISION = 1 << 5,
+		SENSOR_MOCAP = 1 << 6,
+		SENSOR_LAND = 1 << 7,
+	};
+
+	enum estimate_t {
+		EST_XY = 1 << 0,
+		EST_Z = 1 << 1,
+		EST_TZ = 1 << 2,
+	};
+
+	// public methods
 	BlockLocalPositionEstimator();
 	void update();
-	Vector<float, n_x> dynamics(
-		float t,
-		const Vector<float, n_x> &x,
-		const Vector<float, n_u> &u);
 	virtual ~BlockLocalPositionEstimator();
 
 private:
@@ -153,6 +154,11 @@ private:
 
 	// methods
 	// ----------------------------
+	//
+	Vector<float, n_x> dynamics(
+		float t,
+		const Vector<float, n_x> &x,
+		const Vector<float, n_u> &u);
 	void initP();
 	void initSS();
 	void updateSSStates();
@@ -189,7 +195,6 @@ private:
 	int  flowMeasure(Vector<float, n_y_flow> &y);
 	void flowCorrect();
 	void flowInit();
-	void flowDeinit();
 	void flowCheckTimeout();
 
 	// vision
@@ -214,10 +219,8 @@ private:
 	void checkTimeouts();
 
 	// misc
-	float agl();
+	inline float agl() { return _x(X_tz) - _x(X_z); }
-	void correctionLogic(Vector<float, n_x> &dx);
+	bool landed();
-	void covPropagationLogic(Matrix<float, n_x, n_x> &dP);
-	void detectDistanceSensors();
 	int getDelayPeriods(float delay, uint8_t *periods);
 
 	// publications
@@ -257,7 +260,7 @@ private:
 	struct map_projection_reference_s _map_ref;
 
 	// general parameters
-	BlockParamInt  _pub_agl_z;
+	BlockParamInt _fusion;
 	BlockParamFloat  _vxy_pub_thresh;
 	BlockParamFloat  _z_pub_thresh;
 
@@ -277,7 +280,6 @@ private:
 	BlockParamFloat  _baro_stddev;
 
 	// gps parameters
-	BlockParamInt   _gps_on;
 	BlockParamFloat  _gps_delay;
 	BlockParamFloat  _gps_xy_stddev;
 	BlockParamFloat  _gps_z_stddev;
@@ -290,21 +292,22 @@ private:
 	BlockParamFloat  _vision_xy_stddev;
 	BlockParamFloat  _vision_z_stddev;
 	BlockParamFloat  _vision_delay;
-	BlockParamInt   _vision_on;
 
 	// mocap parameters
 	BlockParamFloat  _mocap_p_stddev;
 
 	// flow parameters
-	BlockParamInt  _flow_gyro_comp;
 	BlockParamFloat  _flow_z_offset;
 	BlockParamFloat  _flow_scale;
 	//BlockParamFloat  _flow_board_x_offs;
 	//BlockParamFloat  _flow_board_y_offs;
 	BlockParamInt    _flow_min_q;
+	BlockParamFloat  _flow_r;
+	BlockParamFloat  _flow_rr;
 
 	// land parameters
 	BlockParamFloat  _land_z_stddev;
+	BlockParamFloat  _land_vxy_stddev;
 
 	// process noise
 	BlockParamFloat  _pn_p_noise_density;
@@ -342,10 +345,8 @@ private:
 	// misc
 	px4_pollfd_struct_t _polls[3];
 	uint64_t _timeStamp;
+	uint64_t _timeStampLastBaro;
 	uint64_t _time_last_hist;
-	uint64_t _time_last_xy;
-	uint64_t _time_last_z;
-	uint64_t _time_last_tz;
 	uint64_t _time_last_flow;
 	uint64_t _time_last_baro;
 	uint64_t _time_last_gps;
@@ -356,17 +357,6 @@ private:
 	uint64_t _time_last_mocap;
 	uint64_t _time_last_land;
 
-	// initialization flags
-	bool _receivedGps;
-	bool _baroInitialized;
-	bool _gpsInitialized;
-	bool _lidarInitialized;
-	bool _sonarInitialized;
-	bool _flowInitialized;
-	bool _visionInitialized;
-	bool _mocapInitialized;
-	bool _landInitialized;
-
 	// reference altitudes
 	float _altOrigin;
 	bool _altOriginInitialized;
@@ -374,28 +364,13 @@ private:
 	float _gpsAltOrigin;
 
 	// status
-	bool _validXY;
+	bool _receivedGps;
-	bool _validZ;
-	bool _validTZ;
-	bool _xyTimeout;
-	bool _zTimeout;
-	bool _tzTimeout;
 	bool _lastArmedState;
 
-	// sensor faults
+	// masks
-	fault_t _baroFault;
+	uint8_t _sensorTimeout;
-	fault_t _gpsFault;
+	uint8_t _sensorFault;
-	fault_t _lidarFault;
+	uint8_t _estimatorInitialized;
-	fault_t _flowFault;
-	fault_t _sonarFault;
-	fault_t _visionFault;
-	fault_t _mocapFault;
-	fault_t _landFault;
-
-	// performance counters
-	perf_counter_t _loop_perf;
-	perf_counter_t _interval_perf;
-	perf_counter_t _err_perf;
 
 	// state space
 	Vector<float, n_x>  _x; // state vector

src/modules/local_position_estimator/params.c

@@ -2,14 +2,6 @@
 
 // 16 is max name length
 
-/**
- * Publish AGL as Z
- *
- * @group Local Position Estimator
- * @boolean
- */
-PARAM_DEFINE_FLOAT(LPE_PUB_AGL_Z, 0);
-
 /**
  * Optical flow z offset from center
  *
@@ -33,15 +25,26 @@ PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f);
 PARAM_DEFINE_FLOAT(LPE_FLW_SCALE, 1.3f);
 
 /**
- * Optical flow gyro compensation
+ * Optical flow rotation (roll/pitch) noise gain
  *
  * @group Local Position Estimator
- * @unit m
+ * @unit m/s / (rad)
- * @min -1
+ * @min 0.1
- * @max 1
+ * @max 10.0
  * @decimal 3
  */
-PARAM_DEFINE_INT32(LPE_FLW_GYRO_CMP, 1);
+PARAM_DEFINE_FLOAT(LPE_FLW_R, 7.0f);
+
+/**
+ * Optical flow angular velocity noise gain
+ *
+ * @group Local Position Estimator
+ * @unit m/s / (rad/s)
+ * @min 0.0
+ * @max 10.0
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(LPE_FLW_RR, 7.0f);
 
 /**
  * Optical flow minimum quality threshold
@@ -131,19 +134,11 @@ PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.02f);
  * @group Local Position Estimator
  * @unit m
  * @min 0.01
- * @max 3
+ * @max 100
  * @decimal 2
  */
 PARAM_DEFINE_FLOAT(LPE_BAR_Z, 3.0f);
 
-/**
- * Enables GPS data, also forces alt init with GPS
- *
- * @group Local Position Estimator
- * @boolean
- */
-PARAM_DEFINE_INT32(LPE_GPS_ON, 1);
-
 /**
  * GPS delay compensaton
  *
@@ -251,19 +246,11 @@ PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.1f);
  * @group Local Position Estimator
  * @unit m
  * @min 0.01
- * @max 2
+ * @max 100
  * @decimal 3
  */
 PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f);
 
-/**
- * Vision correction
- *
- * @group Local Position Estimator
- * @boolean
- */
-PARAM_DEFINE_INT32(LPE_VIS_ON, 1);
-
 /**
  * Vicon position standard deviation.
  *
@@ -357,7 +344,7 @@ PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.001f);
  * @max 90
  * @decimal 8
  */
-PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f);
+PARAM_DEFINE_FLOAT(LPE_LAT, 47.397742f);
 
 /**
  * Local origin longitude for nav w/o GPS
@@ -368,7 +355,7 @@ PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f);
  * @max 180
  * @decimal 8
  */
-PARAM_DEFINE_FLOAT(LPE_LON, -86.929);
+PARAM_DEFINE_FLOAT(LPE_LON, 8.545594);
 
 /**
  * Cut frequency for state publication
@@ -413,3 +400,43 @@ PARAM_DEFINE_FLOAT(LPE_Z_PUB, 1.0f);
  * @decimal 3
  */
 PARAM_DEFINE_FLOAT(LPE_LAND_Z, 0.03f);
+
+/**
+ * Land detector xy velocity standard deviation
+ *
+ * @group Local Position Estimator
+ * @unit m/s
+ * @min 0.01
+ * @max 10.0
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(LPE_LAND_VXY, 0.05f);
+
+/**
+ * Integer bitmask controlling data fusion
+ *
+ * Set bits in the following positions to enable:
+ * 0 : Set to true to fuse GPS data if available, also requires GPS for altitude init
+ * 1 : Set to true to fuse optical flow data if available
+ * 2 : Set to true to fuse vision position
+ * 3 : Set to true to fuse vision yaw
+ * 4 : Set to true to fuse land detector
+ * 5 : Set to true to publish AGL as local position down component
+ * 6 : Set to true to enable flow gyro compensation
+ * 7 : Set to true to enable baro fusion
+ *
+ * default (247, no vision yaw)
+ *
+ * @group Local Position Estimator
+ * @min 0
+ * @max 255
+ * @bit 0 fuse GPS, requires GPS for alt. init
+ * @bit 1 fuse optical flow
+ * @bit 2 fuse vision position
+ * @bit 3 fuse vision yaw
+ * @bit 4 fuse land detector
+ * @bit 5 pub agl as lpos down
+ * @bit 6 flow gyro compensation
+ * @bit 7 fuse baro
+ */
+PARAM_DEFINE_INT32(LPE_FUSION, 247);

src/modules/local_position_estimator/sensors/baro.cpp

@@ -26,8 +26,8 @@ void BlockLocalPositionEstimator::baroInit()
 					     "[lpe] baro init %d m std %d cm",
 					     (int)_baroStats.getMean()(0),
 					     (int)(100 * _baroStats.getStdDev()(0)));
-		_baroInitialized = true;
+		_sensorTimeout &= ~SENSOR_BARO;
-		_baroFault = FAULT_NONE;
+		_sensorFault &= ~SENSOR_BARO;
 
 		if (!_altOriginInitialized) {
 			_altOriginInitialized = true;
@@ -74,25 +74,21 @@ void BlockLocalPositionEstimator::baroCorrect()
 	float beta = (r.transpose() * (S_I * r))(0, 0);
 
 	if (beta > BETA_TABLE[n_y_baro]) {
-		if (_baroFault < FAULT_MINOR) {
+		if (!(_sensorFault & SENSOR_BARO)) {
-			if (beta > 2.0f * BETA_TABLE[n_y_baro]) {
+			mavlink_log_critical(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
-				mavlink_log_critical(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
+					     double(r(0)), double(beta));
-						     double(r(0)), double(beta));
+			_sensorFault |= SENSOR_BARO;
-			}
-
-			_baroFault = FAULT_MINOR;
 		}
 
-	} else if (_baroFault) {
+	} else if (_sensorFault & SENSOR_BARO) {
-		_baroFault = FAULT_NONE;
+		_sensorFault &= ~SENSOR_BARO;
-		//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro OK");
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro OK");
 	}
 
 	// kalman filter correction if no fault
-	if (_baroFault < fault_lvl_disable) {
+	if (!(_sensorFault & SENSOR_BARO)) {
 		Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
-		correctionLogic(dx);
 		_x += dx;
 		_P -= K * C * _P;
 	}
@@ -101,8 +97,8 @@ void BlockLocalPositionEstimator::baroCorrect()
 void BlockLocalPositionEstimator::baroCheckTimeout()
 {
 	if (_timeStamp - _time_last_baro > BARO_TIMEOUT) {
-		if (_baroInitialized) {
+		if (!(_sensorTimeout & SENSOR_BARO)) {
-			_baroInitialized = false;
+			_sensorTimeout |= SENSOR_BARO;
 			_baroStats.reset();
 			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro timeout ");
 		}

src/modules/local_position_estimator/sensors/flow.cpp

@@ -29,17 +29,11 @@ void BlockLocalPositionEstimator::flowInit()
 					     "quality %d std %d",
 					     int(_flowQStats.getMean()(0)),
 					     int(_flowQStats.getStdDev()(0)));
-		_flowInitialized = true;
+		_sensorTimeout &= ~SENSOR_FLOW;
-		_flowFault = FAULT_NONE;
+		_sensorFault &= ~SENSOR_FLOW;
 	}
 }
 
-void BlockLocalPositionEstimator::flowDeinit()
-{
-	_flowInitialized = false;
-	_flowQStats.reset();
-}
-
 int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 {
 	// check for sane pitch/roll
@@ -60,7 +54,7 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 	}
 
 	// calculate range to center of image for flow
-	if (!_validTZ) {
+	if (!(_estimatorInitialized & EST_TZ)) {
 		return -1;
 	}
 
@@ -82,7 +76,7 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 	float gyro_x_rad = 0;
 	float gyro_y_rad = 0;
 
-	if (_flow_gyro_comp.get()) {
+	if (_fusion.get() & FUSE_FLOW_GYRO_COMP) {
 		gyro_x_rad = _flow_gyro_x_high_pass.update(
 				     _sub_flow.get().gyro_x_rate_integral);
 		gyro_y_rad = _flow_gyro_y_high_pass.update(
@@ -161,7 +155,15 @@ void BlockLocalPositionEstimator::flowCorrect()
 	// compute polynomial value
 	float flow_vxy_stddev = p[0] * h + p[1] * h * h + p[2] * v + p[3] * v * h + p[4] * v * h * h;
 
-	R(Y_flow_vx, Y_flow_vx) = flow_vxy_stddev * flow_vxy_stddev;
+	float rotrate_sq = _sub_att.get().rollspeed * _sub_att.get().rollspeed
+			   + _sub_att.get().pitchspeed * _sub_att.get().pitchspeed
+			   + _sub_att.get().yawspeed * _sub_att.get().yawspeed;
+
+	float rot_sq = _eul(0) * _eul(0) + _eul(1) * _eul(1);
+
+	R(Y_flow_vx, Y_flow_vx) = flow_vxy_stddev * flow_vxy_stddev +
+				  _flow_r.get() * _flow_r.get() * rot_sq +
+				  _flow_rr.get() * _flow_rr.get() * rotrate_sq;
 	R(Y_flow_vy, Y_flow_vy) = R(Y_flow_vx, Y_flow_vx);
 
 	// residual
@@ -179,21 +181,20 @@ void BlockLocalPositionEstimator::flowCorrect()
 	float beta = (r.transpose() * (S_I * r))(0, 0);
 
 	if (beta > BETA_TABLE[n_y_flow]) {
-		if (_flowFault < FAULT_MINOR) {
+		if (!(_sensorFault & SENSOR_FLOW)) {
-			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow fault,  beta %5.2f", double(beta));
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow fault,  beta %5.2f", double(beta));
-			_flowFault = FAULT_MINOR;
+			_sensorFault |= SENSOR_FLOW;
 		}
 
-	} else if (_flowFault) {
+	} else if (_sensorFault & SENSOR_FLOW) {
-		_flowFault = FAULT_NONE;
+		_sensorFault &= ~SENSOR_FLOW;
-		//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow OK");
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow OK");
 	}
 
-	if (_flowFault < fault_lvl_disable) {
+	if (!(_sensorFault & SENSOR_FLOW)) {
 		Matrix<float, n_x, n_y_flow> K =
 			_P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
-		correctionLogic(dx);
 		_x += dx;
 		_P -= K * C * _P;
 
@@ -204,8 +205,9 @@ void BlockLocalPositionEstimator::flowCorrect()
 void BlockLocalPositionEstimator::flowCheckTimeout()
 {
 	if (_timeStamp - _time_last_flow > FLOW_TIMEOUT) {
-		if (_flowInitialized) {
+		if (!(_sensorTimeout & SENSOR_FLOW)) {
-			flowDeinit();
+			_sensorTimeout |= SENSOR_FLOW;
+			_flowQStats.reset();
 			mavlink_log_critical(&mavlink_log_pub, "[lpe] flow timeout ");
 		}
 	}

src/modules/local_position_estimator/sensors/gps.cpp

@@ -42,8 +42,8 @@ void BlockLocalPositionEstimator::gpsInit()
 		double gpsLon = _gpsStats.getMean()(1);
 		float gpsAlt = _gpsStats.getMean()(2);
 
-		_gpsInitialized = true;
+		_sensorTimeout &= ~SENSOR_GPS;
-		_gpsFault = FAULT_NONE;
+		_sensorFault &= ~SENSOR_GPS;
 		_gpsStats.reset();
 
 		if (!_receivedGps) {
@@ -55,25 +55,29 @@ void BlockLocalPositionEstimator::gpsInit()
 			_gpsAltOrigin = gpsAlt + _x(X_z);
 
 			// find lat, lon of current origin by subtracting x and y
-			double gpsLatOrigin = 0;
+			// if not using vision position since vision will
-			double gpsLonOrigin = 0;
+			// have it's own origin, not necessarily where vehicle starts
-			// reproject at current coordinates
+			if (!(_fusion.get() & FUSE_VIS_POS)) {
-			map_projection_init(&_map_ref, gpsLat, gpsLon);
+				double gpsLatOrigin = 0;
-			// find origin
+				double gpsLonOrigin = 0;
-			map_projection_reproject(&_map_ref, -_x(X_x), -_x(X_y), &gpsLatOrigin, &gpsLonOrigin);
+				// reproject at current coordinates
-			// reinit origin
+				map_projection_init(&_map_ref, gpsLat, gpsLon);
-			map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);
+				// find origin
+				map_projection_reproject(&_map_ref, -_x(X_x), -_x(X_y), &gpsLatOrigin, &gpsLonOrigin);
+				// reinit origin
+				map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);
 
-			// always override alt origin on first GPS to fix
+				// always override alt origin on first GPS to fix
-			// possible baro offset in global altitude at init
+				// possible baro offset in global altitude at init
-			_altOrigin = _gpsAltOrigin;
+				_altOrigin = _gpsAltOrigin;
-			_altOriginInitialized = true;
+				_altOriginInitialized = true;
+			}
 
 			PX4_INFO("[lpe] gps init "
 				 "lat %6.2f lon %6.2f alt %5.1f m",
-				 gpsLatOrigin,
+				 gpsLat,
-				 gpsLonOrigin,
+				 gpsLon,
-				 double(_gpsAltOrigin));
+				 double(gpsAlt));
 		}
 	}
 }
@@ -187,26 +191,22 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	float beta = (r.transpose() * (S_I * r))(0, 0);
 
 	if (beta > BETA_TABLE[n_y_gps]) {
-		if (_gpsFault < FAULT_MINOR) {
+		if (!(_sensorFault & SENSOR_GPS)) {
-			if (beta > 3.0f * BETA_TABLE[n_y_gps]) {
+			mavlink_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g",
-				mavlink_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g",
+					     double(r(0)*r(0) / S_I(0, 0)),  double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)),
-						     double(r(0)*r(0) / S_I(0, 0)),  double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)),
+					     double(r(3)*r(3) / S_I(3, 3)),  double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5)));
-						     double(r(3)*r(3) / S_I(3, 3)),  double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5)));
+			_sensorFault |= SENSOR_GPS;
-			}
-
-			_gpsFault = FAULT_MINOR;
 		}
 
-	} else if (_gpsFault) {
+	} else if (_sensorFault & SENSOR_GPS) {
-		_gpsFault = FAULT_NONE;
+		_sensorFault &= ~SENSOR_GPS;
-		//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS OK");
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS OK");
 	}
 
 	// kalman filter correction if no hard fault
-	if (_gpsFault < fault_lvl_disable) {
+	if (!(_sensorFault & SENSOR_GPS)) {
 		Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
-		correctionLogic(dx);
 		_x += dx;
 		_P -= K * C * _P;
 	}
@@ -215,8 +215,8 @@ void BlockLocalPositionEstimator::gpsCorrect()
 void BlockLocalPositionEstimator::gpsCheckTimeout()
 {
 	if (_timeStamp - _time_last_gps > GPS_TIMEOUT) {
-		if (_gpsInitialized) {
+		if (!(_sensorTimeout & SENSOR_GPS)) {
-			_gpsInitialized = false;
+			_sensorTimeout |= SENSOR_GPS;
 			_gpsStats.reset();
 			mavlink_log_critical(&mavlink_log_pub, "[lpe] GPS timeout ");
 		}

src/modules/local_position_estimator/sensors/land.cpp

@@ -22,8 +22,8 @@ void BlockLocalPositionEstimator::landInit()
 	// if finished
 	if (_landCount > REQ_LAND_INIT_COUNT) {
 		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land init");
-		_landInitialized = true;
+		_sensorTimeout &= ~SENSOR_LAND;
-		_landFault = FAULT_NONE;
+		_sensorFault &= ~SENSOR_LAND;
 	}
 }
 
@@ -54,8 +54,8 @@ void BlockLocalPositionEstimator::landCorrect()
 	// use parameter covariance
 	SquareMatrix<float, n_y_land> R;
 	R.setZero();
-	R(Y_land_vx, Y_land_vx) = 0.1;
+	R(Y_land_vx, Y_land_vx) = _land_vxy_stddev.get() * _land_vxy_stddev.get();
-	R(Y_land_vy, Y_land_vy) = 0.1;
+	R(Y_land_vy, Y_land_vy) = _land_vxy_stddev.get() * _land_vxy_stddev.get();
 	R(Y_land_agl, Y_land_agl) = _land_z_stddev.get() * _land_z_stddev.get();
 
 	// residual
@@ -68,24 +68,23 @@ void BlockLocalPositionEstimator::landCorrect()
 	float beta = (r.transpose() * (S_I * r))(0, 0);
 
 	if (beta > BETA_TABLE[n_y_land]) {
-		if (_landFault < FAULT_MINOR) {
+		if (!(_sensorFault & SENSOR_LAND)) {
-			_landFault = FAULT_MINOR;
+			_sensorFault |= SENSOR_LAND;
 			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land fault,  beta %5.2f", double(beta));
 		}
 
 		// abort correction
 		return;
 
-	} else if (_landFault) { // disable fault if ok
+	} else if (_sensorFault & SENSOR_LAND) {
-		_landFault = FAULT_NONE;
+		_sensorFault &= ~SENSOR_LAND;
-		//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land OK");
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land OK");
 	}
 
 	// kalman filter correction if no fault
-	if (_landFault < fault_lvl_disable) {
+	if (!(_sensorFault & SENSOR_LAND)) {
 		Matrix<float, n_x, n_y_land> K = _P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
-		correctionLogic(dx);
 		_x += dx;
 		_P -= K * C * _P;
 	}
@@ -94,10 +93,11 @@ void BlockLocalPositionEstimator::landCorrect()
 void BlockLocalPositionEstimator::landCheckTimeout()
 {
 	if (_timeStamp - _time_last_land > LAND_TIMEOUT) {
-		if (_landInitialized) {
+		if (!(_sensorTimeout & SENSOR_LAND)) {
-			_landInitialized = false;
+			_sensorTimeout |= SENSOR_LAND;
 			_landCount = 0;
 			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land timeout ");
 		}
 	}
 }
+

src/modules/local_position_estimator/sensors/lidar.cpp

@@ -25,8 +25,8 @@ void BlockLocalPositionEstimator::lidarInit()
 					     "mean %d cm stddev %d cm",
 					     int(100 * _lidarStats.getMean()(0)),
 					     int(100 * _lidarStats.getStdDev()(0)));
-		_lidarInitialized = true;
+		_sensorTimeout &= ~SENSOR_LIDAR;
-		_lidarFault = FAULT_NONE;
+		_sensorFault &= ~SENSOR_LIDAR;
 	}
 }
 
@@ -100,24 +100,23 @@ void BlockLocalPositionEstimator::lidarCorrect()
 	float beta = (r.transpose() * (S_I * r))(0, 0);
 
 	if (beta > BETA_TABLE[n_y_lidar]) {
-		if (_lidarFault < FAULT_MINOR) {
+		if (!(_sensorFault & SENSOR_LIDAR)) {
-			_lidarFault = FAULT_MINOR;
 			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar fault,  beta %5.2f", double(beta));
+			_sensorFault |= SENSOR_LIDAR;
 		}
 
 		// abort correction
 		return;
 
-	} else if (_lidarFault) { // disable fault if ok
+	} else if (_sensorFault & SENSOR_LIDAR) {
-		_lidarFault = FAULT_NONE;
+		_sensorFault &= ~SENSOR_LIDAR;
-		//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar OK");
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar OK");
 	}
 
 	// kalman filter correction if no fault
-	if (_lidarFault < fault_lvl_disable) {
+	if (!(_sensorFault & SENSOR_LIDAR)) {
 		Matrix<float, n_x, n_y_lidar> K = _P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
-		correctionLogic(dx);
 		_x += dx;
 		_P -= K * C * _P;
 	}
@@ -126,8 +125,8 @@ void BlockLocalPositionEstimator::lidarCorrect()
 void BlockLocalPositionEstimator::lidarCheckTimeout()
 {
 	if (_timeStamp - _time_last_lidar > LIDAR_TIMEOUT) {
-		if (_lidarInitialized) {
+		if (!(_sensorTimeout & SENSOR_LIDAR)) {
-			_lidarInitialized = false;
+			_sensorTimeout |= SENSOR_LIDAR;
 			_lidarStats.reset();
 			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar timeout ");
 		}

src/modules/local_position_estimator/sensors/mocap.cpp

@@ -29,8 +29,8 @@ void BlockLocalPositionEstimator::mocapInit()
 					     double(_mocapStats.getStdDev()(0)),
 					     double(_mocapStats.getStdDev()(1)),
 					     double(_mocapStats.getStdDev()(2)));
-		_mocapInitialized = true;
+		_sensorTimeout &= ~SENSOR_MOCAP;
-		_mocapFault = FAULT_NONE;
+		_sensorFault &= ~SENSOR_MOCAP;
 
 		if (!_altOriginInitialized) {
 			_altOriginInitialized = true;
@@ -81,21 +81,20 @@ void BlockLocalPositionEstimator::mocapCorrect()
 	float beta = (r.transpose() * (S_I * r))(0, 0);
 
 	if (beta > BETA_TABLE[n_y_mocap]) {
-		if (_mocapFault < FAULT_MINOR) {
+		if (!(_sensorFault & SENSOR_MOCAP)) {
 			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap fault, beta %5.2f", double(beta));
-			_mocapFault = FAULT_MINOR;
+			_sensorFault |= SENSOR_MOCAP;
 		}
 
-	} else if (_mocapFault) {
+	} else if (_sensorFault & SENSOR_MOCAP) {
-		_mocapFault = FAULT_NONE;
+		_sensorFault &= ~SENSOR_MOCAP;
 		//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap OK");
 	}
 
 	// kalman filter correction if no fault
-	if (_mocapFault < fault_lvl_disable) {
+	if (!(_sensorFault & SENSOR_MOCAP)) {
 		Matrix<float, n_x, n_y_mocap> K = _P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
-		correctionLogic(dx);
 		_x += dx;
 		_P -= K * C * _P;
 	}
@@ -104,8 +103,8 @@ void BlockLocalPositionEstimator::mocapCorrect()
 void BlockLocalPositionEstimator::mocapCheckTimeout()
 {
 	if (_timeStamp - _time_last_mocap > MOCAP_TIMEOUT) {
-		if (_mocapInitialized) {
+		if (!(_sensorTimeout & SENSOR_MOCAP)) {
-			_mocapInitialized = false;
+			_sensorTimeout |= SENSOR_MOCAP;
 			_mocapStats.reset();
 			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap timeout ");
 		}

src/modules/local_position_estimator/sensors/sonar.cpp

@@ -38,8 +38,8 @@ void BlockLocalPositionEstimator::sonarInit()
 				 "mean %d cm std %d cm",
 				 int(100 * _sonarStats.getMean()(0)),
 				 int(100 * _sonarStats.getStdDev()(0)));
-			_sonarInitialized = true;
+			_sensorTimeout &= ~SENSOR_SONAR;
-			_sonarFault = FAULT_NONE;
+			_sensorFault &= ~SENSOR_SONAR;
 		}
 	}
 }
@@ -116,25 +116,24 @@ void BlockLocalPositionEstimator::sonarCorrect()
 	float beta = (r.transpose()  * (S_I * r))(0, 0);
 
 	if (beta > BETA_TABLE[n_y_sonar]) {
-		if (_sonarFault < FAULT_MINOR) {
+		if (!(_sensorFault & SENSOR_SONAR)) {
-			_sonarFault = FAULT_MINOR;
+			_sensorFault |= SENSOR_SONAR;
-			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar fault,  beta %5.2f", double(beta));
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar fault,  beta %5.2f", double(beta));
 		}
 
 		// abort correction
 		return;
 
-	} else if (_sonarFault) {
+	} else if (_sensorFault & SENSOR_SONAR) {
-		_sonarFault = FAULT_NONE;
+		_sensorFault &= ~SENSOR_SONAR;
 		//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar OK");
 	}
 
 	// kalman filter correction if no fault
-	if (_sonarFault < fault_lvl_disable) {
+	if (!(_sensorFault & SENSOR_SONAR)) {
 		Matrix<float, n_x, n_y_sonar> K =
 			_P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
-		correctionLogic(dx);
 		_x += dx;
 		_P -= K * C * _P;
 	}
@@ -144,8 +143,8 @@ void BlockLocalPositionEstimator::sonarCorrect()
 void BlockLocalPositionEstimator::sonarCheckTimeout()
 {
 	if (_timeStamp - _time_last_sonar > SONAR_TIMEOUT) {
-		if (_sonarInitialized) {
+		if (!(_sensorTimeout & SENSOR_SONAR)) {
-			_sonarInitialized = false;
+			_sensorTimeout |= SENSOR_SONAR;
 			_sonarStats.reset();
 			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar timeout ");
 		}

src/modules/local_position_estimator/sensors/vision.cpp

@@ -34,8 +34,8 @@ void BlockLocalPositionEstimator::visionInit()
 					     double(_visionStats.getStdDev()(0)),
 					     double(_visionStats.getStdDev()(1)),
 					     double(_visionStats.getStdDev()(2)));
-		_visionInitialized = true;
+		_sensorTimeout &= ~SENSOR_VISION;
-		_visionFault = FAULT_NONE;
+		_sensorFault &= ~SENSOR_VISION;
 
 		if (!_altOriginInitialized) {
 			_altOriginInitialized = true;
@@ -91,21 +91,20 @@ void BlockLocalPositionEstimator::visionCorrect()
 	float beta = (r.transpose() * (S_I * r))(0, 0);
 
 	if (beta > BETA_TABLE[n_y_vision]) {
-		if (_visionFault < FAULT_MINOR) {
+		if (!(_sensorFault & SENSOR_VISION)) {
-			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position fault, beta %5.2f", double(beta));
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position fault, beta %5.2f", double(beta));
-			_visionFault = FAULT_MINOR;
+			_sensorFault |= SENSOR_VISION;
 		}
 
-	} else if (_visionFault) {
+	} else if (_sensorFault & SENSOR_VISION) {
-		_visionFault = FAULT_NONE;
+		_sensorFault &= ~SENSOR_VISION;
-		//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position OK");
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position OK");
 	}
 
 	// kalman filter correction if no fault
-	if (_visionFault <  fault_lvl_disable) {
+	if (!(_sensorFault & SENSOR_VISION)) {
 		Matrix<float, n_x, n_y_vision> K = _P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
-		correctionLogic(dx);
 		_x += dx;
 		_P -= K * C * _P;
 	}
@@ -114,8 +113,8 @@ void BlockLocalPositionEstimator::visionCorrect()
 void BlockLocalPositionEstimator::visionCheckTimeout()
 {
 	if (_timeStamp - _time_last_vision_p > VISION_TIMEOUT) {
-		if (_visionInitialized) {
+		if (!(_sensorTimeout & SENSOR_VISION)) {
-			_visionInitialized = false;
+			_sensorTimeout |= SENSOR_VISION;
 			_visionStats.reset();
 			mavlink_log_critical(&mavlink_log_pub, "[lpe] vision position timeout ");
 		}