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

Fix transformation issues and use aero params from advanced plane

Browse Source
This commit is contained in:
JaeyoungLim
2025-11-24 06:53:25 -08:00
parent 664d5fc169
commit 47d7fc7688
2 changed files with 9 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_wind_estimator/FixedwingWindEstimator.cpp
+4 -8
View File
@@ -133,8 +133,7 @@ FixedwingWindEstimator::vehicle_acceleration_poll()
vehicle_acceleration_s vehicle_acceleration; vehicle_acceleration_s vehicle_acceleration;
if (_vehicle_acceleration_sub.update(&vehicle_acceleration)) { if (_vehicle_acceleration_sub.update(&vehicle_acceleration)) {
Dcmf R_ib(_attitude); _acceleration = Vector3f(vehicle_acceleration.xyz);
_acceleration = _attitude.rotateVector(Vector3f(vehicle_acceleration.xyz));
} }
} }
@@ -142,16 +141,13 @@ matrix::Vector3f FixedwingWindEstimator::compute_wind_estimate()
{ {
float _rho{1.225}; float _rho{1.225};
Dcmf R_ib(_attitude);
Dcmf R_bi(R_ib.transpose());
// compute expected AoA from g-forces: // compute expected AoA from g-forces:
matrix::Vector3f body_force = _mass * _attitude.rotateVectorInverse(_acceleration + _gravity); matrix::Vector3f body_force = _mass * (_acceleration + _attitude.rotateVectorInverse(_gravity));
// ***************** NEW COMPUTATION FROM MATLAB CALIBRATION **********************
float speed = fmaxf(_calibrated_airspeed, _stall_airspeed); float speed = fmaxf(_calibrated_airspeed, _stall_airspeed);
float u_approx = _true_airspeed; float u_approx = _true_airspeed;
float v_approx = body_force(1) * _true_airspeed / (0.5f * _rho * powf(speed, 2) * _wing_area * _C_B1); float v_approx = -body_force(1) * _true_airspeed / (0.5f * _rho * powf(speed, 2) * _wing_area * _C_B1);
float w_approx = (-body_force(2) * _true_airspeed / (0.5f * _rho * powf(speed, 2) * _wing_area) - _C_A0) / _C_A1; float w_approx = (body_force(2) * _true_airspeed / (0.5f * _rho * powf(speed, 2) * _wing_area) + _C_A0) / _C_A1;
Vector3f vel_air(u_approx, v_approx, w_approx); Vector3f vel_air(u_approx, v_approx, w_approx);
return vel_air; return vel_air;
} }
src/modules/fw_wind_estimator/fw_wind_estimator_params.c
+5 -5
View File
@@ -61,7 +61,7 @@ PARAM_DEFINE_FLOAT(FW_W_MASS, 1.00f);
* @increment 0.01 * @increment 0.01
* @group FW Wind Estimator * @group FW Wind Estimator
*/ */
PARAM_DEFINE_FLOAT(FW_W_AREA, 1.00f); PARAM_DEFINE_FLOAT(FW_W_AREA, 0.34f);
/** /**
* Vehicle Aerodynamic coefficient * Vehicle Aerodynamic coefficient
@@ -73,7 +73,7 @@ PARAM_DEFINE_FLOAT(FW_W_AREA, 1.00f);
* @increment 0.01 * @increment 0.01
* @group FW Wind Estimator * @group FW Wind Estimator
*/ */
PARAM_DEFINE_FLOAT(FW_W_C_B1, 1.00f); PARAM_DEFINE_FLOAT(FW_W_C_B1, 1.0f);
/** /**
* Vehicle Aerodynamic coefficient * Vehicle Aerodynamic coefficient
@@ -85,16 +85,16 @@ PARAM_DEFINE_FLOAT(FW_W_C_B1, 1.00f);
* @increment 0.01 * @increment 0.01
* @group FW Wind Estimator * @group FW Wind Estimator
*/ */
PARAM_DEFINE_FLOAT(FW_W_C_A0, 0.1949f); PARAM_DEFINE_FLOAT(FW_W_C_A0, 0.15188f);
/** /**
* Vehicle Aerodynamic coefficient * Vehicle Aerodynamic coefficient
* *
* @unit %/rad/s * @unit %/rad/s
* @min 0.0 * @min 0.0
* @max 4 * @max 10
* @decimal 3 * @decimal 3
* @increment 0.01 * @increment 0.01
* @group FW Wind Estimator * @group FW Wind Estimator
*/ */
PARAM_DEFINE_FLOAT(FW_W_C_A1, 3.5928f); PARAM_DEFINE_FLOAT(FW_W_C_A1, 5.015f);