mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-27 18:27:05 +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
|
// Replace current setpoint lat/lon with tangent coordinate
|
||||||
waypoint_from_heading_and_distance(curr_sp.lat, curr_sp.lon,
|
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);
|
&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
|
// apply the equations of motion of a rigid body and integrate one step
|
||||||
void Sih::equations_of_motion()
|
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
|
// Equations of motion of a rigid body
|
||||||
_p_I_dot = _v_I; // position differential
|
_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 */
|
/* get current rotation matrix from control state quaternions */
|
||||||
Quatf q_att(att.q[0], att.q[1], att.q[2], att.q[3]);
|
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 e_R_vec;
|
||||||
Vector3f torques;
|
Vector3f torques;
|
||||||
|
|||||||
@@ -261,8 +261,7 @@ bool MathlibTest::testQuaternionfrom_dcm()
|
|||||||
matrix::Matrix3f R_orig;
|
matrix::Matrix3f R_orig;
|
||||||
R_orig.identity();
|
R_orig.identity();
|
||||||
|
|
||||||
matrix::Quatf q;
|
matrix::Quatf q(R_orig);
|
||||||
q.from_dcm(R_orig);
|
|
||||||
|
|
||||||
for (unsigned i = 0; i < 4; i++) {
|
for (unsigned i = 0; i < 4; i++) {
|
||||||
ut_assert("matrix::Quatf method 'from_dcm()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol);
|
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;
|
matrix::Matrix3f R_orig;
|
||||||
R_orig.identity();
|
R_orig.identity();
|
||||||
|
|
||||||
matrix::Quatf q;
|
matrix::Quatf q(R_orig);
|
||||||
q.from_dcm(R_orig);
|
|
||||||
|
|
||||||
q_true = matrix::Eulerf(0.3f, 0.2f, 0.1f);
|
q_true = matrix::Eulerf(0.3f, 0.2f, 0.1f);
|
||||||
q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
|
q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
|
||||||
|
|||||||
@@ -305,21 +305,21 @@ bool MatrixTest::attitudeTests()
|
|||||||
|
|
||||||
// get rotation axis from quaternion (nonzero rotation)
|
// get rotation axis from quaternion (nonzero rotation)
|
||||||
q = Quatf(cosf(1.0f / 2), 0.0f, sinf(1.0f / 2), 0.0f);
|
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(0)) < eps);
|
||||||
ut_test(fabs(rot(1) - 1.0f) < eps);
|
ut_test(fabs(rot(1) - 1.0f) < eps);
|
||||||
ut_test(fabs(rot(2)) < eps);
|
ut_test(fabs(rot(2)) < eps);
|
||||||
|
|
||||||
// get rotation axis from quaternion (zero rotation)
|
// get rotation axis from quaternion (zero rotation)
|
||||||
q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
|
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(0)) < eps);
|
||||||
ut_test(fabs(rot(1)) < eps);
|
ut_test(fabs(rot(1)) < eps);
|
||||||
ut_test(fabs(rot(2)) < eps);
|
ut_test(fabs(rot(2)) < eps);
|
||||||
|
|
||||||
// from axis angle (zero rotation)
|
// from axis angle (zero rotation)
|
||||||
rot(0) = rot(1) = rot(2) = 0.0f;
|
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);
|
q_true = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
|
||||||
ut_test(fabs(q(0) - q_true(0)) < eps);
|
ut_test(fabs(q(0) - q_true(0)) < eps);
|
||||||
ut_test(fabs(q(1) - q_true(1)) < eps);
|
ut_test(fabs(q(1) - q_true(1)) < eps);
|
||||||
|
|||||||
Reference in New Issue
Block a user