1 #include <gtest/gtest.h> 8 #include "eigen3/Eigen/Core" 9 #include "eigen3/Eigen/Geometry" 10 #include "eigen3/Eigen/Dense" 14 #define EXPECT_VEC3_SUPERCLOSE(vec, eig) \ 15 EXPECT_NEAR((vec).x, (eig).x(), 0.0001);\ 16 EXPECT_NEAR((vec).y, (eig).y(), 0.0001);\ 17 EXPECT_NEAR((vec).z, (eig).z(), 0.0001) 18 #define EXPECT_QUAT_SUPERCLOSE(q, q_eig) { \ 19 double e1 = quaternion_error((q_eig), (q)); \ 20 Eigen::Quaternionf q_eig_neg = q_eig; \ 21 q_eig_neg.coeffs() *= -1.0; \ 22 double e2 = quaternion_error(q_eig_neg, (q)); \ 23 double error = (e1 < e2) ? e1 : e2; \ 24 EXPECT_LE(error, 0.0001); \ 26 #define ASSERT_QUAT_SUPERCLOSE(q, q_eig) { \ 27 double e1 = quaternion_error((q_eig), (q)); \ 28 Eigen::Quaternionf q_eig_neg = q_eig; \ 29 q_eig_neg.coeffs() *= -1.0; \ 30 double e2 = quaternion_error(q_eig_neg, (q)); \ 31 double error = (e1 < e2) ? e1 : e2; \ 32 EXPECT_LE(error, 0.0001); \ 35 #define ASSERT_TURBOQUAT_SUPERCLOSE(q, q2) { \ 36 double e1 = quaternion_error(q, q2); \ 37 turbomath::Quaternion q2_neg = q2; \ 42 double e2 = quaternion_error(q, q2_neg); \ 43 double error = (e1 < e2) ? e1 : e2; \ 44 ASSERT_LE(error, 0.0001); \ 48 #define EXPECT_BASICALLYTHESAME(x, y) EXPECT_NEAR(x, y, 0.00001) 49 #define EXPECT_SUPERCLOSE(x, y) EXPECT_NEAR(x, y, 0.0001) 50 #define EXPECT_CLOSE(x, y) EXPECT_NEAR(x, y, 0.01) 51 #define EXPECT_INTHESAMEBALLPARK(x, y) EXPECT_NEAR(x, y, 0.1) 53 #define ASSERT_BASICALLYTHESAME(x, y) ASSERT_NEAR(x, y, 0.00001) 54 #define ASSERT_SUPERCLOSE(x, y) ASSERT_NEAR(x, y, 0.0001) 55 #define ASSERT_CLOSE(x, y) ASSERT_NEAR(x, y, 0.01) 56 #define ASSERT_INTHESAMEBALLPARK(x, y) ASSERT_NEAR(x, y, 0.1)
void step_firmware(rosflight_firmware::ROSflight &rf, rosflight_firmware::testBoard &board, uint32_t us)
double quaternion_error(turbomath::Quaternion q0, turbomath::Quaternion q)