fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 04:47:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

WIP

Browse Source
This commit is contained in:
JaeyoungLim
2025-07-07 15:10:33 +09:00
parent c49fea2a54
commit 3e63aec18d
1 changed files with 9 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_indi_pos_control/FixedwingIndiPosControl.cpp
+9 -5
View File
@@ -157,12 +157,16 @@ Quatf
FixedwingIndiPosControl::get_flat_attitude(Vector3f vel, Vector3f f)
{
Vector3f vel_air = vel - wind_estimate_;
const Vector3f vel_air = vel - wind_estimate_;
// compute force component projected onto lift axis
Vector3f vel_normalized = vel_air.normalized();
Vector3f f_lift = f - (f * vel_normalized) * vel_normalized;
Vector3f lift_normalized = f_lift.normalized();
Vector3f wing_normalized = -vel_normalized.cross(lift_normalized);
const Vector3f vel_normalized = vel_air.normalized();
Vector3f f_drag = (f * vel_normalized) * vel_normalized;
Vector3f f_lift = f - f_drag;
const Vector3f lift_normalized = f_lift.normalized();
const Vector3f wing_normalized = -vel_normalized.cross(lift_normalized);
// compute rotation matrix between ENU and FRD frame
Dcmf R_bi;
R_bi(0, 0) = vel_normalized(0);