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

Consider wind

Browse Source
This commit is contained in:
JaeyoungLim
2025-11-29 17:07:31 -08:00
parent 8ed9bb46b7
commit cc58fba43b
1 changed files with 4 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulation/gz_plugins/airspeed/AirSpeed.cpp
+4 -1
View File
@@ -117,7 +117,10 @@ void AirSpeed::PreUpdate(const gz::sim::UpdateInfo &_info,
// Calculate differential pressure + noise in hPa // Calculate differential pressure + noise in hPa
const float diff_pressure_noise = standard_normal_distribution_(random_generator_) * diff_pressure_stddev_; const float diff_pressure_noise = standard_normal_distribution_(random_generator_) * diff_pressure_stddev_;
// Body-relateive air velocity // Body-relateive air velocity
gz::math::Vector3d body_velocity = _vehicle_attitude.RotateVectorReverse(_vehicle_velocity); ///TODO: Subscribe to wind velocity from the world
gz::math::Vector3d _wind_velocity{0.0, 0.0, 0.0};
gz::math::Vector3d air_relative_velocity = _vehicle_velocity - _wind_velocity;
gz::math::Vector3d body_velocity = _vehicle_attitude.RotateVectorReverse(air_relative_velocity);
double diff_pressure = sign(body_velocity.X()) * 0.005 * (double)air_density * body_velocity.X() * body_velocity.X() + double diff_pressure = sign(body_velocity.X()) * 0.005 * (double)air_density * body_velocity.X() * body_velocity.X() +
(double)diff_pressure_noise; (double)diff_pressure_noise;
// Calculate differential pressure in hPa // Calculate differential pressure in hPa