From 47d7fc7688ef26f2629f91d03f2f00f79dc0cb7c Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Mon, 24 Nov 2025 06:11:56 -0800 Subject: [PATCH] Fix transformation issues and use aero params from advanced plane --- .../fw_wind_estimator/FixedwingWindEstimator.cpp | 12 ++++-------- .../fw_wind_estimator/fw_wind_estimator_params.c | 10 +++++----- 2 files changed, 9 insertions(+), 13 deletions(-) diff --git a/src/modules/fw_wind_estimator/FixedwingWindEstimator.cpp b/src/modules/fw_wind_estimator/FixedwingWindEstimator.cpp index 80078903ee..90eaee8bf7 100644 --- a/src/modules/fw_wind_estimator/FixedwingWindEstimator.cpp +++ b/src/modules/fw_wind_estimator/FixedwingWindEstimator.cpp @@ -133,8 +133,7 @@ FixedwingWindEstimator::vehicle_acceleration_poll() vehicle_acceleration_s vehicle_acceleration; if (_vehicle_acceleration_sub.update(&vehicle_acceleration)) { - Dcmf R_ib(_attitude); - _acceleration = _attitude.rotateVector(Vector3f(vehicle_acceleration.xyz)); + _acceleration = Vector3f(vehicle_acceleration.xyz); } } @@ -142,16 +141,13 @@ matrix::Vector3f FixedwingWindEstimator::compute_wind_estimate() { float _rho{1.225}; - Dcmf R_ib(_attitude); - Dcmf R_bi(R_ib.transpose()); // 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 u_approx = _true_airspeed; - 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 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; Vector3f vel_air(u_approx, v_approx, w_approx); return vel_air; } diff --git a/src/modules/fw_wind_estimator/fw_wind_estimator_params.c b/src/modules/fw_wind_estimator/fw_wind_estimator_params.c index 9308110f25..5c7ce6955f 100644 --- a/src/modules/fw_wind_estimator/fw_wind_estimator_params.c +++ b/src/modules/fw_wind_estimator/fw_wind_estimator_params.c @@ -61,7 +61,7 @@ PARAM_DEFINE_FLOAT(FW_W_MASS, 1.00f); * @increment 0.01 * @group FW Wind Estimator */ -PARAM_DEFINE_FLOAT(FW_W_AREA, 1.00f); +PARAM_DEFINE_FLOAT(FW_W_AREA, 0.34f); /** * Vehicle Aerodynamic coefficient @@ -73,7 +73,7 @@ PARAM_DEFINE_FLOAT(FW_W_AREA, 1.00f); * @increment 0.01 * @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 @@ -85,16 +85,16 @@ PARAM_DEFINE_FLOAT(FW_W_C_B1, 1.00f); * @increment 0.01 * @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 * * @unit %/rad/s * @min 0.0 -* @max 4 +* @max 10 * @decimal 3 * @increment 0.01 * @group FW Wind Estimator */ -PARAM_DEFINE_FLOAT(FW_W_C_A1, 3.5928f); +PARAM_DEFINE_FLOAT(FW_W_C_A1, 5.015f);