Browse Source

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
sbg
kritz 5 years ago committed by GitHub
parent
commit
9d962cdfbc
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
  1. 2
      src/lib/ecl
  2. 2
      src/lib/matrix
  3. 2
      src/modules/navigator/mission_block.cpp
  4. 2
      src/modules/sih/sih.cpp
  5. 2
      src/modules/uuv_att_control/uuv_att_control.cpp
  6. 6
      src/systemcmds/tests/test_mathlib.cpp
  7. 6
      src/systemcmds/tests/test_matrix.cpp

2
src/lib/ecl

@ -1 +1 @@ @@ -1 +1 @@
Subproject commit 7eb2b08eedb70817779bc7968cf34a4cf6f01fa0
Subproject commit ec934908900b23ee273d1a9f82364b7b38423200

2
src/lib/matrix

@ -1 +1 @@ @@ -1 +1 @@
Subproject commit 0fd99c59f1740a7aa2ead03168705b4211bf29e8
Subproject commit 25c04553488bb86470592a1cdccac56979bf59bb

2
src/modules/navigator/mission_block.cpp

@ -440,7 +440,7 @@ MissionBlock::is_mission_item_reached() @@ -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);
}

2
src/modules/sih/sih.cpp

@ -268,7 +268,7 @@ void Sih::generate_force_and_torques() @@ -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<float>(_q); // body to inertial transformation
// Equations of motion of a rigid body
_p_I_dot = _v_I; // position differential

2
src/modules/uuv_att_control/uuv_att_control.cpp

@ -200,7 +200,7 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &att, con @@ -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<float>(q_att);
Vector3f e_R_vec;
Vector3f torques;

6
src/systemcmds/tests/test_mathlib.cpp

@ -261,8 +261,7 @@ bool MathlibTest::testQuaternionfrom_dcm() @@ -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() @@ -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};

6
src/systemcmds/tests/test_matrix.cpp

@ -305,21 +305,21 @@ bool MatrixTest::attitudeTests() @@ -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<float>(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<float>(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<float>(matrix::AxisAngle<float>(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);

Loading…
Cancel
Save