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

Update src/modules/fw_wind_estimator/FixedwingWindEstimator.cpp

Browse Source 
Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
Update src/modules/fw_wind_estimator/FixedwingWindEstimator.cpp

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
Update src/modules/fw_wind_estimator/FixedwingWindEstimator.cpp

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
Update src/modules/fw_wind_estimator/FixedwingWindEstimator.hpp

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
Update src/modules/fw_wind_estimator/fw_wind_estimator_params.c

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
Address review comments
This commit is contained in:
Jaeyoung Lim
2025-11-24 06:03:07 -08:00
committed by JaeyoungLim
parent 8596f491c6
commit 664d5fc169
4 changed files with 5 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_wind_estimator/FixedwingWindEstimator.cpp
+3 -3
View File
@@ -134,7 +134,7 @@ FixedwingWindEstimator::vehicle_acceleration_poll()
if (_vehicle_acceleration_sub.update(&vehicle_acceleration)) {
Dcmf R_ib(_attitude);
_acceleration = R_ib * Vector3f(vehicle_acceleration.xyz);
_acceleration = _attitude.rotateVector(Vector3f(vehicle_acceleration.xyz));
}
}
@@ -145,14 +145,14 @@ matrix::Vector3f FixedwingWindEstimator::compute_wind_estimate()
Dcmf R_ib(_attitude);
Dcmf R_bi(R_ib.transpose());
// compute expected AoA from g-forces:
matrix::Vector3f body_force = _mass * R_bi * (_acceleration + _gravity);
matrix::Vector3f body_force = _mass * _attitude.rotateVectorInverse(_acceleration + _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;
matrix::Vector3f vel_air = matrix::Vector3f{u_approx, v_approx, w_approx};
Vector3f vel_air(u_approx, v_approx, w_approx);
return vel_air;
}
src/modules/fw_wind_estimator/FixedwingWindEstimator.hpp
-2
View File
@@ -64,8 +64,6 @@
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_local_position.h>
using matrix::Eulerf;
using matrix::Quatf;
using uORB::SubscriptionData;
src/modules/fw_wind_estimator/fw_wind_estimator_params.c
+1 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2023 PX4 Development Team. All rights reserved.
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
src/modules/logger/logged_topics.cpp
+1 -1
View File
@@ -217,13 +217,13 @@ void LoggedTopics::add_default_topics()
add_topic("actuator_servos", 100);
add_topic_multi("vehicle_thrust_setpoint", 20, 2);
add_topic_multi("vehicle_torque_setpoint", 20, 2);
add_topic("airflow_groundtruth", 10);
// SYS_HITL: default ground truth logging for simulation
int32_t sys_hitl = 0;
param_get(param_find("SYS_HITL"), &sys_hitl);
if (sys_hitl >= 1) {
add_topic("airflow_groundtruth", 10);
add_topic("vehicle_angular_velocity_groundtruth", 10);
add_topic("vehicle_attitude_groundtruth", 10);
add_topic("vehicle_global_position_groundtruth", 100);