From 6cce823dc6fd0c2f6477c871e1e57dd37098645a Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sat, 24 Oct 2015 15:06:59 -0400 Subject: [PATCH 1/4] Replaced wigen with custom matrix lib. --- .gitmodules | 3 -- src/lib/mathlib/math/Matrix.hpp | 29 +++++++------------ src/modules/commander/commander_helper.cpp | 2 +- src/modules/controllib/blocks.cpp | 4 +-- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 12 ++++---- .../matrix/src/Matrix.hpp | 4 +++ src/modules/navigator/mission_block.cpp | 2 +- src/platforms/px4_defines.h | 1 + 8 files changed, 26 insertions(+), 31 deletions(-) diff --git a/.gitmodules b/.gitmodules index b05206ecfa..50f26b3a99 100644 --- a/.gitmodules +++ b/.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 diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index b81f8868ec..40c32ca348 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -49,8 +49,7 @@ #ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" #else -#include -#include +#include "modules/local_position_estimator/matrix/src/Matrix.hpp" #endif #include @@ -308,11 +307,9 @@ public: arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); return res; #else - Eigen::Matrix Me = Eigen::Map > - (this->arm_mat.pData); - Eigen::Matrix Him = Eigen::Map > - (m.arm_mat.pData); - Eigen::Matrix Product = Me * Him; + matrix::Matrix Me(this->arm_mat.pData); + matrix::Matrix Him(m.arm_mat.pData); + matrix::Matrix Product = Me * Him; Matrix 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 Me = Eigen::Map > - (this->arm_mat.pData); - Me.transposeInPlace(); - Matrix res(Me.data()); + matrix::Matrix Me(this->arm_mat.pData); + Matrix res(Me.transpose().data()); return res; #endif } @@ -344,9 +339,8 @@ public: arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); return res; #else - Eigen::Matrix Me = Eigen::Map > - (this->arm_mat.pData); - Eigen::Matrix MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea + matrix::Matrix Me(this->arm_mat.pData); + matrix::Matrix MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea Matrix 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 Me = Eigen::Map > - (this->arm_mat.pData); - Eigen::VectorXf Vec = Eigen::Map(v.arm_col.pData, N); - Eigen::VectorXf Product = Me * Vec; + matrix::Matrix Me(this->arm_mat.pData); + matrix::Matrix Vec(v.arm_col.pData); + matrix::Matrix Product = Me * Vec; Vector res(Product.data()); #endif return res; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 0ffaea5bec..f411e1c128 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/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; } diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index 04b96753ef..3915ecc5e1 100644 --- a/src/modules/controllib/blocks.cpp +++ b/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); } diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 6de6d9d8f0..7a045fb1bb 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/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) || - !isfinite(altitudeSp) || !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) { + if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(altitude) || + !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) || - !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) { + if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) || + !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) || - !isfinite(airspeedFiltered) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) { + if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) || + !PX4_ISFINITE(airspeedFiltered) || !PX4_ISFINITE(accelerationLongitudinalSp) || !PX4_ISFINITE(mode)) { return -1; } /* time measurement */ diff --git a/src/modules/local_position_estimator/matrix/src/Matrix.hpp b/src/modules/local_position_estimator/matrix/src/Matrix.hpp index 8a0432b7ef..222c5d1872 100644 --- a/src/modules/local_position_estimator/matrix/src/Matrix.hpp +++ b/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; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 2ce37ef94d..94ded8a4b9 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/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); diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 06bddf2eb6..4d453e5983 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -216,6 +216,7 @@ __END_DECLS #ifndef __PX4_QURT #if defined(__cplusplus) +#include #define PX4_ISFINITE(x) std::isfinite(x) #else #define PX4_ISFINITE(x) isfinite(x) From 0acf6db64f0d6a79998879517519df628e388023 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sat, 24 Oct 2015 15:15:33 -0400 Subject: [PATCH 2/4] Removed more eigen references. --- CMakeLists.txt | 3 +-- src/lib/eigen | 1 - src/platforms/common/CMakeLists.txt | 4 ---- 3 files changed, 1 insertion(+), 7 deletions(-) delete mode 160000 src/lib/eigen diff --git a/CMakeLists.txt b/CMakeLists.txt index 9486bf13fb..fb13d8283a 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/src/lib/eigen b/src/lib/eigen deleted file mode 160000 index eb7213abbf..0000000000 --- a/src/lib/eigen +++ /dev/null @@ -1 +0,0 @@ -Subproject commit eb7213abbf38a987a18ed5071172d9d26cc38306 diff --git a/src/platforms/common/CMakeLists.txt b/src/platforms/common/CMakeLists.txt index 7b50768ac1..09defae9b4 100644 --- a/src/platforms/common/CMakeLists.txt +++ b/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 From 65a0de38bb168eb70c4c98bf4949a3d97b3400e0 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sat, 24 Oct 2015 15:51:32 -0400 Subject: [PATCH 3/4] Format fix. --- src/modules/local_position_estimator/matrix/src/Matrix.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/local_position_estimator/matrix/src/Matrix.hpp b/src/modules/local_position_estimator/matrix/src/Matrix.hpp index 222c5d1872..ae1111894c 100644 --- a/src/modules/local_position_estimator/matrix/src/Matrix.hpp +++ b/src/modules/local_position_estimator/matrix/src/Matrix.hpp @@ -58,7 +58,8 @@ public: * Accessors/ Assignment etc. */ - T * data() { + T *data() + { return _data[0]; } From d198c6a324967fa27cc59e9e18c4a66bcee4b953 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sat, 24 Oct 2015 15:59:11 -0400 Subject: [PATCH 4/4] Removed comment. --- src/lib/mathlib/math/Matrix.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 40c32ca348..38322e56a7 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -406,7 +406,6 @@ public: Vector 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 Me(this->arm_mat.pData); matrix::Matrix Vec(v.arm_col.pData); matrix::Matrix Product = Me * Vec;