From ca5b61b218e94112137a4584aa0b7ae44ee9f54d Mon Sep 17 00:00:00 2001 From: FARHANG Date: Fri, 12 Dec 2025 18:33:11 -0500 Subject: [PATCH] Revert both airspeed-related commits for testing This reverts commits: - 18477554e0 "wind-estimator: learn airspeed scale faster at beginning of flight" - 5df20b8e9b "Add custom gz airspeed plugin and add wind effects (#26018)" Testing if reverting both together fixes the VTOL CI failures. The combination of the new airspeed plugin and aggressive wind estimator learning may be causing issues. Related CI failure: standard_vtol tests failing. --- .../airframes/4008_gz_advanced_plane | 2 + src/lib/wind_estimator/WindEstimator.cpp | 7 +- src/lib/wind_estimator/WindEstimator.hpp | 4 - .../simulation/gz_plugins/CMakeLists.txt | 5 +- .../gz_plugins/airspeed/AirSpeed.cpp | 137 ------------------ .../gz_plugins/airspeed/AirSpeed.hpp | 96 ------------ .../gz_plugins/airspeed/CMakeLists.txt | 57 -------- 7 files changed, 5 insertions(+), 303 deletions(-) delete mode 100644 src/modules/simulation/gz_plugins/airspeed/AirSpeed.cpp delete mode 100644 src/modules/simulation/gz_plugins/airspeed/AirSpeed.hpp delete mode 100644 src/modules/simulation/gz_plugins/airspeed/CMakeLists.txt diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane index 184ed7ba0d..69a4ee5e0d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane @@ -11,6 +11,8 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=advanced_plane} param set-default SIM_GZ_EN 1 +param set-default SENS_EN_ARSPDSIM 1 + param set-default FW_LND_ANG 8 param set-default FW_PR_FF 0.08 diff --git a/src/lib/wind_estimator/WindEstimator.cpp b/src/lib/wind_estimator/WindEstimator.cpp index 93602d75a6..ee6c6c9ed8 100644 --- a/src/lib/wind_estimator/WindEstimator.cpp +++ b/src/lib/wind_estimator/WindEstimator.cpp @@ -68,7 +68,6 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_vari } _wind_estimator_reset = true; - _time_initialised = hrt_absolute_time(); return true; } @@ -95,14 +94,10 @@ WindEstimator::update(uint64_t time_now) const float dt = (float)(time_now - _time_last_update) * 1e-6f; _time_last_update = time_now; - // if airspeed scale is at default (1.0) and we are in the first 5 minutes of flight time, multiply _tas_scale_psd by 100 for faster learning - const float tas_psd_multiplier = (fabsf(_scale_init - 1.0f) < FLT_EPSILON && (time_now - _time_initialised < kTASScaleFastLearnTime)) ? - kTASScalePSDMultiplier : 1.f; - matrix::Matrix3f Qk; Qk(INDEX_W_N, INDEX_W_N) = _wind_psd * dt; Qk(INDEX_W_E, INDEX_W_E) = Qk(INDEX_W_N, INDEX_W_N); - Qk(INDEX_TAS_SCALE, INDEX_TAS_SCALE) = _tas_scale_psd * tas_psd_multiplier * dt; + Qk(INDEX_TAS_SCALE, INDEX_TAS_SCALE) = _tas_scale_psd * dt; _P += Qk; } diff --git a/src/lib/wind_estimator/WindEstimator.hpp b/src/lib/wind_estimator/WindEstimator.hpp index b69ee9cb85..dbdf854da9 100644 --- a/src/lib/wind_estimator/WindEstimator.hpp +++ b/src/lib/wind_estimator/WindEstimator.hpp @@ -139,10 +139,6 @@ private: uint64_t _time_last_airspeed_fuse = 0; ///< timestamp of last airspeed fusion uint64_t _time_last_beta_fuse = 0; ///< timestamp of last sideslip fusion uint64_t _time_last_update = 0; ///< timestamp of last covariance prediction - uint64_t _time_initialised = 0; ///< timestamp when estimator is initialised - - static constexpr float kTASScalePSDMultiplier = 100; - static constexpr hrt_abstime kTASScaleFastLearnTime = 300_s; bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle diff --git a/src/modules/simulation/gz_plugins/CMakeLists.txt b/src/modules/simulation/gz_plugins/CMakeLists.txt index 16bfa6077c..4423a6b139 100644 --- a/src/modules/simulation/gz_plugins/CMakeLists.txt +++ b/src/modules/simulation/gz_plugins/CMakeLists.txt @@ -71,12 +71,11 @@ if (gz-transport_FOUND) add_subdirectory(buoyancy) add_subdirectory(spacecraft_thruster) add_subdirectory(motor_failure) - add_subdirectory(airspeed) # Add an alias target for each plugin if (TARGET GstCameraSystem) - add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin GstCameraSystem SpacecraftThrusterModelPlugin MotorFailurePlugin AirSpeedPlugin) + add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin GstCameraSystem SpacecraftThrusterModelPlugin MotorFailurePlugin) else() - add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin SpacecraftThrusterModelPlugin MotorFailurePlugin AirSpeedPlugin) + add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin SpacecraftThrusterModelPlugin MotorFailurePlugin) endif() endif() diff --git a/src/modules/simulation/gz_plugins/airspeed/AirSpeed.cpp b/src/modules/simulation/gz_plugins/airspeed/AirSpeed.cpp deleted file mode 100644 index 8d0c74dab7..0000000000 --- a/src/modules/simulation/gz_plugins/airspeed/AirSpeed.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/**************************************************************************** - * - * 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "AirSpeed.hpp" - -#include -#include - -using namespace px4; - -// Sign function taken from https://stackoverflow.com/a/4609795/8548472 -template int sign(T val) -{ - return (T(0) < val) - (val < T(0)); -} - -// Register the plugin -GZ_ADD_PLUGIN( - AirSpeed, - gz::sim::System, - AirSpeed::ISystemPreUpdate, - AirSpeed::ISystemConfigure -) - -void AirSpeed::Configure(const gz::sim::Entity &entity, - const std::shared_ptr &sdf, - gz::sim::EntityComponentManager &ecm, - gz::sim::EventManager &eventMgr) -{ - _entity = entity; - _model = gz::sim::Model(entity); - - const std::string link_name = sdf->Get("link_name"); - _link_entity = _model.LinkByName(ecm, link_name); - std::string model_name = _model.Name(ecm); - - if (!_link_entity) { - throw std::runtime_error("Airspeed::Configure: Link \"" + link_name + "\" was not found. " - "Please ensure that your model contains the corresponding link."); - } - - _link = gz::sim::Link(_link_entity); - - // Needed to report linear & angular velocity - _link.EnableVelocityChecks(ecm, true); - - _world_entity = gz::sim::worldEntity(ecm); - _world = gz::sim::World(_world_entity); - std::string world_name = _world.Name(ecm).value_or("default"); - - std::string airspeed_topic = "/world/" + world_name + "/model/" + model_name + - "/link/airspeed_link/sensor/air_speed/air_speed"; - _pub = _node.Advertise(airspeed_topic); - - std::string wind_topic = "/world/" + world_name + "/wind_info"; - - _node.Subscribe(wind_topic, &AirSpeed::windCallback, this); - - ///TODO: Read sdf for altitude home position -} - -void AirSpeed::PreUpdate(const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) -{ - const auto optional_pose = _link.WorldPose(_ecm); - - if (optional_pose.has_value()) { - _vehicle_position = optional_pose.value().Pos(); - _vehicle_attitude = optional_pose.value().Rot(); - - } else { - gzerr << "Unable to get pose" << std::endl; - } - - const auto optional_vel = _link.WorldLinearVelocity(_ecm); - - if (optional_vel.has_value()) { - _vehicle_velocity = optional_vel.value(); - - } else { - gzerr << "Unable to get linear velocity" << std::endl; - } - - // Compute the air density at the local altitude / temperature - const float alt_rel = _vehicle_position.Z(); // Z-component from ENU - const float alt_amsl = (float)_alt_home + alt_rel; - const float temperature_local = TEMPERATURE_MSL - LAPSE_RATE * alt_amsl; - const float density_ratio = powf(TEMPERATURE_MSL / temperature_local, 4.256f); - const float air_density = AIR_DENSITY_MSL / density_ratio; - - // Calculate differential pressure + noise in hPa - const float diff_pressure_noise = standard_normal_distribution_(random_generator_) * diff_pressure_stddev_; - // Body-relateive air velocity - gz::math::Vector3d air_relative_velocity = _vehicle_velocity - _wind_velocity; - gz::math::Vector3d body_velocity = _vehicle_attitude.RotateVectorReverse(air_relative_velocity); - // Calculate differential pressure in hPa - double diff_pressure = sign(body_velocity.X()) * 0.005 * (double)air_density * body_velocity.X() * body_velocity.X() + - (double)diff_pressure_noise; - gz::msgs::AirSpeed airspeed_msg; - airspeed_msg.set_diff_pressure(diff_pressure * 100.0); - _pub.Publish(airspeed_msg); -} - -void AirSpeed::windCallback(const gz::msgs::Wind &msg) -{ - _wind_velocity = gz::math::Vector3d(msg.linear_velocity().x(), msg.linear_velocity().y(), msg.linear_velocity().z()); -} diff --git a/src/modules/simulation/gz_plugins/airspeed/AirSpeed.hpp b/src/modules/simulation/gz_plugins/airspeed/AirSpeed.hpp deleted file mode 100644 index 108ffd47f5..0000000000 --- a/src/modules/simulation/gz_plugins/airspeed/AirSpeed.hpp +++ /dev/null @@ -1,96 +0,0 @@ -/**************************************************************************** - * - * 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include -#include -#include "gz/sim/components/LinearVelocity.hh" -#include - -#include - -#include - -namespace px4 -{ - -static constexpr float DEFAULT_HOME_ALT_AMSL = 488.0; // altitude AMSL at Irchel Park, Zurich, Switzerland [m] - -// international standard atmosphere (troposphere model - valid up to 11km) see [1] -static constexpr float TEMPERATURE_MSL = 288.15; // temperature at MSL [K] (15 [C]) -static constexpr float PRESSURE_MSL = 101325.0; // pressure at MSL [Pa] -static constexpr float LAPSE_RATE = 0.0065; // reduction in temperature with altitude for troposphere [K/m] -static constexpr float AIR_DENSITY_MSL = 1.225; // air density at MSL [kg/m^3] - -class AirSpeed: - public gz::sim::System, - public gz::sim::ISystemPreUpdate, - public gz::sim::ISystemConfigure -{ -public: - void PreUpdate(const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) final; - - void Configure(const gz::sim::Entity &entity, - const std::shared_ptr &sdf, - gz::sim::EntityComponentManager &ecm, - gz::sim::EventManager &eventMgr) override; - void windCallback(const gz::msgs::Wind &msg); - -private: - gz::sim::Entity _entity; - gz::sim::Model _model{gz::sim::kNullEntity}; - gz::sim::Entity _link_entity; - gz::sim::Link _link; - gz::sim::Entity _world_entity; - gz::sim::World _world; - - gz::transport::Node _node; - gz::transport::Node::Publisher _pub; - - gz::math::Quaterniond _vehicle_attitude; - gz::math::Vector3d _vehicle_velocity{0., 0., 0.}; - gz::math::Vector3d _vehicle_position{0., 0., 0.}; - gz::math::Vector3d _wind_velocity{0., 0., 0.}; - - std::default_random_engine random_generator_; - std::normal_distribution standard_normal_distribution_; - - float diff_pressure_stddev_{0.01f}; // [hPa] - float _alt_home{DEFAULT_HOME_ALT_AMSL}; -}; -} // end namespace px4 diff --git a/src/modules/simulation/gz_plugins/airspeed/CMakeLists.txt b/src/modules/simulation/gz_plugins/airspeed/CMakeLists.txt deleted file mode 100644 index 2afd37c86e..0000000000 --- a/src/modules/simulation/gz_plugins/airspeed/CMakeLists.txt +++ /dev/null @@ -1,57 +0,0 @@ -############################################################################ -# -# 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 -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -project(AirSpeedPlugin) - - -add_library(${PROJECT_NAME} SHARED - AirSpeed.cpp -) - -target_link_libraries(${PROJECT_NAME} - PUBLIC px4_gz_msgs - PUBLIC ${GZ_SENSORS_TARGET} - PUBLIC ${GZ_PLUGIN_TARGET} - PUBLIC ${GZ_SIM_TARGET} - PUBLIC ${GZ_TRANSPORT_TARGET} -) - -target_include_directories(${PROJECT_NAME} - PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} - PUBLIC ${CMAKE_CURRENT_BINARY_DIR} - PUBLIC px4_gz_msgs -) - -if (NOT CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT) - install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION lib/px4_gz_plugins) -endif()