From 2816dba447cadd6a73e8664928d6c919d4e292ab Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 3 Feb 2015 13:48:01 +0100
Subject: [PATCH] EKF estimator: Support multi-sensor setups

---
 .../ekf_att_pos_estimator_main.cpp            | 60 ++++++++++++-------
 .../estimator_22states.cpp                    |  5 ++
 2 files changed, 42 insertions(+), 23 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 38660b4332..0b9a5f977a 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -209,9 +209,13 @@ private:
 	struct wind_estimate_s				_wind;			/**< wind estimate */
 	struct range_finder_report			_distance;		/**< distance estimate */
 
-	struct gyro_scale				_gyro_offsets;
-	struct accel_scale				_accel_offsets;
-	struct mag_scale				_mag_offsets;
+	struct gyro_scale				_gyro_offsets[3];
+	struct accel_scale				_accel_offsets[3];
+	struct mag_scale				_mag_offsets[3];
+
+	struct gyro_scale				_gyro1_offsets;
+	struct accel_scale				_accel1_offsets;
+	struct mag_scale				_mag1_offsets;
 
 #ifdef SENSOR_COMBINED_SUB
 	struct sensor_combined_s			_sensor_combined;
@@ -386,6 +390,10 @@ FixedwingEstimator::FixedwingEstimator() :
 	_accel_offsets({}),
 	_mag_offsets({}),
 
+	_gyro1_offsets({}),
+	_accel1_offsets({}),
+	_mag1_offsets({}),
+
 	#ifdef SENSOR_COMBINED_SUB
 	_sensor_combined{},
 	#endif
@@ -452,36 +460,42 @@ FixedwingEstimator::FixedwingEstimator() :
 
 	int fd, res;
 
-	fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+	for (unsigned s = 0; s < 3; s++) {
+		char str[30];
+		(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
+		fd = open(str, O_RDONLY);
 
-	if (fd > 0) {
-		res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets);
-		close(fd);
+		if (fd >= 0) {
+			res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets[s]);
+			close(fd);
 
-		if (res) {
-			warnx("G SCALE FAIL");
+			if (res) {
+				warnx("G%u SCALE FAIL", s);
+			}
 		}
-	}
 
-	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+		(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
+		fd = open(str, O_RDONLY);
 
-	if (fd > 0) {
-		res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets);
-		close(fd);
+		if (fd >= 0) {
+			res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets[s]);
+			close(fd);
 
-		if (res) {
-			warnx("A SCALE FAIL");
+			if (res) {
+				warnx("A%u SCALE FAIL", s);
+			}
 		}
-	}
 
-	fd = open(MAG_DEVICE_PATH, O_RDONLY);
+		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
+		fd = open(str, O_RDONLY);
 
-	if (fd > 0) {
-		res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets);
-		close(fd);
+		if (fd >= 0) {
+			res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets[s]);
+			close(fd);
 
-		if (res) {
-			warnx("M SCALE FAIL");
+			if (res) {
+				warnx("M%u SCALE FAIL", s);
+			}
 		}
 	}
 }
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
index 15d018ab62..1cc883d90d 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
@@ -2355,6 +2355,11 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
         }
     }
 
+    float q[4];
+    float eul[3];
+
+    eul2quat(q, eul);
+
     return ret;
 }