Replaced wigen with custom matrix lib.

.gitmodules

@@ -13,9 +13,6 @@
 [submodule "Tools/gencpp"]
 	path = Tools/gencpp
 	url = https://github.com/ros/gencpp.git
-[submodule "src/lib/eigen"]
-	path = src/lib/eigen
-	url = https://github.com/PX4/eigen.git
 [submodule "src/lib/dspal"]
 	path = src/lib/dspal
 	url = https://github.com/mcharleb/dspal.git

src/lib/mathlib/math/Matrix.hpp

@@ -49,8 +49,7 @@
 #ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
 #else
-#include <platforms/ros/eigen_math.h>
+#include "modules/local_position_estimator/matrix/src/Matrix.hpp"
-#include <Eigen/Eigen>
 #endif
 #include <platforms/px4_defines.h>
 
@@ -308,11 +307,9 @@ public:
 		arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
 		return res;
 #else
-		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+		matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
-				(this->arm_mat.pData);
+		matrix::Matrix<float, N, P> Him(m.arm_mat.pData);
-		Eigen::Matrix<float, N, P, Eigen::RowMajor> Him = Eigen::Map<Eigen::Matrix<float, N, P, Eigen::RowMajor> >
+		matrix::Matrix<float, M, P> Product = Me * Him;
-				(m.arm_mat.pData);
-		Eigen::Matrix<float, M, P, Eigen::RowMajor> Product = Me * Him;
 		Matrix<M, P> res(Product.data());
 		return res;
 #endif
@@ -327,10 +324,8 @@ public:
 		arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
 		return res;
 #else
-		Eigen::Matrix<float, N, M, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, N, M, Eigen::RowMajor> >
+		matrix::Matrix<float, N, M> Me(this->arm_mat.pData);
-				(this->arm_mat.pData);
+		Matrix<N, M> res(Me.transpose().data());
-		Me.transposeInPlace();
-		Matrix<N, M> res(Me.data());
 		return res;
 #endif
 	}
@@ -344,9 +339,8 @@ public:
 		arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
 		return res;
 #else
-		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+		matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
-				(this->arm_mat.pData);
+		matrix::Matrix<float, M, N> MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea
-		Eigen::Matrix<float, M, N, Eigen::RowMajor> MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea
 		Matrix<M, N> res(MyInverse.data());
 		return res;
 #endif
@@ -413,10 +407,9 @@ public:
 		arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
 #else
 		//probably nicer if this could go into a function like "eigen_mat_mult" or so
-		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+		matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
-				(this->arm_mat.pData);
+		matrix::Matrix<float, N, 1> Vec(v.arm_col.pData);
-		Eigen::VectorXf Vec = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData, N);
+		matrix::Matrix<float, M, 1> Product = Me * Vec;
-		Eigen::VectorXf Product = Me * Vec;
 		Vector<M> res(Product.data());
 #endif
 		return res;

src/modules/commander/commander_helper.cpp

@@ -387,7 +387,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float
 	// XXX this time constant needs to become tunable
 	// but really, the right fix are smart batteries.
 	float val = throttle_lowpassed * 0.97f + throttle_normalized * 0.03f;
-	if (isfinite(val)) {
+	if (PX4_ISFINITE(val)) {
 		throttle_lowpassed = val;
 	}
 

src/modules/controllib/blocks.cpp

@@ -123,7 +123,7 @@ int blockLimitSymTest()
 
 float BlockLowPass::update(float input)
 {
-	if (!isfinite(getState())) {
+	if (!PX4_ISFINITE(getState())) {
 		setState(input);
 	}
 
@@ -203,7 +203,7 @@ int blockHighPassTest()
 
 float BlockLowPass2::update(float input)
 {
-	if (!isfinite(getState())) {
+	if (!PX4_ISFINITE(getState())) {
 		setState(input);
 	}
 

src/modules/fw_pos_control_l1/mtecs/mTecs.cpp

@@ -88,8 +88,8 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
 		float airspeedSp, unsigned mode, LimitOverride limitOverride)
 {
 	/* check if all input arguments are numbers and abort if not so */
-	if (!isfinite(flightPathAngle) || !isfinite(altitude) ||
+	if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(altitude) ||
-			!isfinite(altitudeSp) || !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) {
+			!PX4_ISFINITE(altitudeSp) || !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) {
 		return -1;
 	}
 
@@ -125,8 +125,8 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 		float airspeedSp, unsigned mode, LimitOverride limitOverride)
 {
 	/* check if all input arguments are numbers and abort if not so */
-	if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
+	if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) ||
-			!isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) {
+			!PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) {
 		return -1;
 	}
 
@@ -163,8 +163,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 		float accelerationLongitudinalSp, unsigned mode, LimitOverride limitOverride)
 {
 	/* check if all input arguments are numbers and abort if not so */
-	if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
+	if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) ||
-			!isfinite(airspeedFiltered) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) {
+			!PX4_ISFINITE(airspeedFiltered) || !PX4_ISFINITE(accelerationLongitudinalSp) || !PX4_ISFINITE(mode)) {
 		return -1;
 	}
 	/* time measurement */

src/modules/local_position_estimator/matrix/src/Matrix.hpp

@@ -58,6 +58,10 @@ public:
 	 * Accessors/ Assignment etc.
 	 */
 
+	T * data() {
+		return _data[0];
+	}
+
 	inline size_t rows() const
 	{
 		return _rows;

src/modules/navigator/mission_block.cpp

@@ -153,7 +153,7 @@ MissionBlock::is_mission_item_reached()
 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 
 		/* TODO: removed takeoff, why? */
-		if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
+		if (_navigator->get_vstatus()->is_rotary_wing && PX4_ISFINITE(_mission_item.yaw)) {
 
 			/* check yaw if defined only for rotary wing except takeoff */
 			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);

src/platforms/px4_defines.h

@@ -216,6 +216,7 @@ __END_DECLS
 #ifndef __PX4_QURT
 
 #if defined(__cplusplus)
+#include <cmath>
 #define PX4_ISFINITE(x) std::isfinite(x)
 #else
 #define PX4_ISFINITE(x) isfinite(x)