From ec1c37e1af507bcf3df356027356b3c09c7e427d Mon Sep 17 00:00:00 2001
From: TSC21 <n.marques21@hotmail.com>
Date: Sun, 14 Jan 2018 15:18:56 -0500
Subject: [PATCH] modules: lpe: push sensor status update to object; prioritize
 lidar readings over sonar

---
 .../BlockLocalPositionEstimator.cpp           | 47 +++++++++++--------
 .../BlockLocalPositionEstimator.hpp           | 10 ++++
 .../sensors/sonar.cpp                         |  6 ++-
 3 files changed, 42 insertions(+), 21 deletions(-)

diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 26f2da396e..091f62c424 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -149,7 +149,17 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// masks
 	_sensorTimeout(UINT16_MAX),
 	_sensorFault(0),
-	_estimatorInitialized(0)
+	_estimatorInitialized(0),
+
+	// sensor update flags
+	_flowUpdated(false),
+	_gpsUpdated(false),
+	_visionUpdated(false),
+	_mocapUpdated(false),
+	_lidarUpdated(false),
+	_sonarUpdated(false),
+	_landUpdated(false),
+	_baroUpdated(false)
 {
 	// assign distance subs to array
 	_dist_subs[0] = &_sub_dist0;
@@ -280,7 +290,7 @@ void BlockLocalPositionEstimator::update()
 
 	// see which updates are available
 	bool paramsUpdated = _sub_param_update.updated();
-	bool baroUpdated = false;
+	_baroUpdated = false;
 
 	if ((_fusion.get() & FUSE_BARO) && _sub_sensor.updated()) {
 		int32_t baro_timestamp_relative = _sub_sensor.get().baro_timestamp_relative;
@@ -290,20 +300,19 @@ void BlockLocalPositionEstimator::update()
 						  _sub_sensor.get().baro_timestamp_relative;
 
 			if (baro_timestamp != _timeStampLastBaro) {
-				baroUpdated = true;
+				_baroUpdated = true;
 				_timeStampLastBaro = baro_timestamp;
 			}
 		}
 	}
 
-	bool flowUpdated = (_fusion.get() & FUSE_FLOW) && _sub_flow.updated();
-	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 != nullptr) && _sub_lidar->updated();
-	bool sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated();
-	bool landUpdated = landed()
-			   && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE);		// throttle rate
+	_flowUpdated = (_fusion.get() & FUSE_FLOW) && _sub_flow.updated();
+	_gpsUpdated = (_fusion.get() & FUSE_GPS) && _sub_gps.updated();
+	_visionUpdated = (_fusion.get() & FUSE_VIS_POS) && _sub_vision_pos.updated();
+	_mocapUpdated = _sub_mocap.updated();
+	_lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->updated();
+	_sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated();
+	_landUpdated = landed() && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE);// throttle rate
 
 	// get new data
 	updateSubscriptions();
@@ -447,7 +456,7 @@ void BlockLocalPositionEstimator::update()
 	predict();
 
 	// sensor corrections/ initializations
-	if (gpsUpdated) {
+	if (_gpsUpdated) {
 		if (_sensorTimeout & SENSOR_GPS) {
 			gpsInit();
 
@@ -456,7 +465,7 @@ void BlockLocalPositionEstimator::update()
 		}
 	}
 
-	if (baroUpdated) {
+	if (_baroUpdated) {
 		if (_sensorTimeout & SENSOR_BARO) {
 			baroInit();
 
@@ -465,7 +474,7 @@ void BlockLocalPositionEstimator::update()
 		}
 	}
 
-	if (lidarUpdated) {
+	if (_lidarUpdated) {
 		if (_sensorTimeout & SENSOR_LIDAR) {
 			lidarInit();
 
@@ -474,7 +483,7 @@ void BlockLocalPositionEstimator::update()
 		}
 	}
 
-	if (sonarUpdated) {
+	if (_sonarUpdated) {
 		if (_sensorTimeout & SENSOR_SONAR) {
 			sonarInit();
 
@@ -483,7 +492,7 @@ void BlockLocalPositionEstimator::update()
 		}
 	}
 
-	if (flowUpdated) {
+	if (_flowUpdated) {
 		if (_sensorTimeout & SENSOR_FLOW) {
 			flowInit();
 
@@ -492,7 +501,7 @@ void BlockLocalPositionEstimator::update()
 		}
 	}
 
-	if (visionUpdated) {
+	if (_visionUpdated) {
 		if (_sensorTimeout & SENSOR_VISION) {
 			visionInit();
 
@@ -501,7 +510,7 @@ void BlockLocalPositionEstimator::update()
 		}
 	}
 
-	if (mocapUpdated) {
+	if (_mocapUpdated) {
 		if (_sensorTimeout & SENSOR_MOCAP) {
 			mocapInit();
 
@@ -510,7 +519,7 @@ void BlockLocalPositionEstimator::update()
 		}
 	}
 
-	if (landUpdated) {
+	if (_landUpdated) {
 		if (_sensorTimeout & SENSOR_LAND) {
 			landInit();
 
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index d10e56172a..1699a1fd1c 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -377,6 +377,16 @@ private:
 	uint16_t _sensorFault;
 	uint8_t _estimatorInitialized;
 
+	// sensor update flags
+	bool _flowUpdated;
+	bool _gpsUpdated;
+	bool _visionUpdated;
+	bool _mocapUpdated;
+	bool _lidarUpdated;
+	bool _sonarUpdated;
+	bool _landUpdated;
+	bool _baroUpdated;
+
 	// state space
 	Vector<float, n_x>  _x;	// state vector
 	Vector<float, n_u>  _u;	// input vector
diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp
index 337e35b27a..04bd043611 100644
--- a/src/modules/local_position_estimator/sensors/sonar.cpp
+++ b/src/modules/local_position_estimator/sensors/sonar.cpp
@@ -79,8 +79,10 @@ void BlockLocalPositionEstimator::sonarCorrect()
 
 	if (sonarMeasure(y) != OK) { return; }
 
-	// do not use sonar if lidar is active
-	//if (_lidarInitialized && (_lidarFault < fault_lvl_disable)) { return; }
+	// do not use sonar if lidar is active and not faulty or timed out
+	if (_lidarUpdated
+	    && !(_sensorFault & SENSOR_LIDAR)
+	    && !(_sensorTimeout & SENSOR_LIDAR)) { return; }
 
 	// calculate covariance
 	float cov = _sub_sonar->get().covariance;