fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 03:23:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Remove euler angles from attitude setpoint (#23482)

Browse Source 
* Remove euler angles from attitude setpoint message

* Remove usage of euler angles in attitude setpoint messages

This commit removes the usage of euler angles in the vehicle_attitude_setpoint messages

* Fix standard vtol
This commit is contained in:
Jaeyoung Lim
2024-08-12 16:42:51 +02:00
committed by GitHub
parent c9343ca11d
commit e008ca24f1
16 changed files with 234 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/ControlMath.cpp
-6
View File
@@ -111,12 +111,6 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
// copy quaternion setpoint to attitude setpoint topic
const Quatf q_sp{R_sp};
q_sp.copyTo(att_sp.q_d);
// calculate euler angles, for logging only, must not be used for control
const Eulerf euler{R_sp};
att_sp.roll_body = euler.phi();
att_sp.pitch_body = euler.theta();
att_sp.yaw_body = euler.psi();
}
Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp
+12 -9
View File
@@ -108,18 +108,20 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
float yaw = 0.f;
vehicle_attitude_setpoint_s att{};
thrustToAttitude(thr, yaw, att);
EXPECT_FLOAT_EQ(att.roll_body, 0.f);
EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
EXPECT_FLOAT_EQ(att.yaw_body, 0.f);
EXPECT_FLOAT_EQ(att.q_d[0], 1.f);
EXPECT_FLOAT_EQ(att.q_d[1], 0.f);
EXPECT_FLOAT_EQ(att.q_d[2], 0.f);
EXPECT_FLOAT_EQ(att.q_d[3], 0.f);
EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
/* expected: same as before but with 90 yaw
* reason: only yaw changed */
yaw = M_PI_2_F;
thrustToAttitude(thr, yaw, att);
EXPECT_FLOAT_EQ(att.roll_body, 0.f);
EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
Eulerf euler_att(Quatf(att.q_d));
EXPECT_FLOAT_EQ(euler_att.phi(), 0.f);
EXPECT_FLOAT_EQ(euler_att.theta(), 0.f);
EXPECT_FLOAT_EQ(euler_att.psi(), M_PI_2_F);
EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
/* expected: same as before but roll 180
@@ -127,9 +129,10 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
* order is: 1. roll, 2. pitch, 3. yaw */
thr = Vector3f(0.f, 0.f, 1.f);
thrustToAttitude(thr, yaw, att);
EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F);
EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
Eulerf euler_att2(Quatf(att.q_d));
EXPECT_FLOAT_EQ(std::abs(euler_att2.phi()), std::abs(M_PI_F));
EXPECT_FLOAT_EQ(euler_att2.theta(), 0.f);
EXPECT_FLOAT_EQ(euler_att2.psi(), M_PI_2_F);
EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
}
src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp
+12 -8
View File
@@ -56,9 +56,10 @@ TEST(PositionControlTest, EmptySetpoint)
vehicle_attitude_setpoint_s attitude{};
position_control.getAttitudeSetpoint(attitude);
EXPECT_FLOAT_EQ(attitude.roll_body, 0.f);
EXPECT_FLOAT_EQ(attitude.pitch_body, 0.f);
EXPECT_FLOAT_EQ(attitude.yaw_body, 0.f);
Eulerf euler_att(Quatf(attitude.q_d));
EXPECT_FLOAT_EQ(euler_att.phi(), 0.f);
EXPECT_FLOAT_EQ(euler_att.theta(), 0.f);
EXPECT_FLOAT_EQ(euler_att.psi(), 0.f);
EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f);
EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f));
EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f));
@@ -184,7 +185,8 @@ TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit)
EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -MAXIMUM_THRUST);
// Then the horizontal margin results in a tilt with the ratio of: horizontal margin / maximum thrust
EXPECT_FLOAT_EQ(_attitude.roll_body, asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST));
Eulerf euler_att(Quatf(_attitude.q_d));
EXPECT_FLOAT_EQ(euler_att.phi(), asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST));
// TODO: add this line back once attitude setpoint generation strategy does not align body yaw with heading all the time anymore
// EXPECT_FLOAT_EQ(_attitude.pitch_body, -asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST));
}
@@ -198,9 +200,10 @@ TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit)
EXPECT_FLOAT_EQ(thrust.length(), 0.1f);
EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.1f);
Eulerf euler_att(Quatf(_attitude.q_d));
EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f);
EXPECT_FLOAT_EQ(_attitude.pitch_body, -1.f);
EXPECT_FLOAT_EQ(euler_att.phi(), 0.f);
EXPECT_FLOAT_EQ(euler_att.theta(), -1.f);
}
TEST_F(PositionControlBasicTest, FailsafeInput)
@@ -377,6 +380,7 @@ TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint)
EXPECT_TRUE(runController());
// THEN: the integral did not wind up and produce unexpected deviation
EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f);
EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f);
Eulerf euler_att(Quatf(_attitude.q_d));
EXPECT_FLOAT_EQ(euler_att.phi(), 0.f);
EXPECT_FLOAT_EQ(euler_att.theta(), 0.f);
}