Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-18 00:08:22 +08:00
Code Issues Actions 4 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
msg/VehicleAttitudeSetpoint.msg
-4
View File
@@ -1,9 +1,5 @@
uint64 timestamp # time since system start (microseconds)
float32 roll_body # body angle in NED frame (can be NaN for FW)
float32 pitch_body # body angle in NED frame (can be NaN for FW)
float32 yaw_body # body angle in NED frame (can be NaN for FW)
float32 yaw_sp_move_rate # rad/s (commanded by user)
# For quaternion-based attitude control
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+17 -12
View File
@@ -96,17 +96,16 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
// STABILIZED mode generate the attitude setpoint from manual user inputs
_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get());
const float roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get());
_att_sp.pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get())
+ radians(_param_fw_psp_off.get());
_att_sp.pitch_body = constrain(_att_sp.pitch_body,
-radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
float pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get())
+ radians(_param_fw_psp_off.get());
pitch_body = constrain(pitch_body,
-radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
_att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw
_att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f;
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
const Quatf q(Eulerf(roll_body, pitch_body, yaw_body));
q.copyTo(_att_sp.q_d);
_att_sp.reset_integral = false;
@@ -325,16 +324,22 @@ void FixedwingAttitudeControl::Run()
/* Run attitude controllers */
if (_vcontrol_mode.flag_control_attitude_enabled && _in_fw_or_transition_wo_tailsitter_transition) {
if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) {
_roll_ctrl.control_roll(_att_sp.roll_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
const Eulerf setpoint(Quatf(_att_sp.q_d));
const float roll_body = setpoint.phi();
const float pitch_body = setpoint.theta();
if (PX4_ISFINITE(roll_body) && PX4_ISFINITE(pitch_body)) {
_roll_ctrl.control_roll(roll_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
euler_angles.theta());
_pitch_ctrl.control_pitch(_att_sp.pitch_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
_pitch_ctrl.control_pitch(pitch_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
euler_angles.theta());
_yaw_ctrl.control_yaw(_att_sp.roll_body, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
_yaw_ctrl.control_yaw(roll_body, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
euler_angles.theta(), get_airspeed_constrained());
if (wheel_control) {
_wheel_ctrl.control_attitude(_att_sp.yaw_body, euler_angles.psi());
Eulerf attitude_setpoint(Quatf(_att_sp.q_d));
_wheel_ctrl.control_attitude(attitude_setpoint.psi(), euler_angles.psi());
} else {
_wheel_ctrl.reset_integrator();
src/modules/fw_pos_control/FixedwingPositionControl.cpp
+153 -74
View File
File diff suppressed because it is too large Load Diff
src/modules/mavlink/mavlink_receiver.cpp
-5
View File
@@ -1626,11 +1626,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
const matrix::Quatf q{attitude_target.q};
q.copyTo(attitude_setpoint.q_d);
matrix::Eulerf euler{q};
attitude_setpoint.roll_body = euler.phi();
attitude_setpoint.pitch_body = euler.theta();
attitude_setpoint.yaw_body = euler.psi();
// TODO: review use case
attitude_setpoint.yaw_sp_move_rate = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE) ?
(float)NAN : attitude_target.body_yaw_rate;
src/modules/mavlink/streams/HIGH_LATENCY2.hpp
+3 -1
View File
@@ -264,7 +264,9 @@ private:
vehicle_attitude_setpoint_s attitude_sp;
if (_attitude_sp_sub.update(&attitude_sp)) {
msg->target_heading = static_cast<uint8_t>(math::degrees(matrix::wrap_2pi(attitude_sp.yaw_body)) * 0.5f);
msg->target_heading = static_cast<uint8_t>(math::degrees(matrix::wrap_2pi(matrix::Eulerf(matrix::Quatf(
attitude_sp.q_d)).psi())) * 0.5f);
return true;
}
src/modules/mc_att_control/mc_att_control_main.cpp
-6
View File
@@ -182,12 +182,6 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
q_sp.copyTo(attitude_setpoint.q_d);
// Transform to euler angles for logging only
const Eulerf euler_sp(q_sp);
attitude_setpoint.roll_body = euler_sp(0);
attitude_setpoint.pitch_body = euler_sp(1);
attitude_setpoint.yaw_body = euler_sp(2);
attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_setpoint.throttle);
attitude_setpoint.timestamp = hrt_absolute_time();
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);
}
src/modules/rover_pos_control/RoverPositionControl.cpp
+4 -4
View File
@@ -122,8 +122,8 @@ RoverPositionControl::manual_control_setpoint_poll()
if (_control_mode.flag_control_attitude_enabled) {
// STABILIZED mode generate the attitude setpoint from manual user inputs
_att_sp.roll_body = 0.0;
_att_sp.pitch_body = 0.0;
float roll_body = 0.0;
float pitch_body = 0.0;
/* reset yaw setpoint to current position if needed */
if (_reset_yaw_sp) {
@@ -137,10 +137,10 @@ RoverPositionControl::manual_control_setpoint_poll()
_manual_yaw_sp = wrap_pi(_manual_yaw_sp + _att_sp.yaw_sp_move_rate * dt);
}
_att_sp.yaw_body = _manual_yaw_sp;
float yaw_body = _manual_yaw_sp;
_att_sp.thrust_body[0] = _manual_control_setpoint.throttle;
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
const Quatf q(Eulerf(roll_body, pitch_body, yaw_body));
q.copyTo(_att_sp.q_d);
_att_sp.timestamp = hrt_absolute_time();
src/modules/uuv_att_control/uuv_att_control.cpp
+6 -6
View File
@@ -138,9 +138,10 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude
*/
Eulerf euler_angles(matrix::Quatf(attitude.q));
float roll_body = attitude_setpoint.roll_body;
float pitch_body = attitude_setpoint.pitch_body;
float yaw_body = attitude_setpoint.yaw_body;
const Eulerf setpoint_euler_angles(matrix::Quatf(attitude_setpoint.q_d));
const float roll_body = setpoint_euler_angles(0);
const float pitch_body = setpoint_euler_angles(1);
const float yaw_body = setpoint_euler_angles(2);
float roll_rate_desired = rates_setpoint.roll;
float pitch_rate_desired = rates_setpoint.pitch;
@@ -227,9 +228,8 @@ void UUVAttitudeControl::Run()
_vehicle_rates_setpoint_sub.update(&_rates_setpoint);
if (input_mode == 1) { // process manual data
_attitude_setpoint.roll_body = _param_direct_roll.get();
_attitude_setpoint.pitch_body = _param_direct_pitch.get();
_attitude_setpoint.yaw_body = _param_direct_yaw.get();
Quatf attitude_setpoint(Eulerf(_param_direct_roll.get(), _param_direct_pitch.get(), _param_direct_yaw.get()));
attitude_setpoint.copyTo(_attitude_setpoint.q_d);
_attitude_setpoint.thrust_body[0] = _param_direct_thrust.get();
_attitude_setpoint.thrust_body[1] = 0.f;
_attitude_setpoint.thrust_body[2] = 0.f;
src/modules/uuv_pos_control/uuv_pos_control.cpp
+2 -3
View File
@@ -97,9 +97,8 @@ void UUVPOSControl::publish_attitude_setpoint(const float thrust_x, const float
vehicle_attitude_setpoint_s vehicle_attitude_setpoint = {};
vehicle_attitude_setpoint.timestamp = hrt_absolute_time();
vehicle_attitude_setpoint.roll_body = roll_des;
vehicle_attitude_setpoint.pitch_body = pitch_des;
vehicle_attitude_setpoint.yaw_body = yaw_des;
const Quatf attitude_setpoint(Eulerf(roll_des, pitch_des, yaw_des));
attitude_setpoint.copyTo(vehicle_attitude_setpoint.q_d);
vehicle_attitude_setpoint.thrust_body[0] = thrust_x;
vehicle_attitude_setpoint.thrust_body[1] = thrust_y;
src/modules/vtol_att_control/standard.cpp
+10 -7
View File
@@ -172,6 +172,11 @@ void Standard::update_transition_state()
VtolType::update_transition_state();
const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d));
float roll_body = attitude_setpoint_euler.phi();
float pitch_body = attitude_setpoint_euler.theta();
float yaw_body = attitude_setpoint_euler.psi();
// we get attitude setpoint from a multirotor flighttask if climbrate is controlled.
// in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input.
if (_v_control_mode->flag_control_climb_rate_enabled) {
@@ -181,7 +186,7 @@ void Standard::update_transition_state()
}
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi();
} else {
// we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active)
@@ -227,21 +232,19 @@ void Standard::update_transition_state()
}
// ramp up FW_PSP_OFF
_v_att_sp->pitch_body = math::radians(_param_fw_psp_off.get()) * (1.0f - mc_weight);
pitch_body = math::radians(_param_fw_psp_off.get()) * (1.0f - mc_weight);
_v_att_sp->thrust_body[0] = _pusher_throttle;
const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body));
q_sp.copyTo(_v_att_sp->q_d);
} else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) {
if (_v_control_mode->flag_control_climb_rate_enabled) {
// control backtransition deceleration using pitch.
_v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp();
pitch_body = update_and_get_backtransition_pitch_sp();
}
const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body));
q_sp.copyTo(_v_att_sp->q_d);
_pusher_throttle = 0.0f;
src/modules/vtol_att_control/tailsitter.cpp
+4 -6
View File
@@ -179,7 +179,8 @@ void Tailsitter::update_transition_state()
// the yaw setpoint and zero roll since we want wings level transition.
// If for some reason the fw attitude setpoint is not recent then don't use it and assume 0 pitch
if (_fw_virtual_att_sp->timestamp > (now - 1_s)) {
_q_trans_start = Eulerf(0.f, _fw_virtual_att_sp->pitch_body, yaw_sp);
const float pitch_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).theta();
_q_trans_start = Eulerf(0.f, pitch_body, yaw_sp);
} else {
_q_trans_start = Eulerf(0.f, 0.f, yaw_sp);
@@ -191,7 +192,8 @@ void Tailsitter::update_transition_state()
} else if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) {
// initial attitude setpoint for the transition should be with wings level
_q_trans_start = Eulerf(0.f, _mc_virtual_att_sp->pitch_body, _mc_virtual_att_sp->yaw_body);
const Eulerf setpoint_euler(Quatf(_mc_virtual_att_sp->q_d));
_q_trans_start = Eulerf(0.f, setpoint_euler.theta(), setpoint_euler.psi());
Vector3f x = Dcmf(Quatf(_v_att->q)) * Vector3f(1.f, 0.f, 0.f);
_trans_rot_axis = -x.cross(Vector3f(0.f, 0.f, -1.f));
}
@@ -238,10 +240,6 @@ void Tailsitter::update_transition_state()
_v_att_sp->timestamp = hrt_absolute_time();
const Eulerf euler_sp(_q_trans_sp);
_v_att_sp->roll_body = euler_sp.phi();
_v_att_sp->pitch_body = euler_sp.theta();
_v_att_sp->yaw_body = euler_sp.psi();
_q_trans_sp.copyTo(_v_att_sp->q_d);
}
src/modules/vtol_att_control/tiltrotor.cpp
+8 -3
View File
@@ -203,6 +203,11 @@ void Tiltrotor::update_transition_state()
const hrt_abstime now = hrt_absolute_time();
const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d));
float roll_body = attitude_setpoint_euler.phi();
float pitch_body = attitude_setpoint_euler.theta();
float yaw_body = attitude_setpoint_euler.psi();
// we get attitude setpoint from a multirotor flighttask if altitude is controlled.
// in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input.
if (_v_control_mode->flag_control_climb_rate_enabled) {
@@ -212,7 +217,7 @@ void Tiltrotor::update_transition_state()
}
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi();
_thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
} else {
@@ -290,7 +295,7 @@ void Tiltrotor::update_transition_state()
// control backtransition deceleration using pitch.
if (_v_control_mode->flag_control_climb_rate_enabled) {
_v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp();
pitch_body = update_and_get_backtransition_pitch_sp();
}
if (_time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) {
@@ -321,7 +326,7 @@ void Tiltrotor::update_transition_state()
_v_att_sp->thrust_body[2] = -_thrust_transition;
const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body));
q_sp.copyTo(_v_att_sp->q_d);
_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
src/modules/vtol_att_control/vtol_type.cpp
+3 -3
View File
@@ -563,10 +563,10 @@ float VtolType::pusher_assist()
tilt_new = R_yaw_correction * tilt_new;
// now extract roll and pitch setpoints
_v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2));
_v_att_sp->roll_body = -asinf(tilt_new(1));
const float pitch_body = atan2f(tilt_new(0), tilt_new(2));
const float roll_body = -asinf(tilt_new(1));
const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2)));
const Quatf q_sp(Eulerf(roll_body, pitch_body, euler_sp(2)));
q_sp.copyTo(_v_att_sp->q_d);
}