dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

TEMP

This commit is contained in:
Matthias Grob 2023-09-20 20:13:31 +02:00
parent 612de1e0f3
commit 7c8dc3d229
3 changed files with 38 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/ekf2/EKF2.cpp
View File

@ -1593,8 +1593,9 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
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();

28
src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp
View File

@ -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);
}

9
src/modules/mc_pos_control/PositionControl/PositionAttitudeTest.cpp
View File

@ -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);
}