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

added min saturation for airspeed scaling

Browse Source
This commit is contained in:
Marvin Harms
2022-06-14 16:12:14 +02:00
parent 7cc062f384
commit e5689bdd0b
2 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/sitl_gazebo
+1 -1
Submodule Tools/sitl_gazebo updated: a3eef9d3a1...59a34c17ff
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+1 -1
View File
@@ -1112,7 +1112,7 @@ FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl)
// compute velocity in body frame
Dcmf R_ib(_att);
Vector3f vel_body = R_ib.transpose()*_vel;
float q = 0.5f*sqrtf(vel_body*vel_body)*vel_body(0); // dynamic pressure
float q = fmaxf(0.5f*sqrtf(vel_body*vel_body)*vel_body(0), 0.5f*5.f*5.f); // dynamic pressure, saturates at minimum 5m/s stall
//Vector3f vel_body_2 = Dcmf(Quatf(_attitude.q)).transpose()*Vector3f{_local_pos.vx,_local_pos.vy,_local_pos.vz};
//PX4_INFO("ENU body frame velocity: \t%.2f\t%.2f\t%.2f", (double)vel_body_2(0), (double)vel_body_2(1), (double)vel_body_2(2));
//PX4_INFO("FRD body frame velocity: \t%.2f\t%.2f\t%.2f", (double)vel_body(0), (double)vel_body(1), (double)vel_body(2));