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

feat(fw_att_control): use Axis angles instead of Euler Angles

Browse Source
This commit is contained in:
ttechnick 2026-03-16 14:05:13 +01:00 committed by Nick
parent 22cad6ebf7
commit ca4ae7cf41
2 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -105,7 +105,10 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
pitch_body = constrain(pitch_body,
-radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
const Quatf q(Eulerf(roll_body, pitch_body, yaw_body));
const Quatf q_sp_rp = AxisAnglef(roll_body, pitch_body, 0.f);
const Quatf q_sp_yaw(cosf(yaw_body / 2.f), 0.f, 0.f, sinf(yaw_body / 2.f));
Quatf q = q_sp_yaw * q_sp_rp;
q.copyTo(_att_sp.q_d);
_att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f;

1
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

@ -64,6 +64,7 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
using matrix::AxisAnglef;
using matrix::Eulerf;
using matrix::Quatf;
using matrix::Vector3f;