fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw-att: avoid multiple conversions

Browse Source
This commit is contained in:
bresch 2025-01-03 14:11:28 +01:00 committed by Mathieu Bresciani
parent 9bf5257e0e
commit f1a339e7c6
1 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -321,22 +321,22 @@ void FixedwingAttitudeControl::Run()
/* Run attitude controllers */
if (_vcontrol_mode.flag_control_attitude_enabled && _in_fw_or_transition_wo_tailsitter_transition) {
const Eulerf setpoint(Quatf(_att_sp.q_d));
const float roll_body = setpoint.phi();
const float pitch_body = setpoint.theta();
const Quatf q_sp(_att_sp.q_d);
if (PX4_ISFINITE(roll_body) && PX4_ISFINITE(pitch_body)) {
if (q_sp.isAllFinite()) {
const Eulerf euler_sp(q_sp);
const float roll_sp = euler_sp.phi();
const float pitch_sp = euler_sp.theta();
_roll_ctrl.control_roll(roll_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
_roll_ctrl.control_roll(roll_sp, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
euler_angles.theta());
_pitch_ctrl.control_pitch(pitch_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
_pitch_ctrl.control_pitch(pitch_sp, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
euler_angles.theta());
_yaw_ctrl.control_yaw(roll_body, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
_yaw_ctrl.control_yaw(roll_sp, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
euler_angles.theta(), get_airspeed_constrained());
if (wheel_control) {
Eulerf attitude_setpoint(Quatf(_att_sp.q_d));
_wheel_ctrl.control_attitude(attitude_setpoint.psi(), euler_angles.psi());
_wheel_ctrl.control_attitude(euler_sp.psi(), euler_angles.psi());
} else {
_wheel_ctrl.reset_integrator();