From 7c8dc3d2299d7579b65bdef3f824cb84ead1c46b Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 20 Sep 2023 20:13:31 +0200 Subject: [PATCH] TEMP --- src/modules/ekf2/EKF2.cpp | 5 ++-- .../AttitudeControl/AttitudeControlTest.cpp | 28 +++++++++++++++++++ .../PositionControl/PositionAttitudeTest.cpp | 9 ++++-- 3 files changed, 38 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 48354773a4..100c8e2550 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1593,8 +1593,9 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) Quatf delta_q_reset; _ekf.get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter); - lpos.heading = Eulerf(_ekf.getQuaternion()).psi(); - lpos.unaided_heading = _ekf.getUnaidedYaw(); + const Quatf q = _ekf.getQuaternion(); + lpos.heading = 2.f * atan2f(q(3), q(0)); + lpos.unaided_heading = _ekf.getUnaidedYaw(); // TODO: Needs to change as well lpos.delta_heading = Eulerf(delta_q_reset).psi(); lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete(); diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp index 2803686c12..ca9186637c 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp @@ -138,3 +138,31 @@ TEST(AttitudeControlTest, YawWeightScaling) // THEN: no actuation (also no NAN) EXPECT_EQ(rate_setpoint, Vector3f()); } + +TEST(AttitudeControlTest, HeadingCorrectness) +{ + //Quatf q(0.863f, 0.358f, 0.358f, 0.f); + Quatf q(0.733f, 0.462f, 0.191f, 0.462f); + q.normalize(); + float yaw = matrix::Eulerf(q).psi(); + + Quatf q_red(Vector3f(0, 0, 1), q.dcm_z()); + Quatf q_mix = q_red.inversed() * q; + q_mix.print(); + + Quatf q_mix2 = q; + q_mix2(1) = 0.f; + q_mix2(2) = 0.f; + q_mix2.normalize(); + q_mix2.print(); + + q_mix2(3) = math::constrain(q_mix2(3), -1.f, 1.f); + float heading = 2.f * asinf(q_mix2(3)); + + float heading2 = 2.f * atan2f(q(3), q(0)); + EXPECT_FLOAT_EQ(heading, heading2); + + EXPECT_GT(fabsf(yaw), FLT_EPSILON); + EXPECT_LT(fabsf(heading), FLT_EPSILON); + EXPECT_TRUE(false); +} diff --git a/src/modules/mc_pos_control/PositionControl/PositionAttitudeTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionAttitudeTest.cpp index 10aecabc13..d630aeb34f 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionAttitudeTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionAttitudeTest.cpp @@ -49,12 +49,13 @@ TEST(PositionControlTest, NavigationYawInfluence) // Set up attitude control AttitudeControl attitude_control; - attitude_control.setProportionalGain(Vector3f(1.f, 1.f, 1.f)); + attitude_control.setProportionalGain(Vector3f(1.f, 1.f, 1.f), 1.f); attitude_control.setRateLimit(Vector3f(100.f, 100.f, 100.f)); // Execute on attitude Quatf qd(attitude_setpoint.q_d); - const Vector3f rate_setpoint = attitude_control.update(Quatf(), qd, 0.f); + attitude_control.setAttitudeSetpoint(qd, 0.f); + const Vector3f rate_setpoint = attitude_control.update(Quatf()); rate_setpoint.print(); // expect symmetric angular rate command towards thrust direction without any body yaw correction @@ -62,4 +63,8 @@ TEST(PositionControlTest, NavigationYawInfluence) EXPECT_LT(rate_setpoint(1), 0.f); EXPECT_FLOAT_EQ(fabsf(rate_setpoint(0)), fabsf(rate_setpoint(1))); EXPECT_FLOAT_EQ(rate_setpoint(2), 0.f); + + qd.print(); + Eulerf(qd).print(); + EXPECT_FLOAT_EQ(2.f * atan2f(qd(3), qd(0)), yaw_setpoint); }