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.cpp

@@ -15,11 +15,12 @@ static const uint32_t 		EST_SRC_TIMEOUT = 10000; // 0.01 s
 static const uint32_t 		EST_STDDEV_XY_VALID = 2.0; // 2.0 m
 static const uint32_t 		EST_STDDEV_Z_VALID = 2.0; // 2.0 m
 static const uint32_t 		EST_STDDEV_TZ_VALID = 2.0; // 2.0 m
-static const bool integrate = true; // use accel for integrating
 
 static const float P_MAX = 1.0e6f; // max allowed value in state covariance
 static const float LAND_RATE = 10.0f; // rate of land detector correction
 
+static const char *msg_label = "[lpe] ";  // rate of land detector correction
+
 BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// this block has no parent, and has name LPE
 	SuperBlock(NULL, "LPE"),
@@ -59,7 +60,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_map_ref(),
 
 	// block parameters
-	_pub_agl_z(this, "PUB_AGL_Z"),
+	_fusion(this, "FUSION"),
 	_vxy_pub_thresh(this, "VXY_PUB"),
 	_z_pub_thresh(this, "Z_PUB"),
 	_sonar_z_stddev(this, "SNR_Z"),
@@ -69,7 +70,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_accel_xy_stddev(this, "ACC_XY"),
 	_accel_z_stddev(this, "ACC_Z"),
 	_baro_stddev(this, "BAR_Z"),
-	_gps_on(this, "GPS_ON"),
 	_gps_delay(this, "GPS_DELAY"),
 	_gps_xy_stddev(this, "GPS_XY"),
 	_gps_z_stddev(this, "GPS_Z"),
@@ -80,15 +80,16 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_vision_xy_stddev(this, "VIS_XY"),
 	_vision_z_stddev(this, "VIS_Z"),
 	_vision_delay(this, "VIS_DELAY"),
-	_vision_on(this, "VIS_ON"),
 	_mocap_p_stddev(this, "VIC_P"),
-	_flow_gyro_comp(this, "FLW_GYRO_CMP"),
 	_flow_z_offset(this, "FLW_OFF_Z"),
 	_flow_scale(this, "FLW_SCALE"),
 	//_flow_board_x_offs(NULL, "SENS_FLW_XOFF"),
 	//_flow_board_y_offs(NULL, "SENS_FLW_YOFF"),
 	_flow_min_q(this, "FLW_QMIN"),
+	_flow_r(this, "FLW_R"),
+	_flow_rr(this, "FLW_RR"),
 	_land_z_stddev(this, "LAND_Z"),
+	_land_vxy_stddev(this, "LAND_VXY"),
 	_pn_p_noise_density(this, "PN_P"),
 	_pn_v_noise_density(this, "PN_V"),
 	_pn_b_noise_density(this, "PN_B"),
@@ -124,10 +125,8 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// misc
 	_polls(),
 	_timeStamp(hrt_absolute_time()),
+	_timeStampLastBaro(hrt_absolute_time()),
 	_time_last_hist(0),
-	_time_last_xy(0),
-	_time_last_z(0),
-	_time_last_tz(0),
 	_time_last_flow(0),
 	_time_last_baro(0),
 	_time_last_gps(0),
@@ -138,17 +137,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_time_last_mocap(0),
 	_time_last_land(0),
 
-	// initialization flags
-	_receivedGps(false),
-	_baroInitialized(false),
-	_gpsInitialized(false),
-	_lidarInitialized(false),
-	_sonarInitialized(false),
-	_flowInitialized(false),
-	_visionInitialized(false),
-	_mocapInitialized(false),
-	_landInitialized(false),
-
 	// reference altitudes
 	_altOrigin(0),
 	_altOriginInitialized(false),
@@ -156,28 +144,13 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_gpsAltOrigin(0),
 
 	// status
-	_validXY(false),
+	_receivedGps(false),
-	_validZ(false),
-	_validTZ(false),
-	_xyTimeout(true),
-	_zTimeout(true),
-	_tzTimeout(true),
 	_lastArmedState(false),
 
-	// faults
+	// masks
-	_baroFault(FAULT_NONE),
+	_sensorTimeout(255),
-	_gpsFault(FAULT_NONE),
+	_sensorFault(0),
-	_lidarFault(FAULT_NONE),
+	_estimatorInitialized(0),
-	_flowFault(FAULT_NONE),
-	_sonarFault(FAULT_NONE),
-	_visionFault(FAULT_NONE),
-	_mocapFault(FAULT_NONE),
-	_landFault(FAULT_NONE),
-
-	// loop performance
-	_loop_perf(),
-	_interval_perf(),
-	_err_perf(),
 
 	// kf matrices
 	_x(), _u(), _P(), _R_att(), _eul()
@@ -203,18 +176,22 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_u.setZero();
 	initSS();
 
-	// perf counters
-	_loop_perf = perf_alloc(PC_ELAPSED,
-				"local_position_estimator_runtime");
-	//_interval_perf = perf_alloc(PC_INTERVAL,
-	//"local_position_estimator_interval");
-	_err_perf = perf_alloc(PC_COUNT, "local_position_estimator_err");
-
 	// map
 	_map_ref.init_done = false;
 
 	// intialize parameter dependent matrices
 	updateParams();
+
+	// print fusion settings to console
+	printf("[lpe] fuse gps: %d, flow: %d, vis_pos: %d, "
+	       "vis_yaw: %d, land: %d, pub_agl_z: %d, flow_gyro: %d\n",
+	       (_fusion.get() & FUSE_GPS) != 0,
+	       (_fusion.get() & FUSE_FLOW) != 0,
+	       (_fusion.get() & FUSE_VIS_POS) != 0,
+	       (_fusion.get() & FUSE_VIS_YAW) != 0,
+	       (_fusion.get() & FUSE_LAND) != 0,
+	       (_fusion.get() & FUSE_PUB_AGL_Z) != 0,
+	       (_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0);
 }
 
 BlockLocalPositionEstimator::~BlockLocalPositionEstimator()
@@ -236,8 +213,6 @@ void BlockLocalPositionEstimator::update()
 	int ret = px4_poll(_polls, 3, 100);
 
 	if (ret < 0) {
-		/* poll error, count it in perf */
-		perf_count(_err_perf);
 		return;
 	}
 
@@ -252,7 +227,32 @@ void BlockLocalPositionEstimator::update()
 	bool armedState = _sub_armed.get().armed;
 
 	if (!armedState && (_sub_lidar == NULL || _sub_sonar == NULL)) {
-		detectDistanceSensors();
+
+		// detect distance sensors
+		for (int i = 0; i < N_DIST_SUBS; i++) {
+			uORB::Subscription<distance_sensor_s> *s = _dist_subs[i];
+
+			if (s == _sub_lidar || s == _sub_sonar) { continue; }
+
+			if (s->updated()) {
+				s->update();
+
+				if (s->get().timestamp == 0) { continue; }
+
+				if (s->get().type == \
+				    distance_sensor_s::MAV_DISTANCE_SENSOR_LASER &&
+				    _sub_lidar == NULL) {
+					_sub_lidar = s;
+					mavlink_and_console_log_info(&mavlink_log_pub, "%sLidar detected with ID %i", msg_label, i);
+
+				} else if (s->get().type == \
+					   distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND &&
+					   _sub_sonar == NULL) {
+					_sub_sonar = s;
+					mavlink_and_console_log_info(&mavlink_log_pub, "%sSonar detected with ID %i", msg_label, i);
+				}
+			}
+		}
 	}
 
 	// reset pos, vel, and terrain on arming
@@ -286,17 +286,29 @@ void BlockLocalPositionEstimator::update()
 	// see which updates are available
 	bool flowUpdated = _sub_flow.updated();
 	bool paramsUpdated = _sub_param_update.updated();
-	bool baroUpdated = _sub_sensor.updated();
+	bool baroUpdated = false;
-	bool gpsUpdated = _gps_on.get() && _sub_gps.updated();
+
-	bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated();
+	if ((_fusion.get() & FUSE_BARO) && _sub_sensor.updated()) {
+		int32_t baro_timestamp_relative = _sub_sensor.get().baro_timestamp_relative;
+
+		if (baro_timestamp_relative != _sub_sensor.get().RELATIVE_TIMESTAMP_INVALID) {
+			uint64_t baro_timestamp = _sub_sensor.get().timestamp + \
+						  _sub_sensor.get().baro_timestamp_relative;
+
+			if (baro_timestamp != _timeStampLastBaro) {
+				baroUpdated = true;
+				_timeStampLastBaro = baro_timestamp;
+			}
+		}
+	}
+
+	bool gpsUpdated = (_fusion.get() & FUSE_GPS) && _sub_gps.updated();
+	bool visionUpdated = (_fusion.get() & FUSE_VIS_POS) && _sub_vision_pos.updated();
 	bool mocapUpdated = _sub_mocap.updated();
 	bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated();
 	bool sonarUpdated = (_sub_sonar != NULL) && _sub_sonar->updated();
-	bool landUpdated = (
+	bool landUpdated = landed()
-				   (_sub_land.get().landed ||
+			   && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE); // throttle rate
-				    ((!_sub_armed.get().armed) && (!_sub_land.get().freefall)))
-				   && (!(_lidarInitialized || _mocapInitialized || _visionInitialized || _sonarInitialized))
-				   && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE));
 
 	// get new data
 	updateSubscriptions();
@@ -314,16 +326,21 @@ void BlockLocalPositionEstimator::update()
 		vxy_stddev_ok = true;
 	}
 
-	if (_validXY) {
+	if (_estimatorInitialized & EST_XY) {
 		// if valid and gps has timed out, set to not valid
-		if (!vxy_stddev_ok && !_gpsInitialized) {
+		if (!vxy_stddev_ok && (_sensorTimeout & SENSOR_GPS)) {
-			_validXY = false;
+			_estimatorInitialized &= ~EST_XY;
 		}
 
 	} else {
 		if (vxy_stddev_ok) {
-			if (_flowInitialized || _gpsInitialized || _visionInitialized || _mocapInitialized) {
+			if (!(_sensorTimeout & SENSOR_GPS)
-				_validXY = true;
+			    || !(_sensorTimeout & SENSOR_FLOW)
+			    || !(_sensorTimeout & SENSOR_VISION)
+			    || !(_sensorTimeout & SENSOR_MOCAP)
+			    || !(_sensorTimeout & SENSOR_LAND)
+			   ) {
+				_estimatorInitialized |= EST_XY;
 			}
 		}
 	}
@@ -331,50 +348,37 @@ void BlockLocalPositionEstimator::update()
 	// is z valid?
 	bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();
 
-	if (_validZ) {
+	if (_estimatorInitialized & EST_Z) {
 		// if valid and baro has timed out, set to not valid
-		if (!z_stddev_ok && !_baroInitialized) {
+		if (!z_stddev_ok && (_sensorTimeout & SENSOR_BARO)) {
-			_validZ = false;
+			_estimatorInitialized &= ~EST_Z;
 		}
 
 	} else {
 		if (z_stddev_ok) {
-			_validZ = true;
+			_estimatorInitialized |= EST_Z;
 		}
 	}
 
 	// is terrain valid?
 	bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();
 
-	if (_validTZ) {
+	if (_estimatorInitialized & EST_TZ) {
 		if (!tz_stddev_ok) {
-			_validTZ = false;
+			_estimatorInitialized &= ~EST_TZ;
 		}
 
 	} else {
 		if (tz_stddev_ok) {
-			_validTZ = true;
+			_estimatorInitialized |= EST_TZ;
 		}
 	}
 
-	// timeouts
-	if (_validXY) {
-		_time_last_xy = _timeStamp;
-	}
-
-	if (_validZ) {
-		_time_last_z = _timeStamp;
-	}
-
-	if (_validTZ) {
-		_time_last_tz = _timeStamp;
-	}
-
 	// check timeouts
 	checkTimeouts();
 
 	// if we have no lat, lon initialize projection at 0,0
-	if (_validXY && !_map_ref.init_done) {
+	if ((_estimatorInitialized & EST_XY) && !_map_ref.init_done) {
 		map_projection_init(&_map_ref,
 				    _init_origin_lat.get(),
 				    _init_origin_lon.get());
@@ -389,7 +393,7 @@ void BlockLocalPositionEstimator::update()
 		// don't want it to take too long
 		if (!PX4_ISFINITE(_x(i))) {
 			reinit_x = true;
-			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x, x(%d) not finite", i);
+			mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x, x(%d) not finite", msg_label, i);
 			break;
 		}
 	}
@@ -399,7 +403,7 @@ void BlockLocalPositionEstimator::update()
 			_x(i) = 0;
 		}
 
-		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x");
+		mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x", msg_label);
 	}
 
 	// force P symmetry and reinitialize P if necessary
@@ -409,7 +413,7 @@ void BlockLocalPositionEstimator::update()
 		for (int j = 0; j <= i; j++) {
 			if (!PX4_ISFINITE(_P(i, j))) {
 				mavlink_and_console_log_info(&mavlink_log_pub,
-							     "[lpe] reinit P (%d, %d) not finite", i, j);
+							     "%sreinit P (%d, %d) not finite", msg_label, i, j);
 				reinit_P = true;
 			}
 
@@ -417,7 +421,7 @@ void BlockLocalPositionEstimator::update()
 				// make sure diagonal elements are positive
 				if (_P(i, i) <= 0) {
 					mavlink_and_console_log_info(&mavlink_log_pub,
-								     "[lpe] reinit P (%d, %d) negative", i, j);
+								     "%sreinit P (%d, %d) negative", msg_label, i, j);
 					reinit_P = true;
 				}
 
@@ -442,7 +446,7 @@ void BlockLocalPositionEstimator::update()
 
 	// sensor corrections/ initializations
 	if (gpsUpdated) {
-		if (!_gpsInitialized) {
+		if (_sensorTimeout & SENSOR_GPS) {
 			gpsInit();
 
 		} else {
@@ -451,7 +455,7 @@ void BlockLocalPositionEstimator::update()
 	}
 
 	if (baroUpdated) {
-		if (!_baroInitialized) {
+		if (_sensorTimeout & SENSOR_BARO) {
 			baroInit();
 
 		} else {
@@ -460,7 +464,7 @@ void BlockLocalPositionEstimator::update()
 	}
 
 	if (lidarUpdated) {
-		if (!_lidarInitialized) {
+		if (_sensorTimeout & SENSOR_LIDAR) {
 			lidarInit();
 
 		} else {
@@ -469,7 +473,7 @@ void BlockLocalPositionEstimator::update()
 	}
 
 	if (sonarUpdated) {
-		if (!_sonarInitialized) {
+		if (_sensorTimeout & SENSOR_SONAR) {
 			sonarInit();
 
 		} else {
@@ -478,19 +482,16 @@ void BlockLocalPositionEstimator::update()
 	}
 
 	if (flowUpdated) {
-		if (!_flowInitialized) {
+		if (_sensorTimeout & SENSOR_FLOW) {
 			flowInit();
 
 		} else {
-			perf_begin(_loop_perf);// TODO
 			flowCorrect();
-			//perf_count(_interval_perf);
-			perf_end(_loop_perf);
 		}
 	}
 
 	if (visionUpdated) {
-		if (!_visionInitialized) {
+		if (_sensorTimeout & SENSOR_VISION) {
 			visionInit();
 
 		} else {
@@ -499,7 +500,7 @@ void BlockLocalPositionEstimator::update()
 	}
 
 	if (mocapUpdated) {
-		if (!_mocapInitialized) {
+		if (_sensorTimeout & SENSOR_MOCAP) {
 			mocapInit();
 
 		} else {
@@ -508,7 +509,7 @@ void BlockLocalPositionEstimator::update()
 	}
 
 	if (landUpdated) {
-		if (!_landInitialized) {
+		if (_sensorTimeout & SENSOR_LAND) {
 			landInit();
 
 		} else {
@@ -522,7 +523,7 @@ void BlockLocalPositionEstimator::update()
 		publishEstimatorStatus();
 		_pub_innov.update();
 
-		if (_validXY) {
+		if ((_estimatorInitialized & EST_XY)) {
 			publishGlobalPos();
 		}
 	}
@@ -542,155 +543,31 @@ void BlockLocalPositionEstimator::update()
 
 void BlockLocalPositionEstimator::checkTimeouts()
 {
-	if (_timeStamp - _time_last_xy > EST_SRC_TIMEOUT) {
-		if (!_xyTimeout) {
-			_xyTimeout = true;
-			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] xy timeout ");
-		}
-
-	} else if (_xyTimeout) {
-		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] xy resume ");
-		_xyTimeout = false;
-	}
-
-	if (_timeStamp - _time_last_z > EST_SRC_TIMEOUT) {
-		if (!_zTimeout) {
-			_zTimeout = true;
-			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] z timeout ");
-		}
-
-	} else if (_zTimeout) {
-		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] z resume ");
-		_zTimeout = false;
-	}
-
-	if (_timeStamp - _time_last_tz > EST_SRC_TIMEOUT) {
-		if (!_tzTimeout) {
-			_tzTimeout = true;
-			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] tz timeout ");
-		}
-
-	} else if (_tzTimeout) {
-		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] tz resume ");
-		_tzTimeout = false;
-	}
-
-	lidarCheckTimeout();
-	sonarCheckTimeout();
 	baroCheckTimeout();
 	gpsCheckTimeout();
+	lidarCheckTimeout();
 	flowCheckTimeout();
+	sonarCheckTimeout();
 	visionCheckTimeout();
 	mocapCheckTimeout();
+	landCheckTimeout();
 }
 
-float BlockLocalPositionEstimator::agl()
+bool BlockLocalPositionEstimator::landed()
 {
-	return _x(X_tz) - _x(X_z);
+	if (!(_fusion.get() & FUSE_LAND)) {
+		return false;
+	}
+
+	bool disarmed_not_falling = (!_sub_armed.get().armed) && (!_sub_land.get().freefall);
+
+	if (!(_sub_land.get().landed || disarmed_not_falling)) {
+		return false;
+	}
+
+	return true;
 }
 
-void BlockLocalPositionEstimator::correctionLogic(Vector<float, n_x> &dx)
-{
-	// don't correct bias when rotating rapidly
-	float ang_speed = sqrtf(
-				  _sub_att.get().rollspeed * _sub_att.get().rollspeed +
-				  _sub_att.get().pitchspeed * _sub_att.get().pitchspeed +
-				  _sub_att.get().yawspeed * _sub_att.get().yawspeed);
-
-	if (ang_speed > 1) {
-		dx(X_bx) = 0;
-		dx(X_by) = 0;
-		dx(X_bz) = 0;
-	}
-
-	// if xy not valid, stop estimating
-	if (!_validXY) {
-		dx(X_x) = 0;
-		dx(X_y) = 0;
-		dx(X_vx) = 0;
-		dx(X_vy) = 0;
-		dx(X_bx) = 0;
-		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;
-	}
-
-	// saturate bias
-	float bx = dx(X_bx) + _x(X_bx);
-	float by = dx(X_by) + _x(X_by);
-	float bz = dx(X_bz) + _x(X_bz);
-
-	if (std::abs(bx) > BIAS_MAX) {
-		bx = BIAS_MAX * bx / std::abs(bx);
-		dx(X_bx) = bx - _x(X_bx);
-	}
-
-	if (std::abs(by) > BIAS_MAX) {
-		by = BIAS_MAX * by / std::abs(by);
-		dx(X_by) = by - _x(X_by);
-	}
-
-	if (std::abs(bz) > BIAS_MAX) {
-		bz = BIAS_MAX * bz / std::abs(bz);
-		dx(X_bz) = bz - _x(X_bz);
-	}
-}
-
-
-void BlockLocalPositionEstimator::covPropagationLogic(Matrix<float, n_x, n_x> &dP)
-{
-	for (int i = 0; i < n_x; i++) {
-		if (_P(i, i) > P_MAX) {
-			// if diagonal element greater than max, stop propagating
-			dP(i, i) = 0;
-
-			for (int j = 0; j < n_x; j++) {
-				dP(i, j) = 0;
-				dP(j, i) = 0;
-			}
-		}
-	}
-}
-
-void BlockLocalPositionEstimator::detectDistanceSensors()
-{
-	for (int i = 0; i < N_DIST_SUBS; i++) {
-		uORB::Subscription<distance_sensor_s> *s = _dist_subs[i];
-
-		if (s == _sub_lidar || s == _sub_sonar) { continue; }
-
-		if (s->updated()) {
-			s->update();
-
-			if (s->get().timestamp == 0) { continue; }
-
-			if (s->get().type == \
-			    distance_sensor_s::MAV_DISTANCE_SENSOR_LASER &&
-			    _sub_lidar == NULL) {
-				_sub_lidar = s;
-				mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] Lidar detected with ID %i", i);
-
-			} else if (s->get().type == \
-				   distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND &&
-				   _sub_sonar == NULL) {
-				_sub_sonar = s;
-				mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] Sonar detected with ID %i", i);
-			}
-		}
-	}
-}
-
-
 void BlockLocalPositionEstimator::publishLocalPos()
 {
 	const Vector<float, n_x> &xLP = _xLowPass.getState();
@@ -717,14 +594,14 @@ void BlockLocalPositionEstimator::publishLocalPos()
 	    PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
 	    && PX4_ISFINITE(_x(X_vz))) {
 		_pub_lpos.get().timestamp = _timeStamp;
-		_pub_lpos.get().xy_valid = _validXY;
+		_pub_lpos.get().xy_valid = _estimatorInitialized & EST_XY;
-		_pub_lpos.get().z_valid = _validZ;
+		_pub_lpos.get().z_valid = _estimatorInitialized & EST_Z;
-		_pub_lpos.get().v_xy_valid = _validXY;
+		_pub_lpos.get().v_xy_valid = _estimatorInitialized & EST_XY;
-		_pub_lpos.get().v_z_valid = _validZ;
+		_pub_lpos.get().v_z_valid = _estimatorInitialized & EST_Z;
 		_pub_lpos.get().x = xLP(X_x); 	// north
 		_pub_lpos.get().y = xLP(X_y);  	// east
 
-		if (_pub_agl_z.get()) {
+		if (_fusion.get() & FUSE_PUB_AGL_Z) {
 			_pub_lpos.get().z = -_aglLowPass.getState(); // agl
 
 		} else {
@@ -736,8 +613,8 @@ void BlockLocalPositionEstimator::publishLocalPos()
 		_pub_lpos.get().vz = xLP(X_vz); // down
 
 		_pub_lpos.get().yaw = _eul(2);
-		_pub_lpos.get().xy_global = _validXY;
+		_pub_lpos.get().xy_global = _estimatorInitialized & EST_XY;
-		_pub_lpos.get().z_global = _baroInitialized;
+		_pub_lpos.get().z_global = !(_sensorTimeout & SENSOR_BARO);
 		_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;
@@ -749,7 +626,7 @@ void BlockLocalPositionEstimator::publishLocalPos()
 		// if you are in terrain following mode this is important
 		// so that if terrain estimation fails there isn't a
 		// sudden altitude jump
-		_pub_lpos.get().dist_bottom_valid = _validZ;
+		_pub_lpos.get().dist_bottom_valid = _estimatorInitialized & EST_Z;
 		_pub_lpos.get().eph = eph;
 		_pub_lpos.get().epv = epv;
 		_pub_lpos.update();
@@ -767,22 +644,8 @@ void BlockLocalPositionEstimator::publishEstimatorStatus()
 
 	_pub_est_status.get().n_states = n_x;
 	_pub_est_status.get().nan_flags = 0;
-	_pub_est_status.get().health_flags =
+	_pub_est_status.get().health_flags = _sensorFault;
-		((_baroFault > FAULT_NONE) << SENSOR_BARO)
+	_pub_est_status.get().timeout_flags = _sensorTimeout;
-		+ ((_gpsFault > FAULT_NONE) << SENSOR_GPS)
-		+ ((_lidarFault > FAULT_NONE) << SENSOR_LIDAR)
-		+ ((_flowFault > FAULT_NONE) << SENSOR_FLOW)
-		+ ((_sonarFault > FAULT_NONE) << SENSOR_SONAR)
-		+ ((_visionFault > FAULT_NONE) << SENSOR_VISION)
-		+ ((_mocapFault > FAULT_NONE) << SENSOR_MOCAP);
-	_pub_est_status.get().timeout_flags =
-		(_baroInitialized << SENSOR_BARO)
-		+ (_gpsInitialized << SENSOR_GPS)
-		+ (_flowInitialized << SENSOR_FLOW)
-		+ (_lidarInitialized << SENSOR_LIDAR)
-		+ (_sonarInitialized << SENSOR_SONAR)
-		+ (_visionInitialized << SENSOR_VISION)
-		+ (_mocapInitialized << SENSOR_MOCAP);
 	_pub_est_status.get().pos_horiz_accuracy = _pub_gpos.get().eph;
 	_pub_est_status.get().pos_vert_accuracy = _pub_gpos.get().epv;
 
@@ -830,8 +693,8 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 		_pub_gpos.get().eph = eph;
 		_pub_gpos.get().epv = epv;
 		_pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz);
-		_pub_gpos.get().terrain_alt_valid = _validTZ;
+		_pub_gpos.get().terrain_alt_valid = _estimatorInitialized & EST_TZ;
-		_pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout;
+		_pub_gpos.get().dead_reckoning = !(_estimatorInitialized & EST_XY);
 		_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter;
 		_pub_gpos.update();
 	}
@@ -931,22 +794,14 @@ void BlockLocalPositionEstimator::updateSSParams()
 
 void BlockLocalPositionEstimator::predict()
 {
-	// if can't update anything, don't propagate
+	// get acceleration
-	// state or covariance
+	matrix::Quaternion<float> q(&_sub_att.get().q[0]);
-	if (!_validXY && !_validZ) { return; }
+	_eul = matrix::Euler<float>(q);
-
+	_R_att = matrix::Dcm<float>(q);
-	if (integrate) {
+	Vector3f a(_sub_sensor.get().accelerometer_m_s2);
-		matrix::Quaternion<float> q(&_sub_att.get().q[0]);
+	// note, bias is removed in dynamics function
-		_eul = matrix::Euler<float>(q);
+	_u = _R_att * a;
-		_R_att = matrix::Dcm<float>(q);
+	_u(U_az) += 9.81f; // add g
-		Vector3f a(_sub_sensor.get().accelerometer_m_s2);
-		// note, bias is removed in dynamics function
-		_u = _R_att * a;
-		_u(U_az) += 9.81f; // add g
-
-	} else {
-		_u = Vector3f(0, 0, 0);
-	}
 
 	// update state space based on new states
 	updateSSStates();
@@ -963,12 +818,62 @@ void BlockLocalPositionEstimator::predict()
 	k4 = dynamics(h, _x + k3 * h, _u);
 	Vector<float, n_x> dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
 
+	// don't integrate position if no valid xy data
+	if (!(_estimatorInitialized & EST_XY))  {
+		dx(X_x) = 0;
+		dx(X_vx) = 0;
+		dx(X_y) = 0;
+		dx(X_vy) = 0;
+	}
+
+	// don't integrate z if no valid z data
+	if (!(_estimatorInitialized & EST_Z))  {
+		dx(X_z) = 0;
+	}
+
+	// don't integrate tz if no valid tz data
+	if (!(_estimatorInitialized & EST_TZ))  {
+		dx(X_tz) = 0;
+	}
+
+	// saturate bias
+	float bx = dx(X_bx) + _x(X_bx);
+	float by = dx(X_by) + _x(X_by);
+	float bz = dx(X_bz) + _x(X_bz);
+
+	if (std::abs(bx) > BIAS_MAX) {
+		bx = BIAS_MAX * bx / std::abs(bx);
+		dx(X_bx) = bx - _x(X_bx);
+	}
+
+	if (std::abs(by) > BIAS_MAX) {
+		by = BIAS_MAX * by / std::abs(by);
+		dx(X_by) = by - _x(X_by);
+	}
+
+	if (std::abs(bz) > BIAS_MAX) {
+		bz = BIAS_MAX * bz / std::abs(bz);
+		dx(X_bz) = bz - _x(X_bz);
+	}
+
 	// propagate
-	correctionLogic(dx);
 	_x += dx;
 	Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +
 				      _B * _R * _B.transpose() + _Q) * getDt();
-	covPropagationLogic(dP);
+
+	// covariance propagation logic
+	for (int i = 0; i < n_x; i++) {
+		if (_P(i, i) > P_MAX) {
+			// if diagonal element greater than max, stop propagating
+			dP(i, i) = 0;
+
+			for (int j = 0; j < n_x; j++) {
+				dP(i, j) = 0;
+				dP(j, i) = 0;
+			}
+		}
+	}
+
 	_P += dP;
 
 	_xLowPass.update(_x);
@@ -991,7 +896,7 @@ int BlockLocalPositionEstimator::getDelayPeriods(float delay, uint8_t *periods)
 	*periods = i_hist;
 
 	if (t_delay > DELAY_MAX) {
-		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] delayed data too old: %8.4f", double(t_delay));
+		mavlink_and_console_log_info(&mavlink_log_pub, "%sdelayed data old: %8.4f", msg_label, double(t_delay));
 		return -1;
 	}
 

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 ");
 		}