Merge pull request #3065 from dronecrew/lpe-no-eigen

Replaced eigen 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

CMakeLists.txt

@@ -225,7 +225,6 @@ px4_add_git_submodule(TARGET git_gencpp PATH "Tools/gencpp")
 px4_add_git_submodule(TARGET git_mavlink PATH "mavlink/include/mavlink/v1.0")
 px4_add_git_submodule(TARGET git_gtest PATH "unittets/gtest")
 px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan")
-px4_add_git_submodule(TARGET git_eigen PATH "src/lib/eigen")
 px4_add_git_submodule(TARGET git_nuttx PATH "NuttX")
 px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal")
 px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim")
@@ -288,7 +287,7 @@ px4_generate_messages(TARGET msg_gen
 px4_generate_parameters_xml(OUT parameters.xml BOARD ${BOARD})
 px4_generate_airframes_xml(OUT airframes.xml BOARD ${BOARD})
 add_custom_target(xml_gen
-	DEPENDS git_eigen parameters.xml airframes.xml)
+	DEPENDS parameters.xml airframes.xml)
 
 #=============================================================================
 # external projects

src/lib/eigen

@@ -1 +0,0 @@
-Subproject commit eb7213abbf38a987a18ed5071172d9d26cc38306

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
@@ -412,11 +406,9 @@ public:
 		Vector<M> res;
 		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
+		matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
-		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+		matrix::Matrix<float, N, 1> Vec(v.arm_col.pData);
-				(this->arm_mat.pData);
+		matrix::Matrix<float, M, 1> Product = Me * Vec;
-		Eigen::VectorXf Vec = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData, N);
-		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,11 @@ 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/common/CMakeLists.txt

@@ -37,10 +37,6 @@ set(depends
 	git_uavcan
 	)
 
-if(${OS} STREQUAL "posix" OR ${BOARD} STREQUAL "sim")
-	list(APPEND depends git_eigen)
-endif()
-
 px4_add_module(
 	MODULE platforms__common
 	SRCS

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)