diff --git a/src/modules/fw_wind_estimator/FixedwingWindEstimator.cpp b/src/modules/fw_wind_estimator/FixedwingWindEstimator.cpp index 63a80e1326..80078903ee 100644 --- a/src/modules/fw_wind_estimator/FixedwingWindEstimator.cpp +++ b/src/modules/fw_wind_estimator/FixedwingWindEstimator.cpp @@ -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; } diff --git a/src/modules/fw_wind_estimator/FixedwingWindEstimator.hpp b/src/modules/fw_wind_estimator/FixedwingWindEstimator.hpp index 71825a7b18..18acab8759 100644 --- a/src/modules/fw_wind_estimator/FixedwingWindEstimator.hpp +++ b/src/modules/fw_wind_estimator/FixedwingWindEstimator.hpp @@ -64,8 +64,6 @@ #include #include -using matrix::Eulerf; -using matrix::Quatf; using uORB::SubscriptionData; 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 39604bca25..9308110f25 100644 --- a/src/modules/fw_wind_estimator/fw_wind_estimator_params.c +++ b/src/modules/fw_wind_estimator/fw_wind_estimator_params.c @@ -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 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index d14a509b95..2ed33ef4cf 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -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);