diff --git a/src/lib/matrix/matrix/Quaternion.hpp b/src/lib/matrix/matrix/Quaternion.hpp index dd8d7b5cd9..5b91dde0a4 100644 --- a/src/lib/matrix/matrix/Quaternion.hpp +++ b/src/lib/matrix/matrix/Quaternion.hpp @@ -50,7 +50,7 @@ class AxisAngle; * described by this class. */ template -class Quaternion : public Vector +class Quaternion : public Vector4 { public: using Matrix41 = Matrix; @@ -62,7 +62,7 @@ public: * @param data_ array */ explicit Quaternion(const Type data_[4]) : - Vector(data_) + Vector4(data_) { } @@ -84,7 +84,7 @@ public: * @param other Matrix41 to copy */ Quaternion(const Matrix41 &other) : - Vector(other) + Vector4(other) { } diff --git a/src/lib/matrix/test/MatrixAttitudeTest.cpp b/src/lib/matrix/test/MatrixAttitudeTest.cpp index 3ea81ebf3a..62df99823c 100644 --- a/src/lib/matrix/test/MatrixAttitudeTest.cpp +++ b/src/lib/matrix/test/MatrixAttitudeTest.cpp @@ -260,7 +260,7 @@ TEST(MatrixAttitudeTest, Attitude) // quaterion copy ctors float data_v4[] = {1, 2, 3, 4}; - Vector v4(data_v4); + Vector4f v4(data_v4); Quatf q_from_v(v4); EXPECT_EQ(q_from_v, v4); @@ -270,15 +270,13 @@ TEST(MatrixAttitudeTest, Attitude) // quaternion derivative in frame 1 Quatf q1(0, 1, 0, 0); - Vector q1_dot1 = q1.derivative1(Vector3f(1, 2, 3)); - float data_q_dot1_check[] = { -0.5f, 0.0f, -1.5f, 1.0f}; - Vector q1_dot1_check(data_q_dot1_check); + Vector4f q1_dot1 = q1.derivative1(Vector3f(1, 2, 3)); + Vector4f q1_dot1_check(-0.5f, 0.0f, -1.5f, 1.0f); EXPECT_EQ(q1_dot1, q1_dot1_check); // quaternion derivative in frame 2 - Vector q1_dot2 = q1.derivative2(Vector3f(1, 2, 3)); - float data_q_dot2_check[] = { -0.5f, 0.0f, 1.5f, -1.0f}; - Vector q1_dot2_check(data_q_dot2_check); + Vector4f q1_dot2 = q1.derivative2(Vector3f(1, 2, 3)); + Vector4f q1_dot2_check(-0.5f, 0.0f, 1.5f, -1.0f); EXPECT_EQ(q1_dot2, q1_dot2_check); // quaternion product diff --git a/src/lib/matrix/test/MatrixLeastSquaresTest.cpp b/src/lib/matrix/test/MatrixLeastSquaresTest.cpp index b3742271ef..678296e45c 100644 --- a/src/lib/matrix/test/MatrixLeastSquaresTest.cpp +++ b/src/lib/matrix/test/MatrixLeastSquaresTest.cpp @@ -50,19 +50,12 @@ TEST(MatrixLeastSquaresTest, 4x3) -1.f, -1.1f, -1.2f }; Matrix A(data); - - float b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f}; - Vector b(b_data); - - float x_check_data[3] = {-0.69168233f, - -0.26227593f, - -1.03767522f - }; - Vector x_check(x_check_data); + Vector4f b(2.0f, 3.0f, 4.0f, 5.0f); + Vector3f x_check(-0.69168233f, -0.26227593f, -1.03767522f); LeastSquaresSolver qrd = LeastSquaresSolver(A); - Vector x = qrd.solve(b); + Vector3f x = qrd.solve(b); EXPECT_EQ(x, x_check); } @@ -75,20 +68,12 @@ TEST(MatrixLeastSquaresTest, 4x4) -1.f, -1.1f, -1.2f, -1.3f }; Matrix A(data); - - float b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f}; - Vector b(b_data); - - float x_check_data[4] = { 0.97893433f, - -2.80798701f, - -0.03175765f, - -2.19387649f - }; - Vector x_check(x_check_data); + Vector4f b(2.0f, 3.0f, 4.0f, 5.0f); + Vector4f x_check(0.97893433f, -2.80798701f, -0.03175765f, -2.19387649f); LeastSquaresSolver qrd = LeastSquaresSolver(A); - Vector x = qrd.solve(b); + Vector4f x = qrd.solve(b); EXPECT_EQ(x, x_check); } diff --git a/src/lib/matrix/test/MatrixSparseVectorTest.cpp b/src/lib/matrix/test/MatrixSparseVectorTest.cpp index 14fd28af1d..24799d1de3 100644 --- a/src/lib/matrix/test/MatrixSparseVectorTest.cpp +++ b/src/lib/matrix/test/MatrixSparseVectorTest.cpp @@ -92,11 +92,11 @@ TEST(MatrixSparseVectorTest, setZero) TEST(MatrixSparseVectorTest, additionWithDenseVector) { - Vector dense_vec; + Vector4f dense_vec; dense_vec.setAll(1.f); const float data[3] = {1.f, 2.f, 3.f}; const SparseVectorf<4, 1, 2, 3> sparse_vec(data); - const Vector res = sparse_vec + dense_vec; + const Vector4f res = sparse_vec + dense_vec; EXPECT_FLOAT_EQ(res(0), 1.f); EXPECT_FLOAT_EQ(res(1), 2.f); EXPECT_FLOAT_EQ(res(2), 3.f); @@ -115,7 +115,7 @@ TEST(MatrixSparseVectorTest, addScalar) TEST(MatrixSparseVectorTest, dotProductWithDenseVector) { - Vector dense_vec; + Vector4f dense_vec; dense_vec.setAll(3.f); const float data[3] = {1.f, 2.f, 3.f}; const SparseVectorf<4, 1, 2, 3> sparse_vec(data); diff --git a/src/lib/matrix/test/MatrixVector3Test.cpp b/src/lib/matrix/test/MatrixVector3Test.cpp index 4bb9335df0..f125ad92df 100644 --- a/src/lib/matrix/test/MatrixVector3Test.cpp +++ b/src/lib/matrix/test/MatrixVector3Test.cpp @@ -71,13 +71,7 @@ TEST(MatrixVector3Test, Vector3) Vector3f h; EXPECT_EQ(h, Vector3f(0, 0, 0)); - - Vector j; - j(0) = 1; - j(1) = 2; - j(2) = 3; - j(3) = 4; - + Vector4f j(1.f, 2.f, 3.f, 4.f); Vector3f k = j.slice<3, 1>(0, 0); Vector3f k_test(1, 2, 3); EXPECT_EQ(k, k_test); diff --git a/src/lib/system_identification/arx_rls_test.cpp b/src/lib/system_identification/arx_rls_test.cpp index 100578c845..d446ed3692 100644 --- a/src/lib/system_identification/arx_rls_test.cpp +++ b/src/lib/system_identification/arx_rls_test.cpp @@ -61,9 +61,8 @@ TEST_F(ArxRlsTest, test211) _rls.update(1, 2); _rls.update(3, 4); _rls.update(5, 6); - const Vector coefficients = _rls.getCoefficients(); - float data_check[] = {-1.79f, 0.97f, 0.42f, -0.48f}; // generated from Python script - const Vector coefficients_check(data_check); + const Vector4f coefficients = _rls.getCoefficients(); + const Vector4f coefficients_check(-1.79f, 0.97f, 0.42f, -0.48f); // generated from Python script float eps = 1e-2; EXPECT_TRUE((coefficients - coefficients_check).abs().max() < eps); } diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index fef7d70319..aaaea1cafc 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -250,9 +250,9 @@ float EkfWrapper::getYawAngle() const return euler(2); } -matrix::Vector EkfWrapper::getQuaternionVariance() const +matrix::Vector4f EkfWrapper::getQuaternionVariance() const { - return matrix::Vector(_ekf->orientation_covariances().diag()); + return matrix::Vector4f(_ekf->orientation_covariances().diag()); } int EkfWrapper::getQuaternionResetCounter() const diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h index 93576860e5..bbb7654162 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h @@ -111,7 +111,7 @@ public: Eulerf getEulerAngles() const; float getYawAngle() const; - matrix::Vector getQuaternionVariance() const; + matrix::Vector4f getQuaternionVariance() const; int getQuaternionResetCounter() const; matrix::Vector3f getDeltaVelBiasVariance() const; diff --git a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp b/src/modules/ekf2/test/test_EKF_gps_yaw.cpp index de74025d16..f166a93647 100644 --- a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp +++ b/src/modules/ekf2/test/test_EKF_gps_yaw.cpp @@ -331,9 +331,9 @@ TEST_F(EkfGpsHeadingTest, stopOnGround) _ekf_wrapper.setMagFuseTypeNone(); // WHEN: running without yaw aiding - const matrix::Vector quat_variance_before = _ekf_wrapper.getQuaternionVariance(); + const matrix::Vector4f quat_variance_before = _ekf_wrapper.getQuaternionVariance(); _sensor_simulator.runSeconds(20.0); - const matrix::Vector quat_variance_after = _ekf_wrapper.getQuaternionVariance(); + const matrix::Vector4f quat_variance_after = _ekf_wrapper.getQuaternionVariance(); // THEN: the yaw variance is constrained by fusing constant data EXPECT_LT(quat_variance_after(3), quat_variance_before(3)); diff --git a/src/modules/ekf2/test/test_EKF_initialization.cpp b/src/modules/ekf2/test/test_EKF_initialization.cpp index 7869c1d28a..71b09cc3e7 100644 --- a/src/modules/ekf2/test/test_EKF_initialization.cpp +++ b/src/modules/ekf2/test/test_EKF_initialization.cpp @@ -83,7 +83,7 @@ public: void quaternionVarianceBigEnoughAfterOrientationInitialization() { - const matrix::Vector quat_variance = _ekf_wrapper.getQuaternionVariance(); + const matrix::Vector4f quat_variance = _ekf_wrapper.getQuaternionVariance(); const float quat_variance_limit = 0.0001f; EXPECT_TRUE(quat_variance(1) > quat_variance_limit) << "quat_variance(1)" << quat_variance(1); EXPECT_TRUE(quat_variance(2) > quat_variance_limit) << "quat_variance(2)" << quat_variance(2); diff --git a/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp b/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp index fd7c3d1d01..4b49134d19 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp @@ -59,8 +59,8 @@ public: bool isAvailable() { return _input_available; }; // Position : 0 : pitch, 1 : roll, 2 : throttle, 3 : yaw - const matrix::Vector &getPosition() { return _positions; }; // Raw stick position, no deadzone - const matrix::Vector &getPositionExpo() { return _positions_expo; }; // Deadzone and expo applied + const matrix::Vector4f &getPosition() { return _positions; }; // Raw stick position, no deadzone + const matrix::Vector4f &getPositionExpo() { return _positions_expo; }; // Deadzone and expo applied // Helper functions to get stick values more intuitively float getRoll() const { return _positions(1); } @@ -90,8 +90,8 @@ public: private: bool _input_available{false}; - matrix::Vector _positions; ///< unmodified manual stick inputs - matrix::Vector _positions_expo; ///< modified manual sticks using expo function + matrix::Vector4f _positions; ///< unmodified manual stick inputs + matrix::Vector4f _positions_expo; ///< modified manual sticks using expo function uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _failsafe_flags_sub{ORB_ID(failsafe_flags)}; diff --git a/src/systemcmds/tests/test_matrix.cpp b/src/systemcmds/tests/test_matrix.cpp index 545ec46201..7f279918b2 100644 --- a/src/systemcmds/tests/test_matrix.cpp +++ b/src/systemcmds/tests/test_matrix.cpp @@ -233,7 +233,7 @@ bool MatrixTest::attitudeTests() // quaterion copy ctors float data_v4[] = {1, 2, 3, 4}; - Vector v4(data_v4); + Vector4f v4(data_v4); Quatf q_from_v(v4); ut_test(isEqual(q_from_v, v4)); @@ -242,7 +242,7 @@ bool MatrixTest::attitudeTests() ut_test(isEqual(q_from_m, m4)); // quaternion derivative - Vector q_dot = q.derivative1(Vector3f(1, 2, 3)); + Vector4f q_dot = q.derivative1(Vector3f(1, 2, 3)); (void)q_dot; // quaternion product