From 9d962cdfbc30efa6ead0e3f56b9680bbe37380a6 Mon Sep 17 00:00:00 2001 From: kritz Date: Tue, 11 Aug 2020 10:08:41 +0200 Subject: [PATCH] Pr update matrix (#15520) * Update submodule Matrix * replace deprecated matrix functions * update submodule ECL * Update Matrix submodule * Use absolute value of loiter radius * Update ECL submodule --- src/lib/ecl | 2 +- src/lib/matrix | 2 +- src/modules/navigator/mission_block.cpp | 2 +- src/modules/sih/sih.cpp | 2 +- src/modules/uuv_att_control/uuv_att_control.cpp | 2 +- src/systemcmds/tests/test_mathlib.cpp | 6 ++---- src/systemcmds/tests/test_matrix.cpp | 6 +++--- 7 files changed, 10 insertions(+), 12 deletions(-) diff --git a/src/lib/ecl b/src/lib/ecl index 7eb2b08eed..ec93490890 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 7eb2b08eedb70817779bc7968cf34a4cf6f01fa0 +Subproject commit ec934908900b23ee273d1a9f82364b7b38423200 diff --git a/src/lib/matrix b/src/lib/matrix index 0fd99c59f1..25c0455348 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 0fd99c59f1740a7aa2ead03168705b4211bf29e8 +Subproject commit 25c04553488bb86470592a1cdccac56979bf59bb diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index edd2dd4c7c..b02ab050b5 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -440,7 +440,7 @@ MissionBlock::is_mission_item_reached() // Replace current setpoint lat/lon with tangent coordinate waypoint_from_heading_and_distance(curr_sp.lat, curr_sp.lon, - bearing, curr_sp.loiter_radius, + bearing, fabsf(curr_sp.loiter_radius), &curr_sp.lat, &curr_sp.lon); } diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index e08dc0aeef..16e257f501 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -268,7 +268,7 @@ void Sih::generate_force_and_torques() // apply the equations of motion of a rigid body and integrate one step void Sih::equations_of_motion() { - _C_IB = _q.to_dcm(); // body to inertial transformation + _C_IB = matrix::Dcm(_q); // body to inertial transformation // Equations of motion of a rigid body _p_I_dot = _v_I; // position differential diff --git a/src/modules/uuv_att_control/uuv_att_control.cpp b/src/modules/uuv_att_control/uuv_att_control.cpp index bf3e12a5fd..41c5e272e0 100644 --- a/src/modules/uuv_att_control/uuv_att_control.cpp +++ b/src/modules/uuv_att_control/uuv_att_control.cpp @@ -200,7 +200,7 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &att, con /* get current rotation matrix from control state quaternions */ Quatf q_att(att.q[0], att.q[1], att.q[2], att.q[3]); - Matrix3f _rot_att = q_att.to_dcm(); + Matrix3f _rot_att = matrix::Dcm(q_att); Vector3f e_R_vec; Vector3f torques; diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 6dd5b5841c..de48b49b10 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -261,8 +261,7 @@ bool MathlibTest::testQuaternionfrom_dcm() matrix::Matrix3f R_orig; R_orig.identity(); - matrix::Quatf q; - q.from_dcm(R_orig); + matrix::Quatf q(R_orig); for (unsigned i = 0; i < 4; i++) { ut_assert("matrix::Quatf method 'from_dcm()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol); @@ -279,8 +278,7 @@ bool MathlibTest::testQuaternionfrom_euler() matrix::Matrix3f R_orig; R_orig.identity(); - matrix::Quatf q; - q.from_dcm(R_orig); + matrix::Quatf q(R_orig); q_true = matrix::Eulerf(0.3f, 0.2f, 0.1f); q = {0.9833f, 0.1436f, 0.1060f, 0.0343f}; diff --git a/src/systemcmds/tests/test_matrix.cpp b/src/systemcmds/tests/test_matrix.cpp index ab37da8e1b..b3b8021274 100644 --- a/src/systemcmds/tests/test_matrix.cpp +++ b/src/systemcmds/tests/test_matrix.cpp @@ -305,21 +305,21 @@ bool MatrixTest::attitudeTests() // get rotation axis from quaternion (nonzero rotation) q = Quatf(cosf(1.0f / 2), 0.0f, sinf(1.0f / 2), 0.0f); - rot = q.to_axis_angle(); + rot = matrix::AxisAngle(q); ut_test(fabs(rot(0)) < eps); ut_test(fabs(rot(1) - 1.0f) < eps); ut_test(fabs(rot(2)) < eps); // get rotation axis from quaternion (zero rotation) q = Quatf(1.0f, 0.0f, 0.0f, 0.0f); - rot = q.to_axis_angle(); + rot = matrix::AxisAngle(q); ut_test(fabs(rot(0)) < eps); ut_test(fabs(rot(1)) < eps); ut_test(fabs(rot(2)) < eps); // from axis angle (zero rotation) rot(0) = rot(1) = rot(2) = 0.0f; - q.from_axis_angle(rot, 0.0f); + q = Quaternion(matrix::AxisAngle(rot)); q_true = Quatf(1.0f, 0.0f, 0.0f, 0.0f); ut_test(fabs(q(0) - q_true(0)) < eps); ut_test(fabs(q(1) - q_true(1)) < eps);