From ee82e80517ad92d761399486232787ac597124a1 Mon Sep 17 00:00:00 2001
From: ChristophTobler <christoph.tob@gmail.com>
Date: Fri, 22 Sep 2017 08:50:26 +0200
Subject: [PATCH] ekf2: subscribe to multi dist instances and check orientation

---
 src/modules/ekf2/ekf2_main.cpp | 54 +++++++++++++++++++++++++++++-----
 1 file changed, 46 insertions(+), 8 deletions(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 2aec5f1ea9..be684c31a2 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -107,6 +107,7 @@ public:
 	int print_status() override;
 
 private:
+	int getRangeSubIndex(const int *subs);
 	bool 	_replay_mode = false;			///< true when we use replay data from a log
 
 	// time slip monitoring
@@ -437,7 +438,6 @@ void Ekf2::run()
 	int airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	int params_sub = orb_subscribe(ORB_ID(parameter_update));
 	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
-	int range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
 	int ev_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position));
 	int ev_att_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude));
 	int vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
@@ -445,6 +445,14 @@ void Ekf2::run()
 	int sensor_selection_sub = orb_subscribe(ORB_ID(sensor_selection));
 	int sensor_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
 
+	// because we can have several distance sensor instances with different orientations
+	int range_finder_subs[ORB_MULTI_MAX_INSTANCES];
+	int range_finder_sub_index = -1; // index for downward-facing range finder subscription
+
+	for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		range_finder_subs[i] = orb_subscribe_multi(ORB_ID(distance_sensor), i);
+	}
+
 	px4_pollfd_struct_t fds[1] = {};
 	fds[0].fd = sensors_sub;
 	fds[0].events = POLLIN;
@@ -549,15 +557,20 @@ void Ekf2::run()
 			orb_copy(ORB_ID(optical_flow), optical_flow_sub, &optical_flow);
 		}
 
-		orb_check(range_finder_sub, &range_finder_updated);
+		if (range_finder_sub_index >= 0) {
+			orb_check(range_finder_subs[range_finder_sub_index], &range_finder_updated);
 
-		if (range_finder_updated) {
-			orb_copy(ORB_ID(distance_sensor), range_finder_sub, &range_finder);
+			if (range_finder_updated) {
+				orb_copy(ORB_ID(distance_sensor), range_finder_subs[range_finder_sub_index], &range_finder);
 
-			if (range_finder.min_distance >= range_finder.current_distance
-			    || range_finder.max_distance <= range_finder.current_distance) {
-				range_finder_updated = false;
+				if (range_finder.min_distance >= range_finder.current_distance ||
+				    range_finder.max_distance <= range_finder.current_distance) {
+					range_finder_updated = false;
+				}
 			}
+
+		} else {
+			range_finder_sub_index = getRangeSubIndex(range_finder_subs);
 		}
 
 		orb_check(ev_pos_sub, &vision_position_updated);
@@ -1279,13 +1292,38 @@ void Ekf2::run()
 	orb_unsubscribe(airspeed_sub);
 	orb_unsubscribe(params_sub);
 	orb_unsubscribe(optical_flow_sub);
-	orb_unsubscribe(range_finder_sub);
 	orb_unsubscribe(ev_pos_sub);
 	orb_unsubscribe(ev_att_sub);
 	orb_unsubscribe(vehicle_land_detected_sub);
 	orb_unsubscribe(status_sub);
 	orb_unsubscribe(sensor_selection_sub);
 	orb_unsubscribe(sensor_baro_sub);
+
+	for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		orb_unsubscribe(range_finder_subs[i]);
+		range_finder_subs[i] = -1;
+	}
+}
+
+int Ekf2::getRangeSubIndex(const int *subs)
+{
+	for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		bool updated = false;
+		orb_check(subs[i], &updated);
+
+		if (updated) {
+			distance_sensor_s report = {};
+			orb_copy(ORB_ID(distance_sensor), subs[i], &report);
+
+			// only use the first instace which has the correct orientation
+			if (report.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {
+				PX4_INFO("Found range finder with instance %d", i);
+				return i;
+			}
+		}
+	}
+
+	return -1;
 }
 
 Ekf2 *Ekf2::instantiate(int argc, char *argv[])