mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-24 07:09:48 +08:00
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
This commit is contained in:
+1
-1
Submodule src/lib/ecl updated: 7eb2b08eed...ec93490890
+1
-1
Submodule src/lib/matrix updated: 0fd99c59f1...25c0455348
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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};
|
||||
|
||||
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user